diff --git a/src/cupoch/registration/transformation_estimation.cu b/src/cupoch/registration/transformation_estimation.cu index daad683e..a50934c3 100644 --- a/src/cupoch/registration/transformation_estimation.cu +++ b/src/cupoch/registration/transformation_estimation.cu @@ -55,6 +55,52 @@ struct pt2pl_jacobian_residual_functor } }; +struct pl2pl_jacobian_residual_functor + : public utility::jacobian_residual_functor { + pl2pl_jacobian_residual_functor(const Eigen::Vector3f *source, + const Eigen::Vector3f *source_normals, + const Eigen::Vector3f *target_points, + const Eigen::Vector3f *target_normals, + const Eigen::Vector2i *corres) + : source_(source), + source_normals_(source_normals), + target_points_(target_points), + target_normals_(target_normals), + corres_(corres){}; + const Eigen::Vector3f *source_; + const Eigen::Vector3f *source_normals_; + const Eigen::Vector3f *target_points_; + const Eigen::Vector3f *target_normals_; + const Eigen::Vector2i *corres_; + __device__ void operator()(int idx, Eigen::Vector6f &vec, float &r) const { + const Eigen::Vector3f &vs = source_[corres_[idx][0]]; + const Eigen::Vector3f &ns = source_normals_[corres_[idx][0]]; + const Eigen::Vector3f &vt = target_points_[corres_[idx][1]]; + const Eigen::Vector3f &nt = target_normals_[corres_[idx][1]]; + + Eigen::Vector3f n = ns + nt; // sum of normals + Eigen::Vector3f d = vs - vt; // difference of points + + r = d.dot(n); // symmetric residual + + Eigen::Vector3f cross_product = (vs + vt).cross(n); + vec.block<3, 1>(0, 0) = cross_product; + vec.block<3, 1>(3, 0) = n; + } +}; + +// Define a function or a lambda to compute the error using normals +__device__ float ComputeErrorUsingNormals( + const Eigen::Vector3f &source_point, + const Eigen::Vector3f &source_normal, + const Eigen::Vector3f &target_point, + const Eigen::Vector3f &target_normal) { + // Implement the error calculation logic here + // This is just a placeholder logic + return (source_point - target_point).dot(target_normal) + + (source_point - target_point).dot(source_normal); +} + } // namespace float TransformationEstimationPointToPoint::ComputeRMSE( @@ -114,7 +160,8 @@ float TransformationEstimationPointToPlane::ComputeRMSE( target.normals_.begin(), thrust::make_transform_iterator( corres.begin(), - element_get_functor()))), + element_get_functor()))), make_tuple_iterator( thrust::make_permutation_iterator( source.points_.begin(), @@ -130,7 +177,8 @@ float TransformationEstimationPointToPlane::ComputeRMSE( target.normals_.begin(), thrust::make_transform_iterator( corres.end(), - element_get_functor()))), + element_get_functor()))), [] __device__(const thrust::tuple &x) { float r = (thrust::get<0>(x) - thrust::get<1>(x)) @@ -167,5 +215,100 @@ Eigen::Matrix4f TransformationEstimationPointToPlane::ComputeTransformation( utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr, det_thresh_); + return is_success ? extrinsic : Eigen::Matrix4f::Identity(); +} + +float TransformationEstimationSymmetricMethod::ComputeRMSE( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + if (corres.empty() || !source.HasNormals() || !target.HasNormals()) + return 0.0; + const float err = thrust::transform_reduce( + utility::exec_policy(0), + make_tuple_iterator( + thrust::make_permutation_iterator( + source.points_.begin(), + thrust::make_transform_iterator( + corres.begin(), + element_get_functor())), + thrust::make_permutation_iterator( + source.normals_.begin(), + thrust::make_transform_iterator( + corres.begin(), + element_get_functor())), + thrust::make_permutation_iterator( + target.points_.begin(), + thrust::make_transform_iterator( + corres.begin(), + element_get_functor())), + thrust::make_permutation_iterator( + target.normals_.begin(), + thrust::make_transform_iterator( + corres.begin(), + element_get_functor()))), + make_tuple_iterator( + thrust::make_permutation_iterator( + source.points_.begin(), + thrust::make_transform_iterator( + corres.end(), + element_get_functor())), + thrust::make_permutation_iterator( + source.normals_.begin(), + thrust::make_transform_iterator( + corres.end(), + element_get_functor())), + thrust::make_permutation_iterator( + target.points_.begin(), + thrust::make_transform_iterator( + corres.end(), + element_get_functor())), + thrust::make_permutation_iterator( + target.normals_.begin(), + thrust::make_transform_iterator( + corres.end(), + element_get_functor()))), + [] __device__( + const thrust::tuple &x) { + // Compute error using both source and target normals + float r = ComputeErrorUsingNormals( + thrust::get<0>(x), thrust::get<1>(x), thrust::get<2>(x), + thrust::get<3>(x)); + return r * r; + }, + 0.0f, thrust::plus()); + return std::sqrt(err / (float)corres.size()); +} + +Eigen::Matrix4f TransformationEstimationSymmetricMethod::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + if (corres.empty() || !target.HasNormals()) + return Eigen::Matrix4f::Identity(); + + Eigen::Matrix6f JTJ; + Eigen::Vector6f JTr; + float r2; + pl2pl_jacobian_residual_functor func( + thrust::raw_pointer_cast(source.points_.data()), + thrust::raw_pointer_cast(source.normals_.data()), + thrust::raw_pointer_cast(target.points_.data()), + thrust::raw_pointer_cast(target.normals_.data()), + thrust::raw_pointer_cast(corres.data())); + thrust::tie(JTJ, JTr, r2) = + utility::ComputeJTJandJTr( + func, (int)corres.size()); + + bool is_success; + Eigen::Matrix4f extrinsic; + thrust::tie(is_success, extrinsic) = + utility::SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr, + det_thresh_); + return is_success ? extrinsic : Eigen::Matrix4f::Identity(); } \ No newline at end of file diff --git a/src/cupoch/registration/transformation_estimation.h b/src/cupoch/registration/transformation_estimation.h index a0aa262b..fe344abd 100644 --- a/src/cupoch/registration/transformation_estimation.h +++ b/src/cupoch/registration/transformation_estimation.h @@ -39,7 +39,8 @@ enum class TransformationEstimationType { Unspecified = 0, PointToPoint = 1, PointToPlane = 2, - ColoredICP = 3, + SymmetricMethod = 3, + ColoredICP = 4, }; /// Base class that estimates a transformation between two point clouds @@ -113,5 +114,32 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { TransformationEstimationType::PointToPlane; }; +/// Estimate a transformation for plane to plane distance +class TransformationEstimationSymmetricMethod : public TransformationEstimation { +public: + TransformationEstimationSymmetricMethod(float det_thresh = 1.0e-6) + : det_thresh_(det_thresh) {} + ~TransformationEstimationSymmetricMethod() override {} + +public: + TransformationEstimationType GetTransformationEstimationType() + const override { + return type_; + }; + 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; + + float det_thresh_; + +private: + const TransformationEstimationType type_ = + TransformationEstimationType::SymmetricMethod; +}; + } // namespace registration } // namespace cupoch \ No newline at end of file diff --git a/src/python/cupoch_pybind/registration/registration.cpp b/src/python/cupoch_pybind/registration/registration.cpp index 2990b2b2..f00ac38d 100644 --- a/src/python/cupoch_pybind/registration/registration.cpp +++ b/src/python/cupoch_pybind/registration/registration.cpp @@ -119,9 +119,10 @@ void pybind_registration_classes(py::module &m) { py::enum_ tf_type(m, "TransformationEstimationType"); tf_type.value("PointToPoint", registration::TransformationEstimationType::PointToPoint) - .value("PointToPlane", registration::TransformationEstimationType::PointToPlane) - .value("ColoredICP", registration::TransformationEstimationType::ColoredICP) - .export_values(); + .value("PointToPlane", registration::TransformationEstimationType::PointToPlane) + .value("SymmetricMethod", registration::TransformationEstimationType::SymmetricMethod) + .value("ColoredICP", registration::TransformationEstimationType::ColoredICP) + .export_values(); // cupoch.registration.TransformationEstimationPointToPoint: // TransformationEstimation @@ -164,6 +165,25 @@ void pybind_registration_classes(py::module &m) { return std::string("TransformationEstimationPointToPlane"); }); + // cupoch.registration.TransformationEstimationSymmetricMethod: + // TransformationEstimation + py::class_, + registration::TransformationEstimation> + te_l2l(m, "TransformationEstimationSymmetricMethod", + "Class to estimate a transformation for plane to plane " + "distance."); + py::detail::bind_default_constructor< + registration::TransformationEstimationSymmetricMethod>(te_l2l); + py::detail::bind_copy_functions< + registration::TransformationEstimationSymmetricMethod>(te_l2l); + te_l2l.def( + "__repr__", + [](const registration::TransformationEstimationSymmetricMethod &te) { + return std::string("TransformationEstimationSymmetricMethod"); + }); + // cupoch.registration.FastGlobalRegistrationOption: py::class_ fgr_option( m, "FastGlobalRegistrationOption", @@ -351,8 +371,9 @@ static const std::unordered_map {"criteria", "Convergence criteria"}, {"estimation_method", "Estimation method. One of " - "(``registration::TransformationEstimationPointToPoint``, " - "``registration::TransformationEstimationPointToPlane``)"}, + "(``registration::TransformationEstimationPointToPoint``," + "``registration::TransformationEstimationPointToPlane``," + "``registration::TransformationEstimationSymmetricMethod``)"}, {"init", "Initial transformation estimation"}, {"lambda_geometric", "lambda_geometric value"}, {"max_correspondence_distance",