diff --git a/README.md b/README.md index bececd4f..fb60e144 100644 --- 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 new file mode 100644 index 00000000..2470aa18 --- /dev/null +++ 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 new file mode 100644 index 00000000..64a29eff --- /dev/null +++ b/src/cupoch/registration/generalized_icp.cu @@ -0,0 +1,201 @@ +#include "cupoch/registration/generalized_icp.h" + +#include +#include + +#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 InitializePointCloudForGeneralizedICP( + const geometry::PointCloud &pcd, float epsilon) { + auto output = std::make_shared(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 { + 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(M.inverse()); + + Eigen::Matrix 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(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()); + 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 \ No newline at end of file diff --git a/src/cupoch/registration/generalized_icp.h b/src/cupoch/registration/generalized_icp.h new file mode 100644 index 00000000..f7ea7313 --- /dev/null +++ b/src/cupoch/registration/generalized_icp.h @@ -0,0 +1,69 @@ +#pragma once + +#include +#include + +#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 index fe344abd..04971b12 100644 --- 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 index 5c58f726..f5cd855b 100644 --- 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 index 7cbc0cb7..445853e7 100644 --- 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 index 0f76b5ba..aa4c7f12 100644 --- a/src/cupoch/utility/helper.h +++ b/src/cupoch/utility/helper.h @@ -182,6 +182,19 @@ __host__ __device__ Eigen::Matrix device_vectorize( return ans; } +template +__host__ __device__ Eigen::Matrix device_vectorize( + const Eigen::Matrix &x) { + Eigen::Matrix 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 { diff --git a/src/python/cupoch_pybind/registration/registration.cpp b/src/python/cupoch_pybind/registration/registration.cpp index f00ac38d..0edfc457 100644 --- a/src/python/cupoch_pybind/registration/registration.cpp +++ b/src/python/cupoch_pybind/registration/registration.cpp @@ -24,6 +24,7 @@ #include "cupoch/registration/colored_icp.h" #include "cupoch/registration/fast_global_registration.h" #include "cupoch/registration/filterreg.h" +#include "cupoch/registration/generalized_icp.h" #include "cupoch/registration/registration.h" #include "cupoch/utility/console.h" #include "cupoch_pybind/docstring.h" @@ -122,6 +123,7 @@ void pybind_registration_classes(py::module &m) { .value("PointToPlane", registration::TransformationEstimationType::PointToPlane) .value("SymmetricMethod", registration::TransformationEstimationType::SymmetricMethod) .value("ColoredICP", registration::TransformationEstimationType::ColoredICP) + .value("GeneralizedICP", registration::TransformationEstimationType::GeneralizedICP) .export_values(); // cupoch.registration.TransformationEstimationPointToPoint: @@ -184,6 +186,34 @@ void pybind_registration_classes(py::module &m) { return std::string("TransformationEstimationSymmetricMethod"); }); + // cupoch.registration.TransformationEstimationForGeneralizedICP: + // TransformationEstimation + py::class_, + registration::TransformationEstimation> + te_gicp(m, "TransformationEstimationForGeneralizedICP", + "Class to estimate a transformation for Generalized ICP."); + py::detail::bind_copy_functions< + registration::TransformationEstimationForGeneralizedICP>(te_gicp); + te_gicp.def(py::init([](float epsilon) { + return new registration::TransformationEstimationForGeneralizedICP( + epsilon); + }), + "epsilon"_a = 1e-3) + .def_readwrite("epsilon", + ®istration::TransformationEstimationForGeneralizedICP:: + epsilon_, + "Small constant representing covariance along the " + "normal.") + .def("__repr__", + [](const registration::TransformationEstimationForGeneralizedICP + &te) { + return std::string( + "registration::" + "TransformationEstimationForGeneralizedICP"); + }); + // cupoch.registration.FastGlobalRegistrationOption: py::class_ fgr_option( m, "FastGlobalRegistrationOption", @@ -373,7 +403,8 @@ static const std::unordered_map "Estimation method. One of " "(``registration::TransformationEstimationPointToPoint``," "``registration::TransformationEstimationPointToPlane``," - "``registration::TransformationEstimationSymmetricMethod``)"}, + "``registration::TransformationEstimationSymmetricMethod``," + "``registration::TransformationEstimationForGeneralizedICP``)"}, {"init", "Initial transformation estimation"}, {"lambda_geometric", "lambda_geometric value"}, {"max_correspondence_distance", @@ -409,6 +440,13 @@ void pybind_registration_methods(py::module &m) { docstring::FunctionDocInject(m, "registration_colored_icp", map_shared_argument_docstrings); + m.def("registration_generalized_icp", ®istration::RegistrationGeneralizedICP, + "Function for Generalized ICP registration", "source"_a, "target"_a, + "max_correspondence_distance"_a, + "init"_a = Eigen::Matrix4f::Identity(), + "estimation"_a = registration::TransformationEstimationForGeneralizedICP(), + "criteria"_a = registration::ICPConvergenceCriteria()); + m.def("registration_fast_based_on_feature_matching", ®istration::FastGlobalRegistration<33>, "Function for fast global registration based on feature matching",