diff --git a/CMakeLists.txt b/CMakeLists.txt index caf91744..33a4a380 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/src/cupoch/geometry/pointcloud.cu b/src/cupoch/geometry/pointcloud.cu index bdd4c2a1..f9f5a172 100644 --- a/src/cupoch/geometry/pointcloud.cu +++ b/src/cupoch/geometry/pointcloud.cu @@ -279,6 +279,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::Crop( const AxisAlignedBoundingBox<3> &bbox) const { if (bbox.IsEmpty()) { diff --git a/src/cupoch/geometry/pointcloud.h b/src/cupoch/geometry/pointcloud.h index 4eb60979..97ea9ea8 100644 --- a/src/cupoch/geometry/pointcloud.h +++ b/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; diff --git a/src/cupoch/registration/colored_icp.cu b/src/cupoch/registration/colored_icp.cu index 1e89b160..e5eb75a0 100644 --- a/src/cupoch/registration/colored_icp.cu +++ b/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( - func, (int)corres.size()); + stream1, func, (int)corres.size()); bool is_success; Eigen::Matrix4f extrinsic; diff --git a/src/cupoch/registration/correspondence_checker.cu b/src/cupoch/registration/correspondence_checker.cu new file mode 100644 index 00000000..b030f237 --- /dev/null +++ b/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 +#include + +#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(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 diff --git a/src/cupoch/registration/correspondence_checker.h b/src/cupoch/registration/correspondence_checker.h new file mode 100644 index 00000000..8c510563 --- /dev/null +++ b/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 +#include +#include +#include + +#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 diff --git a/src/cupoch/registration/registration.cu b/src/cupoch/registration/registration.cu index 395e0ca7..9ee3c176 100644 --- a/src/cupoch/registration/registration.cu +++ b/src/cupoch/registration/registration.cu @@ -24,6 +24,9 @@ #include "cupoch/registration/registration.h" #include "cupoch/utility/console.h" #include "cupoch/utility/helper.h" +#include "cupoch/utility/random.h" + +#include using namespace cupoch; using namespace cupoch::registration; @@ -31,6 +34,7 @@ using namespace cupoch::registration; namespace { RegistrationResult GetRegistrationResultAndCorrespondences( + cudaStream_t stream, const geometry::PointCloud &source, const geometry::PointCloud &target, const knn::KDTreeFlann &target_kdtree, @@ -48,10 +52,11 @@ RegistrationResult GetRegistrationResultAndCorrespondences( indices, dists); result.correspondence_set_.resize(n_pt); const float error2 = thrust::transform_reduce( - utility::exec_policy(0), dists.begin(), dists.end(), + utility::exec_policy(stream), dists.begin(), dists.end(), [] __device__(float d) { return (isinf(d)) ? 0.0 : d; }, 0.0f, thrust::plus()); - thrust::transform(enumerate_begin(indices), enumerate_end(indices), + thrust::transform(utility::exec_policy(stream), + enumerate_begin(indices), enumerate_end(indices), result.correspondence_set_.begin(), [] __device__(const thrust::tuple &idxs) { int j = thrust::get<1>(idxs); @@ -60,7 +65,8 @@ RegistrationResult GetRegistrationResultAndCorrespondences( j); }); auto end = - thrust::remove_if(result.correspondence_set_.begin(), + thrust::remove_if(utility::exec_policy(stream), + result.correspondence_set_.begin(), result.correspondence_set_.end(), [] __device__(const Eigen::Vector2i &x) -> bool { return (x[0] < 0); @@ -79,6 +85,29 @@ RegistrationResult GetRegistrationResultAndCorrespondences( return result; } +struct correspondence_count_functor { + const Eigen::Vector3f *source_points_; + float max_dis2_; + correspondence_count_functor(float max_correspondence_distance) + : max_dis2_(max_correspondence_distance * max_correspondence_distance) {} + __device__ int operator()(const Eigen::Vector2i &c) const { + float dis2 = (source_points_[c[0]] - source_points_[c[1]]).squaredNorm(); + return (dis2 < max_dis2_) ? 1 : 0; + } +}; + +float EvaluateInlierCorrespondenceRatio( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + float max_correspondence_distance, + const Eigen::Matrix4f &transformation) { + int inlier_corres = thrust::transform_reduce( + utility::exec_policy(0), corres.begin(), corres.end(), + correspondence_count_functor(max_correspondence_distance), 0, thrust::plus()); + return static_cast(inlier_corres / corres.size()); +} + } // namespace RegistrationResult::RegistrationResult(const Eigen::Matrix4f &transformation) @@ -115,7 +144,7 @@ RegistrationResult cupoch::registration::EvaluateRegistration( pcd.Transform(transformation); } return GetRegistrationResultAndCorrespondences( - pcd, target, kdtree, max_correspondence_distance, transformation); + 0, pcd, target, kdtree, max_correspondence_distance, transformation); } RegistrationResult cupoch::registration::RegistrationICP( @@ -150,7 +179,7 @@ RegistrationResult cupoch::registration::RegistrationICP( } RegistrationResult result; result = GetRegistrationResultAndCorrespondences( - pcd, target, kdtree, max_correspondence_distance, transformation); + 0, pcd, target, kdtree, max_correspondence_distance, transformation); for (int i = 0; i < criteria.max_iteration_; i++) { utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", i, result.fitness_, result.inlier_rmse_); @@ -160,7 +189,7 @@ RegistrationResult cupoch::registration::RegistrationICP( pcd.Transform(update); RegistrationResult backup = result; result = GetRegistrationResultAndCorrespondences( - pcd, target, kdtree, max_correspondence_distance, + 0, pcd, target, kdtree, max_correspondence_distance, transformation); if (std::abs(backup.fitness_ - result.fitness_) < criteria.relative_fitness_ && @@ -170,4 +199,114 @@ RegistrationResult cupoch::registration::RegistrationICP( } } return result; +} + +RegistrationResult RegistrationRANSACBasedOnCorrespondence( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + float max_correspondence_distance, + const TransformationEstimation &estimation + /* = TransformationEstimationPointToPoint(false)*/, + int ransac_n /* = 3*/, + const std::vector> + &checkers /* = {}*/, + const RANSACConvergenceCriteria &criteria + /* = RANSACConvergenceCriteria()*/) { + if (ransac_n < 3 || (int)corres.size() < ransac_n || + max_correspondence_distance <= 0.0) { + return RegistrationResult(); + } + RegistrationResult best_result; + knn::KDTreeFlann kdtree(geometry::ConvertVector3fVectorRef(target)); + int est_k_global = criteria.max_iteration_; + int total_validation = 0; + +#pragma omp parallel + { + CorrespondenceSet ransac_corres(ransac_n); + RegistrationResult best_result_local; + int est_k_local = criteria.max_iteration_; + utility::random::UniformIntGenerator rand_gen(0, + corres.size() - 1); + +#pragma omp for nowait + for (int itr = 0; itr < criteria.max_iteration_; itr++) { + if (itr >= est_k_global) { + continue; + } + int thread_id = omp_get_thread_num(); + cudaStream_t stream1 = utility::GetStream((2 * thread_id) % utility::MAX_NUM_STREAMS); + cudaStream_t stream2 = utility::GetStream((2 * thread_id + 1) % utility::MAX_NUM_STREAMS); + // Randomly select ransac_n correspondences + for (int i = 0; i < ransac_n; i++) { + ransac_corres[i] = corres[rand_gen()]; + } + // Estimate transformation + Eigen::Matrix4f transformation = estimation.ComputeTransformation( + stream1, stream2, + source, target, ransac_corres); + + bool check = true; + for (const auto &checker : checkers) { + if (!checker.get().Check(source, target, ransac_corres, + transformation)) { + check = false; + break; + } + } + if (!check) continue; + + geometry::PointCloud pcd = source; + pcd.Transform(stream1, stream2, transformation); + RegistrationResult result = GetRegistrationResultAndCorrespondences( + stream1, pcd, target, kdtree, max_correspondence_distance, + transformation); + if (result.IsBetterRANSACThan(best_result_local)) { + best_result_local = result; + + float corres_inlier_ratio = + EvaluateInlierCorrespondenceRatio( + pcd, target, corres, + max_correspondence_distance, + transformation); + + // Update exit condition if necessary. + // If confidence is 1.0, then it is safely inf, we always + // consume all the iterations. + float est_k_local_d = + std::log(1.0 - criteria.confidence_) / + std::log(1.0 - + std::pow(corres_inlier_ratio, ransac_n)); + est_k_local = + est_k_local_d < est_k_global + ? static_cast(std::ceil(est_k_local_d)) + : est_k_local; + utility::LogDebug( + "Thread {:06d}: registration fitness={:.3f}, " + "corres inlier ratio={:.3f}, " + "Est. max k = {}", + itr, result.fitness_, corres_inlier_ratio, + est_k_local_d); + } +#pragma omp critical + { + total_validation += 1; + if (est_k_local < est_k_global) { + est_k_global = est_k_local; + } + } + } // for loop +#pragma omp critical(RegistrationRANSACBasedOnCorrespondence) + { + if (best_result_local.IsBetterRANSACThan(best_result)) { + best_result = best_result_local; + } + } + } + utility::LogDebug( + "RANSAC exits after {:d} validations. Best inlier ratio {:e}, " + "RMSE {:e}", + total_validation, best_result.fitness_, best_result.inlier_rmse_); + return best_result; } \ No newline at end of file diff --git a/src/cupoch/registration/registration.h b/src/cupoch/registration/registration.h index fb859d8b..e3ed0e81 100644 --- a/src/cupoch/registration/registration.h +++ b/src/cupoch/registration/registration.h @@ -21,6 +21,7 @@ #pragma once #include +#include "cupoch/registration/correspondence_checker.h" #include "cupoch/registration/transformation_estimation.h" #include "cupoch/utility/eigen.h" @@ -48,6 +49,22 @@ class ICPConvergenceCriteria { int max_iteration_; }; + +class RANSACConvergenceCriteria { +public: + RANSACConvergenceCriteria(int max_iteration = 100000, + double confidence = 0.999) + : max_iteration_(max_iteration), + confidence_(std::max(std::min(confidence, 1.0), 0.0)) {} + + ~RANSACConvergenceCriteria() {} + +public: + int max_iteration_; + double confidence_; +}; + + class RegistrationResult { public: RegistrationResult(const Eigen::Matrix4f &transformation = @@ -59,6 +76,11 @@ class RegistrationResult { const thrust::host_vector &corres); thrust::host_vector GetCorrespondenceSet() const; + bool IsBetterRANSACThan(const RegistrationResult &other) const { + return fitness_ > other.fitness_ || (fitness_ == other.fitness_ && + inlier_rmse_ < other.inlier_rmse_); + } + public: Eigen::Matrix4f_u transformation_; CorrespondenceSet correspondence_set_; @@ -90,5 +112,22 @@ RegistrationResult RegistrationICP( TransformationEstimationPointToPoint(), const ICPConvergenceCriteria &criteria = ICPConvergenceCriteria()); + +/// \brief Function for global RANSAC registration based on a given set of +/// correspondences. +RegistrationResult RegistrationRANSACBasedOnCorrespondence( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres, + float max_correspondence_distance, + const TransformationEstimation &estimation = + TransformationEstimationPointToPoint(), + int ransac_n = 3, + const std::vector> + &checkers = {}, + const RANSACConvergenceCriteria &criteria = + RANSACConvergenceCriteria()); + + } // namespace registration } // namespace cupoch \ No newline at end of file diff --git a/src/cupoch/registration/transformation_estimation.cu b/src/cupoch/registration/transformation_estimation.cu index e8602d2b..20e4c2c5 100644 --- a/src/cupoch/registration/transformation_estimation.cu +++ b/src/cupoch/registration/transformation_estimation.cu @@ -106,6 +106,13 @@ __device__ float ComputeErrorUsingNormals( } // namespace +Eigen::Matrix4f TransformationEstimation::ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const { + return ComputeTransformation(0, 0, source, target, corres); +} + float TransformationEstimationPointToPoint::ComputeRMSE( const geometry::PointCloud &source, const geometry::PointCloud &target, @@ -135,10 +142,11 @@ float TransformationEstimationPointToPoint::ComputeRMSE( } Eigen::Matrix4f TransformationEstimationPointToPoint::ComputeTransformation( + cudaStream_t stream1, cudaStream_t stream2, const geometry::PointCloud &source, const geometry::PointCloud &target, const CorrespondenceSet &corres) const { - return Kabsch(source.points_, target.points_, corres); + return Kabsch(stream1, stream2, source.points_, target.points_, corres); } float TransformationEstimationPointToPlane::ComputeRMSE( @@ -193,6 +201,7 @@ float TransformationEstimationPointToPlane::ComputeRMSE( } Eigen::Matrix4f TransformationEstimationPointToPlane::ComputeTransformation( + cudaStream_t stream1, cudaStream_t stream2, const geometry::PointCloud &source, const geometry::PointCloud &target, const CorrespondenceSet &corres) const { @@ -210,7 +219,7 @@ Eigen::Matrix4f TransformationEstimationPointToPlane::ComputeTransformation( thrust::tie(JTJ, JTr, r2) = utility::ComputeJTJandJTr( - func, (int)corres.size()); + stream1, func, (int)corres.size()); bool is_success; Eigen::Matrix4f extrinsic; @@ -287,6 +296,7 @@ float TransformationEstimationSymmetricMethod::ComputeRMSE( } Eigen::Matrix4f TransformationEstimationSymmetricMethod::ComputeTransformation( + cudaStream_t stream1, cudaStream_t stream2, const geometry::PointCloud &source, const geometry::PointCloud &target, const CorrespondenceSet &corres) const { @@ -305,7 +315,7 @@ Eigen::Matrix4f TransformationEstimationSymmetricMethod::ComputeTransformation( thrust::tie(JTJ, JTr, r2) = utility::ComputeJTJandJTr( - func, (int)corres.size()); + stream1, func, (int)corres.size()); bool is_success; Eigen::Matrix4f transformation_matrix; diff --git a/src/cupoch/registration/transformation_estimation.h b/src/cupoch/registration/transformation_estimation.h index 04971b12..5f2527dc 100644 --- a/src/cupoch/registration/transformation_estimation.h +++ b/src/cupoch/registration/transformation_estimation.h @@ -58,7 +58,12 @@ class TransformationEstimation { virtual float ComputeRMSE(const geometry::PointCloud &source, const geometry::PointCloud &target, const CorrespondenceSet &corres) const = 0; + Eigen::Matrix4f ComputeTransformation( + const geometry::PointCloud &source, + const geometry::PointCloud &target, + const CorrespondenceSet &corres) const; virtual Eigen::Matrix4f ComputeTransformation( + cudaStream_t stream1, cudaStream_t stream2, const geometry::PointCloud &source, const geometry::PointCloud &target, const CorrespondenceSet &corres) const = 0; @@ -79,6 +84,7 @@ class TransformationEstimationPointToPoint : public TransformationEstimation { 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; @@ -104,6 +110,7 @@ class TransformationEstimationPointToPlane : public TransformationEstimation { 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; @@ -131,6 +138,7 @@ class TransformationEstimationSymmetricMethod : public TransformationEstimation 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; diff --git a/src/cupoch/utility/random.cpp b/src/cupoch/utility/random.cpp new file mode 100644 index 00000000..9aaa3883 --- /dev/null +++ b/src/cupoch/utility/random.cpp @@ -0,0 +1,59 @@ +#include "cupoch/utility/random.h" + +#include "cupoch/utility/console.h" + +namespace cupoch { +namespace utility { +namespace random { + +/// Global thread-safe singleton instance for random generation. +/// Generates compiler/OS/device independent random numbers. +class RandomContext { +public: + RandomContext(RandomContext const&) = delete; + void operator=(RandomContext const&) = delete; + + /// Returns the singleton instance. + static RandomContext& GetInstance() { + static RandomContext instance; + return instance; + } + + /// Seed the random number generator (globally). + void Seed(const int seed) { + seed_ = seed; + engine_ = std::mt19937(seed_); + } + + /// This is used by other downstream random generators. + /// You must also lock the GetMutex() before calling the engine. + std::mt19937* GetEngine() { return &engine_; } + + /// Get global singleton mutex to protect the engine call. + std::mutex* GetMutex() { return &mutex_; } + +private: + RandomContext() { + // Randomly seed the seed by default. + std::random_device rd; + Seed(rd()); + } + int seed_; + std::mt19937 engine_; + std::mutex mutex_; +}; + +void Seed(const int seed) { RandomContext::GetInstance().Seed(seed); } + +std::mt19937* GetEngine() { return RandomContext::GetInstance().GetEngine(); } + +std::mutex* GetMutex() { return RandomContext::GetInstance().GetMutex(); } + +uint32_t RandUint32() { + std::lock_guard lock(*GetMutex()); + return (*GetEngine())(); +} + +} // namespace random +} // namespace utility +} // namespace cupoch diff --git a/src/cupoch/utility/random.h b/src/cupoch/utility/random.h new file mode 100644 index 00000000..0996a355 --- /dev/null +++ b/src/cupoch/utility/random.h @@ -0,0 +1,86 @@ +#pragma once + +#include +#include + +#include "cupoch/utility/console.h" + +namespace cupoch { +namespace utility { +namespace random { + +/// Set Open3D global random seed. +void Seed(const int seed); + +/// Get global singleton random engine. +/// You must also lock the global mutex before calling the engine. +/// +/// Example: +/// ```cpp +/// #include "open3d/utility/Random.h" +/// +/// { +/// // Put the lock and the call to the engine in the same scope. +/// std::lock_guard lock(*utility::random::GetMutex()); +/// std::shuffle(vals.begin(), vals.end(), *utility::random::GetEngine()); +/// } +/// ``` +std::mt19937* GetEngine(); + +/// Get global singleton mutex to protect the engine call. Also see +/// random::GetEngine(). +std::mutex* GetMutex(); + +/// Generate a random uint32. +/// This function is globally seeded by utility::random::Seed(). +/// This function is automatically protected by the global random mutex. +uint32_t RandUint32(); + +/// Generate uniformly distributed random integers in [low, high). +/// This class is globally seeded by utility::random::Seed(). +/// This class is a wrapper around std::uniform_int_distribution. +/// +/// Example: +/// ```cpp +/// #include "open3d/utility/Random.h" +/// +/// // Globally seed Open3D. This will affect all random functions. +/// utility::random::Seed(0); +/// +/// // Generate a random int in [0, 100). +/// utility::random::UniformIntGenerator gen(0, 100); +/// for (size_t i = 0; i < 10; i++) { +/// std::cout << gen() << std::endl; +/// } +/// ``` +template +class UniformIntGenerator { +public: + /// Generate uniformly distributed random integer from + /// [low, low + 1, ... high - 1]. + /// + /// \param low The lower bound (inclusive). + /// \param high The upper bound (exclusive). \p high must be > \p low. + UniformIntGenerator(const T low, const T high) : distribution_(low, high) { + if (low < 0) { + utility::LogError("low must be > 0, but got {}.", low); + } + if (low >= high) { + utility::LogError("low must be < high, but got low={} and high={}.", + low, high); + } + } + + /// Call this to generate a uniformly distributed integer. + T operator()() { + std::lock_guard lock(*GetMutex()); + return distribution_(*GetEngine()); + } + +protected: + std::uniform_int_distribution distribution_; +}; + +} +} // namespace utility +} // namespace cupoch diff --git a/src/python/cupoch_pybind/geometry/pointcloud.cpp b/src/python/cupoch_pybind/geometry/pointcloud.cpp index 88266bac..a0341acc 100644 --- a/src/python/cupoch_pybind/geometry/pointcloud.cpp +++ b/src/python/cupoch_pybind/geometry/pointcloud.cpp @@ -113,7 +113,8 @@ void pybind_pointcloud(py::module &m) { "Returns ``True`` if the point cloud contains point colors.") .def("normalize_normals", &geometry::PointCloud::NormalizeNormals, "Normalize point normals to length 1.") - .def("transform", &geometry::PointCloud::Transform, + .def("transform", + py::overload_cast(&geometry::PointCloud::Transform), "Apply transformation (4x4 matrix) to the geometry " "coordinates.") .def("get_oriented_bounding_box", diff --git a/src/python/cupoch_pybind/registration/registration.cpp b/src/python/cupoch_pybind/registration/registration.cpp index 0edfc457..05a73306 100644 --- a/src/python/cupoch_pybind/registration/registration.cpp +++ b/src/python/cupoch_pybind/registration/registration.cpp @@ -22,6 +22,7 @@ #include "cupoch/geometry/pointcloud.h" #include "cupoch/registration/colored_icp.h" +#include "cupoch/registration/correspondence_checker.h" #include "cupoch/registration/fast_global_registration.h" #include "cupoch/registration/filterreg.h" #include "cupoch/registration/generalized_icp.h" @@ -52,13 +53,26 @@ class PyTransformationEstimation : public TransformationEstimationBase { Eigen::Matrix4f ComputeTransformation( const geometry::PointCloud &source, const geometry::PointCloud &target, - const registration::CorrespondenceSet &corres) const override { + const registration::CorrespondenceSet &corres) const { PYBIND11_OVERLOAD_PURE(Eigen::Matrix4f, TransformationEstimationBase, source, target, corres); } #endif }; +template +class PyCorrespondenceChecker : public CorrespondenceCheckerBase { +public: + using CorrespondenceCheckerBase::CorrespondenceCheckerBase; + bool Check(const geometry::PointCloud &source, + const geometry::PointCloud &target, + const registration::CorrespondenceSet &corres, + const Eigen::Matrix4f &transformation) const override { + PYBIND11_OVERLOAD_PURE(bool, CorrespondenceCheckerBase, source, target, + corres, transformation); + } +}; + void pybind_registration_classes(py::module &m) { // cupoch.registration.ICPConvergenceCriteria py::class_ convergence_criteria( @@ -113,7 +127,8 @@ void pybind_registration_classes(py::module &m) { "Compute RMSE between source and target points cloud given " "correspondences."); te.def("compute_transformation", - ®istration::TransformationEstimation::ComputeTransformation, + py::overload_cast( + ®istration::TransformationEstimation::ComputeTransformation, py::const_), "source"_a, "target"_a, "corres"_a, "Compute transformation from source to target point cloud given " "correspondences."); @@ -214,6 +229,130 @@ void pybind_registration_classes(py::module &m) { "TransformationEstimationForGeneralizedICP"); }); + // cupoch.registration.CorrespondenceChecker + py::class_> + cc(m, "CorrespondenceChecker", + "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."); + cc.def("Check", ®istration::CorrespondenceChecker::Check, "source"_a, "target"_a, + "corres"_a, "transformation"_a, + "Function to check if two points can be aligned. The two input " + "point clouds must have exact the same number of points."); + cc.def_readwrite( + "require_pointcloud_alignment_", + ®istration::CorrespondenceChecker::require_pointcloud_alignment_, + "Some checkers do not require point clouds to be aligned, e.g., " + "the edge length checker. Some checkers do, e.g., the distance " + "checker."); + docstring::ClassMethodDocInject( + m, "CorrespondenceChecker", "Check", + {{"source", "Source point cloud."}, + {"target", "Target point cloud."}, + {"corres", + "Correspondence set between source and target point cloud."}, + {"transformation", "The estimated transformation (inplace)."}}); + + // cupoch.registration.CorrespondenceCheckerBasedOnEdgeLength: + // CorrespondenceChecker + py::class_, + registration::CorrespondenceChecker> + cc_el(m, "CorrespondenceCheckerBasedOnEdgeLength", + "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)"); + py::detail::bind_copy_functions( + cc_el); + cc_el.def(py::init([](float similarity_threshold) { + return new registration::CorrespondenceCheckerBasedOnEdgeLength( + similarity_threshold); + }), + "similarity_threshold"_a = 0.9) + .def("__repr__", + [](const registration::CorrespondenceCheckerBasedOnEdgeLength &c) { + return fmt::format( + "" + "CorrespondenceCheckerBasedOnEdgeLength " + "with similarity_threshold={:f}", + c.similarity_threshold_); + }) + .def_readwrite( + "similarity_threshold", + ®istration::CorrespondenceCheckerBasedOnEdgeLength:: + similarity_threshold_, + R"(float value between 0 (loose) and 1 (strict): For the +check to be true, + +:math:`||\text{edge}_{\text{source}}|| > \text{similarity_threshold} \times ||\text{edge}_{\text{target}}||` and + +:math:`||\text{edge}_{\text{target}}|| > \text{similarity_threshold} \times ||\text{edge}_{\text{source}}||` + +must hold true for all edges.)"); + + // cupoch.registration.CorrespondenceCheckerBasedOnDistance: + // CorrespondenceChecker + py::class_, + registration::CorrespondenceChecker> + cc_d(m, "CorrespondenceCheckerBasedOnDistance", + "Class to check if aligned point clouds are close (less than " + "specified threshold)."); + py::detail::bind_copy_functions(cc_d); + cc_d.def(py::init([](float distance_threshold) { + return new registration::CorrespondenceCheckerBasedOnDistance( + distance_threshold); + }), + "distance_threshold"_a) + .def("__repr__", + [](const registration::CorrespondenceCheckerBasedOnDistance &c) { + return fmt::format( + "" + "CorrespondenceCheckerBasedOnDistance with " + "distance_threshold={:f}", + c.distance_threshold_); + }) + .def_readwrite( + "distance_threshold", + ®istration::CorrespondenceCheckerBasedOnDistance::distance_threshold_, + "Distance threshold for the check."); + + // cupoch.registration.CorrespondenceCheckerBasedOnNormal: + // CorrespondenceChecker + py::class_, + registration::CorrespondenceChecker> + cc_n(m, "CorrespondenceCheckerBasedOnNormal", + "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."); + py::detail::bind_copy_functions(cc_n); + cc_n.def(py::init([](float normal_angle_threshold) { + return new registration::CorrespondenceCheckerBasedOnNormal( + normal_angle_threshold); + }), + "normal_angle_threshold"_a) + .def("__repr__", + [](const registration::CorrespondenceCheckerBasedOnNormal &c) { + return fmt::format( + "" + "CorrespondenceCheckerBasedOnNormal with " + "normal_threshold={:f}", + c.normal_angle_threshold_); + }) + .def_readwrite("normal_angle_threshold", + ®istration::CorrespondenceCheckerBasedOnNormal:: + normal_angle_threshold_, + "Radian value for angle threshold."); + // cupoch.registration.FastGlobalRegistrationOption: py::class_ fgr_option( m, "FastGlobalRegistrationOption", @@ -390,7 +529,12 @@ void pybind_registration_classes(py::module &m) { // Registration functions have similar arguments, sharing arg docstrings static const std::unordered_map map_shared_argument_docstrings = { - {"checkers", "checkers"}, + {"checkers", + "Vector of Checker class to check if two point " + "clouds can be aligned. One of " + "(``CorrespondenceCheckerBasedOnEdgeLength``, " + "``CorrespondenceCheckerBasedOnDistance``, " + "``CorrespondenceCheckerBasedOnNormal``)"}, {"corres", "Checker class to check if two point clouds can be " "aligned. " @@ -447,6 +591,21 @@ void pybind_registration_methods(py::module &m) { "estimation"_a = registration::TransformationEstimationForGeneralizedICP(), "criteria"_a = registration::ICPConvergenceCriteria()); + m.def("registration_ransac_based_on_correspondence", + ®istration::RegistrationRANSACBasedOnCorrespondence, + py::call_guard(), + "Function for global RANSAC registration based on a set of " + "correspondences", + "source"_a, "target"_a, "corres"_a, "max_correspondence_distance"_a, + "estimation_method"_a = registration::TransformationEstimationPointToPoint(), + "ransac_n"_a = 3, + "checkers"_a = std::vector< + std::reference_wrapper>(), + "criteria"_a = registration::RANSACConvergenceCriteria(100000, 0.999)); + docstring::FunctionDocInject(m, + "registration_ransac_based_on_correspondence", + map_shared_argument_docstrings); + m.def("registration_fast_based_on_feature_matching", ®istration::FastGlobalRegistration<33>, "Function for fast global registration based on feature matching",