Merge pull request #129 from neka-nat/develop

neka-nat · web-flow · commit daadbf989b60 · 2024-08-22T23:30:54.000+09:00
build on cuda 12.6
diff --git a/src/cupoch/geometry/down_sample.cu b/src/cupoch/geometry/down_sample.cu
@@ -75,14 +75,14 @@ struct compute_key_functor {
 };
 
 template <int Index, class... Args>
-struct normalize_and_devide_tuple_functor
+struct normalize_and_divide_tuple_functor
     : public thrust::binary_function<const thrust::tuple<Args...>,
                                      const int,
                                      thrust::tuple<Args...>> {
     __host__ __device__ thrust::tuple<Args...> operator()(
             const thrust::tuple<Args...> &x, const int &y) const {
         thrust::tuple<Args...> ans = x;
-        devide_tuple_impl(ans, y,
+        divide_tuple_impl(ans, y,
                           thrust::make_index_sequence<sizeof...(Args)>{});
         thrust::get<Index>(ans).normalize();
         return ans;
@@ -211,8 +211,17 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
     };
     if (!has_normals && !has_colors) {
         auto begin = make_tuple_begin(output->points_, counts);
-        int n_out = runs(begin, sorted_points);
-        devide_tuple_functor<Eigen::Vector3f> dv_func;
+        thrust::sort_by_key(
+            utility::exec_policy(0), keys.begin(), keys.end(),
+            sorted_points.begin());
+        add_tuple_functor<Eigen::Vector3f, int> add_func;
+        auto end = thrust::reduce_by_key(
+            utility::exec_policy(0), keys.begin(), keys.end(),
+            make_tuple_iterator(sorted_points.begin(),
+                                thrust::make_constant_iterator(1)),
+            thrust::make_discard_iterator(), begin, binary_pred, add_func);
+        int n_out = thrust::distance(begin, end.second);
+        divide_tuple_functor<Eigen::Vector3f> dv_func;
         auto output_begins = make_tuple_begin(output->points_);
         thrust::transform(output_begins, output_begins + n_out, counts.begin(),
                           output_begins, dv_func);
@@ -223,7 +232,7 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
         auto begin =
                 make_tuple_begin(output->points_, output->normals_, counts);
         int n_out = runs(begin, sorted_points, sorted_normals);
-        normalize_and_devide_tuple_functor<1, Eigen::Vector3f, Eigen::Vector3f>
+        normalize_and_divide_tuple_functor<1, Eigen::Vector3f, Eigen::Vector3f>
                 dv_func;
         auto output_begins =
                 make_tuple_begin(output->points_, output->normals_);
@@ -235,7 +244,7 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
         resize_all(n, output->colors_);
         auto begin = make_tuple_begin(output->points_, output->colors_, counts);
         int n_out = runs(begin, sorted_points, sorted_colors);
-        devide_tuple_functor<Eigen::Vector3f, Eigen::Vector3f> dv_func;
+        divide_tuple_functor<Eigen::Vector3f, Eigen::Vector3f> dv_func;
         auto output_begins = make_tuple_begin(output->points_, output->colors_);
         thrust::transform(output_begins, output_begins + n_out, counts.begin(),
                           output_begins, dv_func);
@@ -247,7 +256,7 @@ std::shared_ptr<PointCloud> PointCloud::VoxelDownSample(
         auto begin = make_tuple_begin(output->points_, output->normals_,
                                       output->colors_, counts);
         int n_out = runs(begin, sorted_points, sorted_normals, sorted_colors);
-        normalize_and_devide_tuple_functor<1, Eigen::Vector3f, Eigen::Vector3f,
+        normalize_and_divide_tuple_functor<1, Eigen::Vector3f, Eigen::Vector3f,
                                            Eigen::Vector3f>
                 dv_func;
         auto output_begins = make_tuple_begin(output->points_, output->normals_,
@@ -397,7 +406,7 @@ PointCloud::RemoveStatisticalOutliers(size_t nb_neighbors,
     auto mean_and_count = thrust::transform_reduce(
             utility::exec_policy(0), avg_distances.begin(),
             avg_distances.end(),
-            [] __device__(float const &x) {
+            [] __device__(float const &x) -> thrust::tuple<float, size_t> {
                 return thrust::make_tuple(max(x, 0.0f), (size_t)(x >= 0.0));
             },
             thrust::make_tuple(0.0f, size_t(0)),
@@ -412,8 +421,8 @@ PointCloud::RemoveStatisticalOutliers(size_t nb_neighbors,
     const float sq_sum = thrust::transform_reduce(
             utility::exec_policy(0), avg_distances.begin(),
             avg_distances.end(),
-            [cloud_mean] __device__(const float x) {
-                return (x > 0) ? (x - cloud_mean) * (x - cloud_mean) : 0;
+            [cloud_mean] __device__(const float x) -> float {
+                return (x > 0) ? (x - cloud_mean) * (x - cloud_mean) : 0.0f;
             },
             0.0, thrust::plus<float>());
     // Bessel's correction
diff --git a/src/cupoch/geometry/lineset.cu b/src/cupoch/geometry/lineset.cu
@@ -225,7 +225,7 @@ float LineSet<Dim>::GetMaxLineLength() const {
                                     element_get_functor<Eigen::Vector2i, 1>()))),
             [] __device__(
                     const thrust::tuple<Eigen::Matrix<float, Dim, 1>,
-                                        Eigen::Matrix<float, Dim, 1>> &ppair) {
+                                        Eigen::Matrix<float, Dim, 1>> &ppair) -> float {
                 return (thrust::get<0>(ppair) - thrust::get<1>(ppair)).norm();
             },
             0.0f, thrust::maximum<float>());
diff --git a/src/cupoch/geometry/pointcloud.cu b/src/cupoch/geometry/pointcloud.cu
@@ -327,7 +327,7 @@ std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
             thrust::make_counting_iterator<size_t>(0),
             thrust::make_counting_iterator<size_t>(num_points),
             func,
-            thrust::make_tuple<size_t, float>(farthest_index, 0.0f),
+            thrust::make_tuple<size_t, float>(static_cast<size_t>(farthest_index), 0.0f),
             [] __host__ __device__(const thrust::tuple<size_t, float> &a,
                               const thrust::tuple<size_t, float> &b) -> thrust::tuple<size_t, float> {
                 return thrust::get<1>(a) > thrust::get<1>(b) ? a : b;
diff --git a/src/cupoch/geometry/segmentation.cu b/src/cupoch/geometry/segmentation.cu
@@ -147,7 +147,7 @@ Eigen::Vector4f GetPlaneFromPoints(
             utility::exec_policy(0),
             thrust::make_permutation_iterator(points.begin(), inliers.begin()),
             thrust::make_permutation_iterator(points.begin(), inliers.end()),
-            [centroid] __device__(const Eigen::Vector3f &pt) {
+            [centroid] __device__(const Eigen::Vector3f &pt) -> Eigen::Vector6f {
                 Eigen::Vector3f r = pt - centroid;
                 Eigen::Vector6f ans;
                 ans << r(0) * r(0), r(0) * r(1), r(0) * r(2), r(1) * r(1),
diff --git a/src/cupoch/geometry/trianglemesh.cu b/src/cupoch/geometry/trianglemesh.cu
@@ -929,12 +929,14 @@ TriangleMesh &TriangleMesh::RemoveDuplicatedVertices() {
     if (has_vert_normal && has_vert_color) {
         k = runs(vertex_normals_, vertex_colors_);
     } else if (has_vert_normal) {
-        k = runs(vertex_normals_);
+        thrust::discard_iterable dummy;
+        k = runs(vertex_normals_, dummy);
     } else if (has_vert_color) {
-        k = runs(vertex_colors_);
+        thrust::discard_iterable dummy;
+        k = runs(vertex_colors_, dummy);
     } else {
         thrust::discard_iterable dummy;
-        k = runs(dummy);
+        k = runs(dummy, dummy);
     }
     vertices_.resize(k);
     if (has_vert_normal) vertex_normals_.resize(k);
diff --git a/src/cupoch/geometry/voxelgrid.cu b/src/cupoch/geometry/voxelgrid.cu
@@ -274,7 +274,7 @@ VoxelGrid &VoxelGrid::operator+=(const VoxelGrid &voxelgrid) {
         thrust::swap(voxels_keys_, new_keys);
         thrust::transform(voxels_values_.begin(), voxels_values_.end(),
                           counts.begin(), voxels_values_.begin(),
-                          devide_voxel_color_functor());
+                          divide_voxel_color_functor());
     } else {
         this->AddVoxels(voxelgrid.voxels_values_);
     }
diff --git a/src/cupoch/geometry/voxelgrid.h b/src/cupoch/geometry/voxelgrid.h
@@ -72,7 +72,7 @@ struct add_voxel_color_functor {
     }
 };
 
-struct devide_voxel_color_functor {
+struct divide_voxel_color_functor {
     __device__ Voxel operator()(const Voxel &x, int y) const {
         Voxel ans;
         ans.grid_index_ = x.grid_index_;
diff --git a/src/cupoch/geometry/voxelgrid_factory.cu b/src/cupoch/geometry/voxelgrid_factory.cu
@@ -210,7 +210,7 @@ std::shared_ptr<VoxelGrid> VoxelGrid::CreateFromPointCloudWithinBounds(
     thrust::transform(output->voxels_values_.begin(),
                       output->voxels_values_.end(), counts.begin(),
                       output->voxels_values_.begin(),
-                      devide_voxel_color_functor());
+                      divide_voxel_color_functor());
     utility::LogDebug(
             "Pointcloud is voxelized from {:d} points to {:d} voxels.",
             (int)input.points_.size(), (int)output->voxels_keys_.size());
diff --git a/src/cupoch/registration/fast_global_registration.cu b/src/cupoch/registration/fast_global_registration.cu
@@ -234,7 +234,7 @@ std::tuple<std::vector<Eigen::Vector3f>, float, float> NormalizePointCloud(
                 utility::exec_policy(0),
                 point_cloud_vec[i].points_.begin(),
                 point_cloud_vec[i].points_.end(),
-                [] __device__(const Eigen::Vector3f& pt) { return pt.norm(); },
+                [] __device__(const Eigen::Vector3f& pt) -> float { return pt.norm(); },
                 scale, thrust::maximum<float>());
     }
 
diff --git a/src/cupoch/registration/feature.h b/src/cupoch/registration/feature.h
@@ -23,6 +23,7 @@
 #include <Eigen/Core>
 #include <memory>
 
+#include "cupoch/registration/transformation_estimation.h"
 #include "cupoch/knn/kdtree_search_param.h"
 #include "cupoch/utility/device_vector.h"
 
@@ -65,5 +66,10 @@ std::shared_ptr<Feature<352>> ComputeSHOTFeature(
         const knn::KDTreeSearchParam& search_param =
                 knn::KDTreeSearchParamKNN());
 
+CorrespondenceSet CorrespondencesFromFeatures(const Feature<33> &source_features,
+                                              const Feature<33> &target_features,
+                                              bool mutual_filter = false,
+                                              float mutual_consistency_ratio = 0.1);
+
 }  // namespace registration
 }  // namespace cupoch
diff --git a/src/cupoch/registration/kabsch.cu b/src/cupoch/registration/kabsch.cu
@@ -146,14 +146,14 @@ Eigen::Matrix4f_u cupoch::registration::KabschWeighted(
     Eigen::Vector3f model_center = thrust::transform_reduce(
             utility::exec_policy(0), make_tuple_begin(model, weight),
             make_tuple_end(model, weight),
-            [] __device__(const thrust::tuple<Eigen::Vector3f, float> &x) {
+            [] __device__(const thrust::tuple<Eigen::Vector3f, float> &x) -> Eigen::Vector3f {
                 return thrust::get<0>(x) * thrust::get<1>(x);
             },
             Eigen::Vector3f(0.0, 0.0, 0.0), thrust::plus<Eigen::Vector3f>());
     Eigen::Vector3f target_center = thrust::transform_reduce(
             utility::exec_policy(0), make_tuple_begin(target, weight),
             make_tuple_end(target, weight),
-            [] __device__(const thrust::tuple<Eigen::Vector3f, float> &x) {
+            [] __device__(const thrust::tuple<Eigen::Vector3f, float> &x) -> Eigen::Vector3f {
                 return thrust::get<0>(x) * thrust::get<1>(x);
             },
             Eigen::Vector3f(0.0, 0.0, 0.0), thrust::plus<Eigen::Vector3f>());
@@ -166,7 +166,7 @@ Eigen::Matrix4f_u cupoch::registration::KabschWeighted(
     // Compute the H matrix
     const float h_weight = thrust::transform_reduce(
             utility::exec_policy(0), weight.begin(), weight.end(),
-            [] __device__(float x) { return x * x; }, 0.0f,
+            [] __device__(float x) -> float { return x * x; }, 0.0f,
             thrust::plus<float>());
     const Eigen::Matrix3f init = Eigen::Matrix3f::Zero();
     Eigen::Matrix3f hh = thrust::transform_reduce(
@@ -175,7 +175,7 @@ Eigen::Matrix4f_u cupoch::registration::KabschWeighted(
             make_tuple_end(model, target, weight),
             [model_center, target_center] __device__(
                     const thrust::tuple<Eigen::Vector3f, Eigen::Vector3f, float>
-                            &x) {
+                            &x) -> Eigen::Matrix3f {
                 const Eigen::Vector3f centralized_x =
                         thrust::get<0>(x) - model_center;
                 const Eigen::Vector3f centralized_y =
diff --git a/src/cupoch/registration/registration.cu b/src/cupoch/registration/registration.cu
@@ -49,7 +49,7 @@ RegistrationResult GetRegistrationResultAndCorrespondences(
     result.correspondence_set_.resize(n_pt);
     const float error2 = thrust::transform_reduce(
             utility::exec_policy(0), dists.begin(), dists.end(),
-            [] __device__(float d) { return (isinf(d)) ? 0.0 : d; }, 0.0f,
+            [] __device__(float d) -> float { return (isinf(d)) ? 0.0f : d; }, 0.0f,
             thrust::plus<float>());
     thrust::transform(enumerate_begin(indices), enumerate_end(indices),
                       result.correspondence_set_.begin(),
diff --git a/src/cupoch/registration/transformation_estimation.cu b/src/cupoch/registration/transformation_estimation.cu
@@ -183,7 +183,7 @@ float TransformationEstimationPointToPlane::ComputeRMSE(
                                     element_get_functor<Eigen::Vector2i,
                                                         1>()))),
             [] __device__(const thrust::tuple<Eigen::Vector3f, Eigen::Vector3f,
-                                              Eigen::Vector3f> &x) {
+                                              Eigen::Vector3f> &x) -> float {
                 float r = (thrust::get<0>(x) - thrust::get<1>(x))
                                   .dot(thrust::get<2>(x));
                 return r * r;
@@ -275,7 +275,7 @@ float TransformationEstimationSymmetricMethod::ComputeRMSE(
                                                         1>()))),
             [] __device__(
                     const thrust::tuple<Eigen::Vector3f, Eigen::Vector3f,
-                                        Eigen::Vector3f, Eigen::Vector3f> &x) {
+                                        Eigen::Vector3f, Eigen::Vector3f> &x) -> float{
                 // Compute error using both source and target normals
                 float r = ComputeErrorUsingNormals(
                         thrust::get<0>(x), thrust::get<1>(x), thrust::get<2>(x),
diff --git a/src/cupoch/utility/console.h b/src/cupoch/utility/console.h
@@ -117,7 +117,7 @@ class ConsoleProgressBar {
 private:
     const size_t resolution_ = 40;
     size_t expected_count_;
-    size_t current_count_;
+    int current_count_;
     std::string progress_info_;
     size_t progress_pixel_;
     bool active_;
diff --git a/src/cupoch/utility/eigen.inl b/src/cupoch/utility/eigen.inl
@@ -177,7 +177,7 @@ thrust::tuple<MatType, VecType, float, float> ComputeWeightedJTJandJTr(
             make_tuple_begin(JTJs, JTrs, r2s, ws),
             make_tuple_end(JTJs, JTrs, r2s, ws),
             [] __device__(
-                    const thrust::tuple<MatType, VecType, float, float> &x) {
+                    const thrust::tuple<MatType, VecType, float, float> &x) -> thrust::tuple<MatType, VecType, float> {
                 float w = thrust::get<3>(x);
                 return thrust::make_tuple(thrust::get<0>(x) * w,
                                           thrust::get<1>(x) * w,
diff --git a/src/cupoch/utility/helper.h b/src/cupoch/utility/helper.h
@@ -51,7 +51,6 @@ struct equal_to<Eigen::Matrix<int, Dim, 1>> {
     typedef Eigen::Matrix<int, Dim, 1> second_argument_type;
     typedef bool result_type;
     // clang-format off
-    __thrust_exec_check_disable__
     __host__ __device__ bool operator()(
             const Eigen::Matrix<int, Dim, 1> &lhs,
             const Eigen::Matrix<int, Dim, 1> &rhs) const {
@@ -207,7 +206,7 @@ __host__ __device__ void add_fn(T &x, const T &y) {
 template <typename T, std::size_t... Is>
 __host__ __device__ void add_tuple_impl(T &t,
                                         const T &y,
-                                        std::index_sequence<Is...>) {
+                                        thrust::integer_sequence<std::size_t, Is...>) {
     std::initializer_list<int>{
             ((void)add_fn(thrust::get<Is>(t), thrust::get<Is>(y)), 0)...};
 }
@@ -227,26 +226,26 @@ struct add_tuple_functor
 };
 
 template <typename T>
-__host__ __device__ void devide_fn(T &x, const int &y) {
+__host__ __device__ void divide_fn(T &x, const int &y) {
     x /= static_cast<float>(y);
 }
 
 template <typename T, std::size_t... Is>
-__host__ __device__ void devide_tuple_impl(T &t,
+__host__ __device__ void divide_tuple_impl(T &t,
                                            const int &y,
-                                           std::index_sequence<Is...>) {
-    std::initializer_list<int>{((void)devide_fn(thrust::get<Is>(t), y), 0)...};
+                                           thrust::integer_sequence<std::size_t, Is...>) {
+    std::initializer_list<int>{((void)divide_fn(thrust::get<Is>(t), y), 0)...};
 }
 
 template <class... Args>
-struct devide_tuple_functor
+struct divide_tuple_functor
     : public thrust::binary_function<const thrust::tuple<Args...>,
                                      const int,
                                      thrust::tuple<Args...>> {
     __host__ __device__ thrust::tuple<Args...> operator()(
             const thrust::tuple<Args...> &x, const int &y) const {
         thrust::tuple<Args...> ans = x;
-        devide_tuple_impl(ans, y,
+        divide_tuple_impl(ans, y,
                           thrust::make_index_sequence<sizeof...(Args)>{});
         return ans;
     }
diff --git a/src/cupoch/utility/pinned_allocator.h b/src/cupoch/utility/pinned_allocator.h
@@ -21,7 +21,6 @@
 #pragma once
 
 #include <thrust/detail/config.h>
-#include <thrust/system/cuda/detail/guarded_cuda_runtime_api.h>
 #include <stdexcept>
 #include <limits>
 #include <string>
diff --git a/src/python/pyproject.toml b/src/python/pyproject.toml
@@ -22,6 +22,7 @@ rosmaster = {version = "^1.15.11", optional = true}
 rosbag = {version = "^1.15.11", optional = true}
 transformations = {version = "^2022.9.26", optional = true}
 
+setuptools = "^72.2.0"
 [tool.poetry.dev-dependencies]
 twine = "^3.2.0"
 wheel = "^0.36.2"
diff --git a/src/tests/test_utility/raw.h b/src/tests/test_utility/raw.h
@@ -20,6 +20,7 @@
 **/
 #pragma once
 
+#include <stdint.h>
 #include <iostream>
 #include <string>
 #include <vector>
diff --git a/third_party/stdgpu b/third_party/stdgpu
@@ -1 +1 @@
-Subproject commit 965a29f7c3fa72dc123c5a1abc5ddbadf3c39815
+Subproject commit 3b7d712b5c3a5e0db71cf690df7b94bb470a397b

Original file line number	Diff line number	Diff line change
`@@ -274,7 +274,7 @@ VoxelGrid &VoxelGrid::operator+=(const VoxelGrid &voxelgrid) {`
`274`	`274`	`thrust::swap(voxels_keys_, new_keys);`
`275`	`275`	`thrust::transform(voxels_values_.begin(), voxels_values_.end(),`
`276`	`276`	`counts.begin(), voxels_values_.begin(),`
`277`		`- devide_voxel_color_functor());`
	`277`	`+ divide_voxel_color_functor());`
`278`	`278`	`} else {`
`279`	`279`	`this->AddVoxels(voxelgrid.voxels_values_);`
`280`	`280`	`}`
Original file line number	Diff line number	Diff line change
`@@ -72,7 +72,7 @@ struct add_voxel_color_functor {`
`72`	`72`	`}`
`73`	`73`	`};`
`74`	`74`
`75`		`-struct devide_voxel_color_functor {`
	`75`	`+struct divide_voxel_color_functor {`
`76`	`76`	`__device__ Voxel operator()(const Voxel &x, int y) const {`
`77`	`77`	`Voxel ans;`
`78`	`78`	`ans.grid_index_ = x.grid_index_;`
Original file line number	Diff line number	Diff line change
`@@ -234,7 +234,7 @@ std::tuple<std::vector<Eigen::Vector3f>, float, float> NormalizePointCloud(`
`234`	`234`	`utility::exec_policy(0),`
`235`	`235`	`point_cloud_vec[i].points_.begin(),`
`236`	`236`	`point_cloud_vec[i].points_.end(),`
`237`		`- [] __device__(const Eigen::Vector3f& pt) { return pt.norm(); },`
	`237`	`+ [] __device__(const Eigen::Vector3f& pt) -> float { return pt.norm(); },`
`238`	`238`	`scale, thrust::maximum<float>());`
`239`	`239`	`}`
`240`	`240`