From b724e8fa36b93a6c3b10eeb63e48c2003d3856a6 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 9 Mar 2022 22:06:33 +0800 Subject: [PATCH 1/2] unify ransac fitmodel --- examples/python/test_ransac_and_edge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/python/test_ransac_and_edge.py b/examples/python/test_ransac_and_edge.py index 01634e9..c947449 100644 --- a/examples/python/test_ransac_and_edge.py +++ b/examples/python/test_ransac_and_edge.py @@ -27,7 +27,7 @@ print('Point size after sampling', pcd) t0 = time.time() -w, index = m3d.common.fit_plane(pcd, 0.01, 400, enable_parallel=True) +w, index = m3d.common.fit_plane(pcd, 0.01, 1000) print('Plan fitting time: %.3f' % (time.time() - t0)) plane = pcd.select_by_index(index) From 25ee94340131927c9407d45d1c18cb519763d0b1 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 9 Mar 2022 22:06:42 +0800 Subject: [PATCH 2/2] unify ransac fitmodel --- README.md | 8 +- examples/cpp/test_ransac_and_edge.cpp | 4 +- include/misc3d/common/ransac.h | 126 ++++++-------------------- python/py_common.cpp | 18 ++-- 4 files changed, 37 insertions(+), 119 deletions(-) diff --git a/README.md b/README.md index b30121e..0d6e0e9 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ This library aims at providing some useful 3d processing algorithms which Open3D Core modules: - `common`: 1. Normals estimaiton from PointMap - 2. Ransac for primitives fitting, including plane, sphere and cylinder, and support parallel computing. + 2. Ransac for primitives fitting, including plane, sphere and cylinder, with parallel computing supported. 3. K nearest neighbors search based on [annoy](https://github.com/spotify/annoy). It has the similar API as `open3d.geometry.KDTreeFlann` class (the radius search is not supported). - `preprocessing`: 1. Farthest point sampling @@ -63,9 +63,9 @@ import misc3d as m3d m3d.common.estimate_normals(pcd, (848, 480), 3) # ransac for primitives fitting -w, index = m3d.common.fit_plane(pcd, 0.01, 100, enable_parallel=False) -w, index = m3d.common.fit_sphere(pcd, 0.01, 100, enable_parallel=False) -w, index = m3d.common.fit_cylinder(pcd, 0.01, 100, enable_parallel=False) +w, index = m3d.common.fit_plane(pcd, 0.01, 100) +w, index = m3d.common.fit_sphere(pcd, 0.01, 100) +w, index = m3d.common.fit_cylinder(pcd, 0.01, 100) # farthest point sampling indices = m3d.preprocessing.farthest_point_sampling(pcd, 1000) diff --git a/examples/cpp/test_ransac_and_edge.cpp b/examples/cpp/test_ransac_and_edge.cpp index 4935492..0e0de1d 100644 --- a/examples/cpp/test_ransac_and_edge.cpp +++ b/examples/cpp/test_ransac_and_edge.cpp @@ -34,9 +34,7 @@ int main(int argc, char *argv[]) { misc3d::Timer timer; timer.Start(); misc3d::common::RANSACPlane fit; - fit.SetMaxIteration(400); - fit.SetProbability(0.99); - fit.SetParallel(true); + fit.SetMaxIteration(1000); misc3d::common::Plane plane; std::vector inliers; fit.SetPointCloud(*pcd_down); diff --git a/include/misc3d/common/ransac.h b/include/misc3d/common/ransac.h index 3117e6f..515a1e4 100644 --- a/include/misc3d/common/ransac.h +++ b/include/misc3d/common/ransac.h @@ -459,8 +459,7 @@ class RANSAC { : fitness_(0) , inlier_rmse_(0) , max_iteration_(1000) - , probability_(0.99) - , enable_parallel_(false) {} + , probability_(0.9999) {} /** * @brief Set Point Cloud to be used for RANSAC @@ -490,13 +489,6 @@ class RANSAC { */ void SetMaxIteration(size_t num) { max_iteration_ = num; } - /** - * @brief enable parallel ransac fitting - * - * @param flag - */ - void SetParallel(bool flag) { enable_parallel_ = flag; } - /** * @brief fit model with given parameters * @@ -515,11 +507,7 @@ class RANSAC { return false; } - if (enable_parallel_) { - return FitModelParallel(threshold, model, inlier_indices); - } else { - return FitModelVanilla(threshold, model, inlier_indices); - } + return FitModelParallel(threshold, model, inlier_indices); } private: @@ -556,8 +544,8 @@ class RANSAC { } /** - * @brief vanilla ransac fitting method, the iteration number is varying - * with inlier number in each iteration + * @brief Ransac fitting method, the iteration number is varying + * with inlier number in each iteration using multithreading. * * @param threshold * @param model @@ -565,85 +553,20 @@ class RANSAC { * @return true * @return false */ - bool FitModelVanilla(double threshold, Model &model, - std::vector &inlier_indices) { + bool FitModelParallel(double threshold, Model &model, + std::vector &inlier_indices) { const size_t num_points = pc_.points_.size(); std::vector indices_list(num_points); std::iota(std::begin(indices_list), std::end(indices_list), 0); Model best_model; size_t count = 0; - size_t current_iteration = max_iteration_; - for (size_t i = 0; i < current_iteration; ++i) { - const std::vector sample_indices = - sampler_(indices_list, estimator_.minimal_sample_); - const auto sample = pc_.SelectByIndex(sample_indices); - - bool ret; - ret = estimator_.MinimalFit(*sample, model); - - if (!ret) { + size_t current_iteration = std::numeric_limits::max(); +#pragma omp parallel for schedule(static) + for (size_t i = 0; i < max_iteration_; ++i) { + if (count > current_iteration) { continue; } - - const auto result = EvaluateModel(pc_.points_, threshold, model); - double fitness = std::get<0>(result); - double inlier_rmse = std::get<1>(result); - - // update model if satisfy both fitness and rmse check - if (fitness > fitness_ || - (fitness == fitness_ && inlier_rmse < inlier_rmse_)) { - fitness_ = fitness; - inlier_rmse_ = inlier_rmse; - best_model = model; - const size_t tmp = - log(1 - probability_) / - log(1 - pow(fitness, estimator_.minimal_sample_)); - if (tmp < 0) { - current_iteration = max_iteration_; - } else { - current_iteration = std::min(tmp, max_iteration_); - } - } - - // break the loop if count larger than max iteration - if (current_iteration > max_iteration_) { - break; - } - count++; - } - - misc3d::LogInfo( - "[vanilla ransac] find best model with {}% inliers and run {} " - "iterations", - fitness_ * 100, count); - - const bool ret = RefineModel(threshold, best_model, inlier_indices); - model = best_model; - return ret; - } - - /** - * @brief parallel ransac fitting method, the iteration number is fixed and - * use OpenMP to speed up. Usually used if you want more accurate result. - * - * @param threshold - * @param model - * @param inlier_indices - * @return true - * @return false - */ - bool FitModelParallel(double threshold, Model &model, - std::vector &inlier_indices) { - std::tuple result; - Model best_model; - - const size_t num_points = pc_.points_.size(); - std::vector indices_list(num_points); - std::iota(std::begin(indices_list), std::end(indices_list), 0); - -#pragma omp parallel for shared(indices_list) - for (size_t i = 0; i < max_iteration_; ++i) { const std::vector sample_indices = sampler_(indices_list, estimator_.minimal_sample_); const auto sample = pc_.SelectByIndex(sample_indices); @@ -656,30 +579,34 @@ class RANSAC { continue; } - const auto result_current = - EvaluateModel(pc_.points_, threshold, model_trial); - const double &fitness = std::get<0>(result_current); - const double &inlier_rmse = std::get<1>(result_current); + const auto result = EvaluateModel(pc_.points_, threshold, model_trial); + double fitness = std::get<0>(result); + double inlier_rmse = std::get<1>(result); #pragma omp critical { - const double &fitness_tmp = std::get<0>(result); - const double &inlier_rmse_tmp = std::get<1>(result); - if (fitness > fitness_tmp || - (fitness == fitness_tmp && inlier_rmse < inlier_rmse_tmp)) { - result = result_current; + // update model if satisfy both fitness and rmse check + if (fitness > fitness_ || + (fitness == fitness_ && inlier_rmse < inlier_rmse_)) { + fitness_ = fitness; + inlier_rmse_ = inlier_rmse; best_model = model_trial; + + const size_t tmp = + log(1 - probability_) / + log(1 - pow(fitness, estimator_.minimal_sample_)); + current_iteration = std::min(tmp, max_iteration_); } + count++; } } misc3d::LogInfo( - "[parallel ransac] find best model with {}% inliers and run {} " + "Find best model with {}% inliers and run {} " "iterations", - std::get<0>(result) * 100, max_iteration_); + fitness_ * 100, count); const bool ret = RefineModel(threshold, best_model, inlier_indices); model = best_model; - return ret; } @@ -722,7 +649,6 @@ class RANSAC { double inlier_rmse_; Sampler sampler_; ModelEstimator estimator_; - bool enable_parallel_; }; using RandomIndexSampler = RandomSampler; diff --git a/python/py_common.cpp b/python/py_common.cpp index f573047..2cc422a 100644 --- a/python/py_common.cpp +++ b/python/py_common.cpp @@ -10,11 +10,10 @@ namespace common { std::tuple> FitPlane( const PointCloudPtr &pc, double threshold, size_t max_iteration, - double probability, bool enable_parallel) { + double probability) { RANSACPlane fit; fit.SetMaxIteration(max_iteration); fit.SetProbability(probability); - fit.SetParallel(enable_parallel); Plane plane; std::vector inliers; fit.SetPointCloud(*pc); @@ -29,11 +28,10 @@ std::tuple> FitPlane( std::tuple> FitSphere( const PointCloudPtr &pc, double threshold, size_t max_iteration, - double probability, bool enable_parallel) { + double probability) { RANSACShpere fit; fit.SetMaxIteration(max_iteration); fit.SetProbability(probability); - fit.SetParallel(enable_parallel); Sphere sphere; std::vector inliers; fit.SetPointCloud(*pc); @@ -48,11 +46,10 @@ std::tuple> FitSphere( std::tuple> FitCylinder( const PointCloudPtr &pc, double threshold, size_t max_iteration, - double probability, bool enable_parallel) { + double probability) { RANSACShpere fit; fit.SetMaxIteration(max_iteration); fit.SetProbability(probability); - fit.SetParallel(enable_parallel); Sphere sphere; std::vector inliers; fit.SetPointCloud(*pc); @@ -68,16 +65,13 @@ std::tuple> FitCylinder( void pybind_common(py::module &m) { m.def("fit_plane", &FitPlane, "Fit a plane from point clouds", py::arg("pc"), py::arg("threshold") = 0.01, - py::arg("max_iteration") = 1000, py::arg("probability") = 0.99, - py::arg("enable_parallel") = false); + py::arg("max_iteration") = 1000, py::arg("probability") = 0.9999); m.def("fit_sphere", &FitSphere, "Fit a sphere from point clouds", py::arg("pc"), py::arg("threshold") = 0.01, - py::arg("max_iteration") = 1000, py::arg("probability") = 0.99, - py::arg("enable_parallel") = false); + py::arg("max_iteration") = 1000, py::arg("probability") = 0.9999); m.def("fit_cylinder", &FitCylinder, "Fit a cylinder from point clouds", py::arg("pc"), py::arg("threshold") = 0.01, - py::arg("max_iteration") = 1000, py::arg("probability") = 0.99, - py::arg("enable_parallel") = false); + py::arg("max_iteration") = 1000, py::arg("probability") = 0.9999); m.def( "estimate_normals", [](const PointCloudPtr &pc, const std::tuple shape, int k,