add ransac registration

CMakeLists.txt

@@ -159,6 +159,11 @@ elseif (UNIX)
     message(STATUS "Compiling on Unix")
 endif ()
 
+find_package(OpenMP)
+if(OpenMP_FOUND)
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
 find_package(CUDA REQUIRED)
 if(NOT CMAKE_CUDA_ARCHITECTURES)
   set(CMAKE_CUDA_ARCHITECTURES 86)

src/cupoch/geometry/pointcloud.cu

@@ -298,6 +298,14 @@ PointCloud &PointCloud::Transform(const Eigen::Matrix4f &transformation) {
     return *this;
 }
 
+PointCloud &PointCloud::Transform(cudaStream_t stream1, cudaStream_t stream2,
+                                  const Eigen::Matrix4f &transformation) {
+    TransformPoints<3>(stream1, transformation, points_);
+    TransformNormals(stream2, transformation, normals_);
+    cudaSafeCall(cudaDeviceSynchronize());
+    return *this;
+}
+
 std::shared_ptr<PointCloud> PointCloud::FarthestPointDownSample(
         size_t num_samples) const {
     if (num_samples == 0) {

src/cupoch/geometry/pointcloud.h

@@ -67,6 +67,8 @@ class PointCloud : public GeometryBase3D {
     AxisAlignedBoundingBox<3> GetAxisAlignedBoundingBox() const override;
     OrientedBoundingBox GetOrientedBoundingBox() const;
     PointCloud &Transform(const Eigen::Matrix4f &transformation) override;
+    PointCloud &Transform(cudaStream_t stream1, cudaStream_t stream2,
+                          const Eigen::Matrix4f &transformation);
     PointCloud &Translate(const Eigen::Vector3f &translation,
                           bool relative = true) override;
     PointCloud &Scale(const float scale, bool center = true) override;

src/cupoch/registration/colored_icp.cu

@@ -57,6 +57,7 @@ public:
                       const geometry::PointCloud &target,
                       const CorrespondenceSet &corres) const override;
     Eigen::Matrix4f ComputeTransformation(
+            cudaStream_t stream1, cudaStream_t stream2,
             const geometry::PointCloud &source,
             const geometry::PointCloud &target,
             const CorrespondenceSet &corres) const override;
@@ -216,6 +217,7 @@ struct compute_jacobian_and_residual_functor
 };
 
 Eigen::Matrix4f TransformationEstimationForColoredICP::ComputeTransformation(
+        cudaStream_t stream1, cudaStream_t stream2,
         const geometry::PointCloud &source,
         const geometry::PointCloud &target,
         const CorrespondenceSet &corres) const {
@@ -244,7 +246,7 @@ Eigen::Matrix4f TransformationEstimationForColoredICP::ComputeTransformation(
     thrust::tie(JTJ, JTr, r2) =
             utility::ComputeJTJandJTr<Eigen::Matrix6f, Eigen::Vector6f, 2,
                                       compute_jacobian_and_residual_functor>(
-                    func, (int)corres.size());
+                    stream1, func, (int)corres.size());
 
     bool is_success;
     Eigen::Matrix4f extrinsic;

src/cupoch/registration/correspondence_checker.cu

@@ -0,0 +1,171 @@
+/**
+ * Copyright (c) 2024 Neka-Nat
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ **/
+#include "cupoch/registration/correspondence_checker.h"
+
+#include <Eigen/Dense>
+#include <thrust/logical.h>
+
+#include "cupoch/geometry/pointcloud.h"
+#include "cupoch/utility/console.h"
+
+namespace cupoch {
+namespace registration {
+
+namespace {
+    struct edge_length_checker_functor {
+        float similarity_threshold_;
+        size_t corres_size_;
+        const Eigen::Vector3f* source_points_;
+        const Eigen::Vector3f* target_points_;
+        const Eigen::Vector2i* corres_;
+        edge_length_checker_functor(
+            float similarity_threshold,
+            size_t corres_size,
+            const Eigen::Vector3f* source_points,
+            const Eigen::Vector3f* target_points,
+            const Eigen::Vector2i* corres)
+            : similarity_threshold_(similarity_threshold),
+              corres_size_(corres_size),
+              source_points_(source_points),
+              target_points_(target_points),
+              corres_(corres) {}
+        __device__ bool operator()(int idx) {
+            int i = idx / corres_size_;
+            int j = idx % corres_size_;
+            if (i == j) return true;
+             float dis_source = (source_points_[corres_[i](0)] - source_points_[corres_[j](0)]).norm();
+            float dis_target = (target_points_[corres_[i](1)] - target_points_[corres_[j](1)]).norm();
+            return dis_source >= dis_target * similarity_threshold_ &&
+                   dis_target >= dis_source * similarity_threshold_;
+        }
+
+    };
+
+    struct distance_checker_functor {
+        float distance_threshold_;
+        const Eigen::Vector3f* source_points_;
+        const Eigen::Vector3f* target_points_;
+        const Eigen::Matrix4f transformation_;
+        distance_checker_functor(
+            float distance_threshold,
+            const Eigen::Vector3f* source_points,
+            const Eigen::Vector3f* target_points,
+            const Eigen::Matrix4f transformation)
+            : distance_threshold_(distance_threshold),
+              source_points_(source_points),
+              target_points_(target_points),
+              transformation_(transformation) {}
+        __device__ bool operator()(const Eigen::Vector2i& corr) {
+            const auto &pt = source_points_[corr(0)];
+            Eigen::Vector3f pt_trans =
+                    (transformation_ * Eigen::Vector4f(pt(0), pt(1), pt(2), 1.0))
+                            .block<3, 1>(0, 0);
+            return (target_points_[
+                corr(1)] - pt_trans).norm() <= distance_threshold_;
+        }
+    };
+
+    struct normal_checker_functor {
+        float cos_normal_angle_threshold_;
+        const Eigen::Vector3f* source_points_;
+        const Eigen::Vector3f* target_points_;
+        const Eigen::Vector3f* source_normals_;
+        const Eigen::Vector3f* target_normals_;
+        const Eigen::Matrix4f transformation_;
+        normal_checker_functor(
+            float cos_normal_angle_threshold,
+            const Eigen::Vector3f* source_points,
+            const Eigen::Vector3f* target_points,
+            const Eigen::Vector3f* source_normals,
+            const Eigen::Vector3f* target_normals,
+            const Eigen::Matrix4f transformation)
+            : cos_normal_angle_threshold_(cos_normal_angle_threshold),
+              source_points_(source_points),
+              target_points_(target_points),
+              source_normals_(source_normals),
+              target_normals_(target_normals),
+              transformation_(transformation) {}
+        __device__ bool operator()(const Eigen::Vector2i& corr) {
+            const auto &normal = source_normals_[corr(0)];
+            Eigen::Vector3f normal_trans =
+                    (transformation_ *
+                     Eigen::Vector4f(normal(0), normal(1), normal(2), 0.0))
+                            .block<3, 1>(0, 0);
+            return target_normals_[corr(1)].dot(normal) >= cos_normal_angle_threshold_;
+        }
+    };
+}
+
+bool CorrespondenceCheckerBasedOnEdgeLength::Check(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres,
+        const Eigen::Matrix4f & /*transformation*/) const {
+    return thrust::all_of(
+        thrust::make_counting_iterator<size_t>(0),
+        thrust::make_counting_iterator(corres.size() * corres.size()),
+        edge_length_checker_functor(
+            similarity_threshold_,
+            corres.size(),
+            thrust::raw_pointer_cast(source.points_.data()),
+            thrust::raw_pointer_cast(target.points_.data()),
+            thrust::raw_pointer_cast(corres.data())));
+}
+
+bool CorrespondenceCheckerBasedOnDistance::Check(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres,
+        const Eigen::Matrix4f &transformation) const {
+    return thrust::all_of(
+        corres.begin(), corres.end(),
+        distance_checker_functor(
+            distance_threshold_,
+            thrust::raw_pointer_cast(source.points_.data()),
+            thrust::raw_pointer_cast(target.points_.data()),
+            transformation));
+}
+
+bool CorrespondenceCheckerBasedOnNormal::Check(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres,
+        const Eigen::Matrix4f &transformation) const {
+    if (!source.HasNormals() || !target.HasNormals()) {
+        utility::LogWarning(
+                "[CorrespondenceCheckerBasedOnNormal::Check] Pointcloud has no "
+                "normals.");
+        return true;
+    }
+    float cos_normal_angle_threshold = std::cos(normal_angle_threshold_);
+    return thrust::all_of(
+        corres.begin(), corres.end(),
+        normal_checker_functor(
+            cos_normal_angle_threshold,
+            thrust::raw_pointer_cast(source.points_.data()),
+            thrust::raw_pointer_cast(target.points_.data()),
+            thrust::raw_pointer_cast(source.normals_.data()),
+            thrust::raw_pointer_cast(target.normals_.data()),
+            transformation));
+}
+
+}  // namespace registration
+}  // namespace cupoch

src/cupoch/registration/correspondence_checker.h

@@ -0,0 +1,159 @@
+/**
+ * Copyright (c) 2024 Neka-Nat
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ **/
+#pragma once
+
+#include <Eigen/Core>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "cupoch/registration/transformation_estimation.h"
+
+namespace cupoch {
+
+namespace geometry {
+class PointCloud;
+}
+
+namespace registration {
+
+/// \class CorrespondenceChecker
+///
+/// \brief Base class that checks if two (small) point clouds can be aligned.
+///
+/// This class is used in feature based matching algorithms (such as RANSAC and
+/// FastGlobalRegistration) to prune out outlier correspondences.
+/// The virtual function Check() must be implemented in subclasses.
+class CorrespondenceChecker {
+public:
+    /// \brief Default Constructor.
+    ///
+    /// \param require_pointcloud_alignment Specifies whether point cloud
+    /// alignment is required.
+    CorrespondenceChecker(bool require_pointcloud_alignment)
+        : require_pointcloud_alignment_(require_pointcloud_alignment) {}
+    virtual ~CorrespondenceChecker() {}
+
+public:
+    /// \brief Function to check if two points can be aligned.
+    ///
+    /// \param source Source point cloud.
+    /// \param target Target point cloud.
+    /// \param corres Correspondence set between source and target point cloud.
+    /// \param transformation The estimated transformation (inplace).
+    virtual bool Check(const geometry::PointCloud &source,
+                       const geometry::PointCloud &target,
+                       const CorrespondenceSet &corres,
+                       const Eigen::Matrix4f &transformation) const = 0;
+
+public:
+    /// Some checkers do not require point clouds to be aligned, e.g., the edge
+    /// length checker. Some checkers do, e.g., the distance checker.
+    bool require_pointcloud_alignment_;
+};
+
+/// \class CorrespondenceCheckerBasedOnEdgeLength
+///
+/// \brief Check if two point clouds build the polygons with similar edge
+/// lengths.
+///
+/// That is, checks if the lengths of any two arbitrary edges (line formed by
+/// two vertices) individually drawn withinin source point cloud and within the
+/// target point cloud with correspondences are similar. The only parameter
+/// similarity_threshold is a number between 0 (loose) and 1 (strict).
+class CorrespondenceCheckerBasedOnEdgeLength : public CorrespondenceChecker {
+public:
+    /// \brief Default Constructor.
+    ///
+    /// \param similarity_threshold specifies the threshold within which 2
+    /// arbitrary edges are similar.
+    CorrespondenceCheckerBasedOnEdgeLength(double similarity_threshold = 0.9)
+        : CorrespondenceChecker(false),
+          similarity_threshold_(similarity_threshold) {}
+    ~CorrespondenceCheckerBasedOnEdgeLength() override {}
+
+public:
+    bool Check(const geometry::PointCloud &source,
+               const geometry::PointCloud &target,
+               const CorrespondenceSet &corres,
+               const Eigen::Matrix4f &transformation) const override;
+
+public:
+    /// For the check to be true,
+    /// ||edgesource||>similarity_threshold×||edgetarget|| and
+    /// ||edgetarget||>similarity_threshold×||edgesource|| must hold true for
+    /// all edges.
+    double similarity_threshold_;
+};
+
+/// \class CorrespondenceCheckerBasedOnDistance
+///
+/// \brief Check if two aligned point clouds are close.
+class CorrespondenceCheckerBasedOnDistance : public CorrespondenceChecker {
+public:
+    /// \brief Default Constructor.
+    ///
+    /// \param distance_threshold Distance threshold for the check.
+    CorrespondenceCheckerBasedOnDistance(double distance_threshold)
+        : CorrespondenceChecker(true),
+          distance_threshold_(distance_threshold) {}
+    ~CorrespondenceCheckerBasedOnDistance() override {}
+
+public:
+    bool Check(const geometry::PointCloud &source,
+               const geometry::PointCloud &target,
+               const CorrespondenceSet &corres,
+               const Eigen::Matrix4f &transformation) const override;
+
+public:
+    /// Distance threshold for the check.
+    double distance_threshold_;
+};
+
+/// \class CorrespondenceCheckerBasedOnNormal
+///
+/// \brief Class to check if two aligned point clouds have similar normals.
+///
+/// It considers vertex normal affinity of any correspondences. It computes dot
+/// product of two normal vectors. It takes radian value for the threshold.
+class CorrespondenceCheckerBasedOnNormal : public CorrespondenceChecker {
+public:
+    /// \brief Parameterized Constructor.
+    ///
+    /// \param normal_angle_threshold Radian value for angle threshold.
+    CorrespondenceCheckerBasedOnNormal(double normal_angle_threshold)
+        : CorrespondenceChecker(true),
+          normal_angle_threshold_(normal_angle_threshold) {}
+    ~CorrespondenceCheckerBasedOnNormal() override {}
+
+public:
+    bool Check(const geometry::PointCloud &source,
+               const geometry::PointCloud &target,
+               const CorrespondenceSet &corres,
+               const Eigen::Matrix4f &transformation) const override;
+
+public:
+    /// Radian value for angle threshold.
+    double normal_angle_threshold_;
+};
+
+}  // namespace registration
+}  // namespace cupoch