add generalized icp

neka-nat · Jun 1, 2024 · 433437c · 433437c
1 parent 59071cf
commit 433437c
Show file tree

Hide file tree

Showing 9 changed files with 354 additions and 8 deletions.
diff --git a/README.md b/README.md
@@ -26,6 +26,7 @@ This repository is based on [Open3D](https://github.com/intel-isl/Open3D).
         * [flann](https://github.com/flann-lib/flann)
     * Point cloud registration
         * ICP
+        * Generalized ICP
         * [Symmetric ICP](https://gfx.cs.princeton.edu/pubs/Rusinkiewicz_2019_ASO/symm_icp.pdf) (Implemented by [@eclipse0922](https://github.com/eclipse0922))
         * [Colored Point Cloud Registration](https://ieeexplore.ieee.org/document/8237287)
         * [Fast Global Registration](http://vladlen.info/papers/fast-global-registration.pdf)

diff --git a/examples/python/basic/gicp_registration.py b/examples/python/basic/gicp_registration.py
@@ -0,0 +1,25 @@
+import cupoch as cph
+import numpy as np
+import time
+
+if __name__ == "__main__":
+    source_gpu = cph.io.read_point_cloud("../../testdata/icp/cloud_bin_0.pcd")
+    target_gpu = cph.io.read_point_cloud("../../testdata/icp/cloud_bin_1.pcd")
+    threshold = 0.02
+    trans_init = np.asarray(
+        [[0.862, 0.011, -0.507, 0.5], [-0.139, 0.967, -0.215, 0.7], [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]]
+    )
+
+    start = time.time()
+    reg_p2p = cph.registration.registration_generalized_icp(
+        source_gpu,
+        target_gpu,
+        threshold,
+        trans_init.astype(np.float32),
+        cph.registration.TransformationEstimationForGeneralizedICP(),
+    )
+    elapsed_time = time.time() - start
+    print(reg_p2p.transformation)
+    print("GICP (GPU) [sec]:", elapsed_time)
+    source_gpu.transform(reg_p2p.transformation)
+    cph.visualization.draw_geometries([source_gpu, target_gpu])
diff --git a/src/cupoch/registration/generalized_icp.cu b/src/cupoch/registration/generalized_icp.cu
@@ -0,0 +1,201 @@
+#include "cupoch/registration/generalized_icp.h"
+
+#include <Eigen/Dense>
+#include <unsupported/Eigen/MatrixFunctions>
+
+#include "cupoch/knn/kdtree_search_param.h"
+#include "cupoch/geometry/pointcloud.h"
+#include "cupoch/utility/eigen.h"
+#include "cupoch/utility/console.h"
+
+namespace cupoch {
+namespace registration {
+
+namespace {
+
+/// Obtain the Rotation matrix that transform the basis vector e1 onto the
+/// input vector x.
+inline __device__ Eigen::Matrix3f GetRotationFromE1ToX(const Eigen::Vector3f &x) {
+    const Eigen::Vector3f e1{1, 0, 0};
+    const Eigen::Vector3f v = e1.cross(x);
+    const float c = e1.dot(x);
+    if (c < -0.99) {
+        // Then means that x and e1 are in the same direction
+        return Eigen::Matrix3f::Identity();
+    }
+
+    const Eigen::Matrix3f sv = utility::SkewMatrix(v);
+    const float factor = 1 / (1 + c);
+    return Eigen::Matrix3f::Identity() + sv + (sv * sv) * factor;
+}
+
+/// Compute the covariance matrix according to the original paper. If the input
+/// has already pre-computed covariances returns immediately. If the input has
+/// pre-computed normals but no covariances, compute the covariances from those
+/// normals. If there is no covariances nor normals, compute each covariance
+/// matrix following the original implementation of GICP using 20 NN.
+std::shared_ptr<geometry::PointCloud> InitializePointCloudForGeneralizedICP(
+        const geometry::PointCloud &pcd, float epsilon) {
+    auto output = std::make_shared<geometry::PointCloud>(pcd);
+    if (output->HasCovariances()) {
+        utility::LogDebug("GeneralizedICP: Using pre-computed covariances.");
+        return output;
+    }
+    if (output->HasNormals()) {
+        utility::LogDebug("GeneralizedICP: Computing covariances from normals");
+    } else {
+        // Compute covariances the same way is done in the original GICP paper.
+        utility::LogDebug("GeneralizedICP: Computing covariances from points.");
+        output->EstimateNormals(cupoch::knn::KDTreeSearchParamKNN(20));
+    }
+
+    output->covariances_.resize(output->points_.size());
+    const Eigen::Matrix3f C = Eigen::Vector3f(epsilon, 1, 1).asDiagonal();
+    thrust::transform(output->normals_.begin(), output->normals_.end(),
+                      output->covariances_.begin(),
+                      [C] __device__ (const Eigen::Vector3f &n) {
+                          const auto Rx = GetRotationFromE1ToX(n);
+                          return Rx * C * Rx.transpose();
+                      });
+    return output;
+}
+
+struct compute_jacobian_and_residual_functor
+    : public utility::multiple_jacobians_residuals_functor<Eigen::Vector6f, 3> {
+    compute_jacobian_and_residual_functor(
+            const Eigen::Vector3f *source_points,
+            const Eigen::Matrix3f *source_covariances,
+            const Eigen::Vector3f *target_points,
+            const Eigen::Matrix3f *target_covariances,
+            const Eigen::Vector2i *corres)
+        : source_points_(source_points),
+          source_covariances_(source_covariances),
+          target_points_(target_points),
+          target_covariances_(target_covariances),
+          corres_(corres) {};
+    const Eigen::Vector3f *source_points_;
+    const Eigen::Matrix3f *source_covariances_;
+    const Eigen::Vector3f *target_points_;
+    const Eigen::Matrix3f *target_covariances_;
+    const Eigen::Vector2i *corres_;
+    __device__ void operator()(int i,
+                               Eigen::Vector6f J_r[3],
+                               float r[3]) const {
+        size_t cs = corres_[i][0];
+        size_t ct = corres_[i][1];
+        const Eigen::Vector3f &vs = source_points_[cs];
+        const Eigen::Matrix3f &Cs = source_covariances_[cs];
+        const Eigen::Vector3f &vt = target_points_[ct];
+        const Eigen::Matrix3f &Ct = target_covariances_[ct];
+        const Eigen::Vector3f d = vs - vt;
+        const Eigen::Matrix3f M = Ct + Cs;
+        const Eigen::Matrix3f W = Eigen::device_vectorize<float, 3, 3, sqrtf>(M.inverse());
+
+        Eigen::Matrix<float, 3, 6> J;
+        J.block<3, 3>(0, 0) = -utility::SkewMatrix(vs);
+        J.block<3, 3>(0, 3) = Eigen::Matrix3f::Identity();
+        J = W * J;
+
+        constexpr int n_rows = 3;
+        for (size_t i = 0; i < n_rows; ++i) {
+            r[i] = W.row(i).dot(d);
+            J_r[i] = J.row(i);
+        }
+    }
+};
+
+struct mahalanobis_distance_functor {
+    mahalanobis_distance_functor(
+            const Eigen::Vector3f *source_points,
+            const Eigen::Matrix3f *source_covariances,
+            const Eigen::Vector3f *target_points,
+            const Eigen::Matrix3f *target_covariances)
+        : source_points_(source_points),
+          source_covariances_(source_covariances),
+          target_points_(target_points),
+          target_covariances_(target_covariances) {};
+    const Eigen::Vector3f *source_points_;
+    const Eigen::Matrix3f *source_covariances_;
+    const Eigen::Vector3f *target_points_;
+    const Eigen::Matrix3f *target_covariances_;
+    __device__ float operator()(const Eigen::Vector2i& cor) const {
+        const Eigen::Vector3f &vs = source_points_[cor[0]];
+        const Eigen::Matrix3f &Cs = source_covariances_[cor[0]];
+        const Eigen::Vector3f &vt = target_points_[cor[1]];
+        const Eigen::Matrix3f &Ct = target_covariances_[cor[1]];
+        const Eigen::Vector3f d = vs - vt;
+        const Eigen::Matrix3f M = Ct + Cs;
+        const Eigen::Matrix3f W = Eigen::device_vectorize<float, 3, 3, sqrtf>(M.inverse());
+        return d.transpose() * W * d;
+    }
+};
+
+}  // namespace
+
+float TransformationEstimationForGeneralizedICP::ComputeRMSE(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres) const {
+    if (corres.empty()) {
+        return 0.0;
+    }
+    mahalanobis_distance_functor func(
+        thrust::raw_pointer_cast(source.points_.data()),
+        thrust::raw_pointer_cast(source.covariances_.data()),
+        thrust::raw_pointer_cast(target.points_.data()),
+        thrust::raw_pointer_cast(target.covariances_.data()));
+    float err = thrust::transform_reduce(
+            corres.begin(), corres.end(), func, 0.0f, thrust::plus<float>());
+    return std::sqrt(err / (float)corres.size());
+}
+
+Eigen::Matrix4f
+TransformationEstimationForGeneralizedICP::ComputeTransformation(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres) const {
+    if (corres.empty() || !target.HasCovariances() ||
+        !source.HasCovariances()) {
+        return Eigen::Matrix4f::Identity();
+    }
+
+    compute_jacobian_and_residual_functor func(
+        thrust::raw_pointer_cast(source.points_.data()),
+        thrust::raw_pointer_cast(source.covariances_.data()),
+        thrust::raw_pointer_cast(target.points_.data()),
+        thrust::raw_pointer_cast(target.covariances_.data()),
+        thrust::raw_pointer_cast(corres.data()));
+
+    Eigen::Matrix6f JTJ;
+    Eigen::Vector6f JTr;
+    float r2 = -1.0;
+    thrust::tie(JTJ, JTr, r2) =
+            utility::ComputeJTJandJTr<
+            Eigen::Matrix6f, Eigen::Vector6f, 3, compute_jacobian_and_residual_functor>(
+                    func, (int)corres.size());
+
+    bool is_success = false;
+    Eigen::Matrix4f extrinsic;
+    thrust::tie(is_success, extrinsic) =
+            utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);
+
+    return is_success ? extrinsic : Eigen::Matrix4f::Identity();
+}
+
+RegistrationResult RegistrationGeneralizedICP(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        float max_correspondence_distance,
+        const Eigen::Matrix4f &init /* = Eigen::Matrix4f::Identity()*/,
+        const TransformationEstimationForGeneralizedICP
+                &estimation /* = TransformationEstimationForGeneralizedICP()*/,
+        const ICPConvergenceCriteria
+                &criteria /* = ICPConvergenceCriteria()*/) {
+    return RegistrationICP(
+            *InitializePointCloudForGeneralizedICP(source, estimation.epsilon_),
+            *InitializePointCloudForGeneralizedICP(target, estimation.epsilon_),
+            max_correspondence_distance, init, estimation, criteria);
+}
+
+}  // namespace registration
+}  // namespace cupoch
diff --git a/src/cupoch/registration/generalized_icp.h b/src/cupoch/registration/generalized_icp.h
@@ -0,0 +1,69 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <memory>
+
+#include "cupoch/registration/registration.h"
+#include "cupoch/registration/transformation_estimation.h"
+
+namespace cupoch {
+namespace registration {
+
+class RegistrationResult;
+
+class TransformationEstimationForGeneralizedICP
+    : public TransformationEstimation {
+public:
+    ~TransformationEstimationForGeneralizedICP() override = default;
+
+    TransformationEstimationType GetTransformationEstimationType()
+            const override {
+        return type_;
+    };
+
+    explicit TransformationEstimationForGeneralizedICP(
+            float epsilon = 1e-3)
+        : epsilon_(epsilon) {}
+
+public:
+    float ComputeRMSE(const geometry::PointCloud &source,
+                       const geometry::PointCloud &target,
+                       const CorrespondenceSet &corres) const override;
+
+    Eigen::Matrix4f ComputeTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            const CorrespondenceSet &corres) const override;
+
+public:
+    /// Small constant representing covariance along the normal.
+    float epsilon_ = 1e-3;
+
+private:
+    const TransformationEstimationType type_ =
+            TransformationEstimationType::GeneralizedICP;
+};
+
+/// \brief Function for Generalized ICP registration.
+///
+/// This is implementation of following paper
+//  A. Segal, D .Haehnel, S. Thrun
+/// Generalized-ICP, RSS 2009.
+///
+/// \param source The source point cloud.
+/// \param target The target point cloud.
+/// \param max_distance Maximum correspondence points-pair distance.
+/// \param init Initial transformation estimation.
+/// Default value: array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.],
+/// [0., 0., 0., 1.]]). \param criteria  Convergence criteria. \param
+RegistrationResult RegistrationGeneralizedICP(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        float max_correspondence_distance,
+        const Eigen::Matrix4f &init = Eigen::Matrix4f::Identity(),
+        const TransformationEstimationForGeneralizedICP &estimation =
+                TransformationEstimationForGeneralizedICP(),
+        const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria());
+
+}  // namespace registration
+}  // namespace cupoch
diff --git a/src/cupoch/registration/transformation_estimation.h b/src/cupoch/registration/transformation_estimation.h
@@ -41,6 +41,7 @@ enum class TransformationEstimationType {
     PointToPlane = 2,
     SymmetricMethod = 3,
     ColoredICP = 4,
+    GeneralizedICP = 5,
 };
 
 /// Base class that estimates a transformation between two point clouds

diff --git a/src/cupoch/utility/eigen.cu b/src/cupoch/utility/eigen.cu
@@ -25,12 +25,6 @@
 namespace cupoch {
 namespace utility {
 
-Eigen::Matrix3f SkewMatrix(const Eigen::Vector3f &vec) {
-    return (Eigen::Matrix3f() << 0,      -vec.z(),  vec.y(),
-                                 vec.z(), 0,       -vec.x(),
-                                -vec.y(), vec.x(),  0).finished();
-}
-
 Eigen::Matrix4f TransformVector6fToMatrix4f(const Eigen::Vector6f &input) {
     Eigen::Matrix4f output = Eigen::Matrix4f::Identity();
     output.topRightCorner<3, 1>() = input.tail<3>();

diff --git a/src/cupoch/utility/eigen.h b/src/cupoch/utility/eigen.h
@@ -54,7 +54,11 @@ struct multiple_jacobians_residuals_functor {
 };
 
 /// Genretate a skew-symmetric matrix from a vector 3x1.
-Eigen::Matrix3f SkewMatrix(const Eigen::Vector3f &vec);
+inline __device__ Eigen::Matrix3f SkewMatrix(const Eigen::Vector3f &vec) {
+    return (Eigen::Matrix3f() << 0,      -vec.z(),  vec.y(),
+                                 vec.z(), 0,       -vec.x(),
+                                -vec.y(), vec.x(),  0).finished();
+}
 
 /// Function to transform 6D motion vector to 4D motion matrix
 Eigen::Matrix4f TransformVector6fToMatrix4f(const Eigen::Vector6f &input);

diff --git a/src/cupoch/utility/helper.h b/src/cupoch/utility/helper.h
@@ -182,6 +182,19 @@ __host__ __device__ Eigen::Matrix<T, Dim, 1> device_vectorize(
     return ans;
 }
 
+template <typename T, int M, int N, float (*Func)(float)>
+__host__ __device__ Eigen::Matrix<T, M, N> device_vectorize(
+        const Eigen::Matrix<T, M, N> &x) {
+    Eigen::Matrix<T, M, N> ans;
+#pragma unroll
+    for (int k = 0; k < M * N; ++k) {
+        int i = k / N;
+        int j = i % N;
+        ans(i, j) = Func(x(i, j));
+    }
+    return ans;
+}
+
 }  // namespace Eigen
 
 namespace cupoch {