Merge pull request #5 from yuecideng/dev/UnifyRansac

Unify Ransac
yuecideng · Mar 9, 2022 · 9a2e4f4 · 9a2e4f4
2 parents beb9cbb + 25ee943
commit 9a2e4f4
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 120 deletions.
diff --git 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
@@ -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<size_t> inliers;
     fit.SetPointCloud(*pcd_down);

diff --git 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)

diff --git 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,94 +544,29 @@ 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
      * @param inlier_indices
      * @return true
      * @return false
      */
-    bool FitModelVanilla(double threshold, Model &model,
-                         std::vector<size_t> &inlier_indices) {
+    bool FitModelParallel(double threshold, Model &model,
+                          std::vector<size_t> &inlier_indices) {
         const size_t num_points = pc_.points_.size();
         std::vector<size_t> 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<size_t> 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<size_t>::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<size_t> &inlier_indices) {
-        std::tuple<double, double> result;
-        Model best_model;
-
-        const size_t num_points = pc_.points_.size();
-        std::vector<size_t> 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<size_t> 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<size_t>;

diff --git a/python/py_common.cpp b/python/py_common.cpp
@@ -10,11 +10,10 @@ namespace common {
 
 std::tuple<Eigen::VectorXd, std::vector<size_t>> 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<size_t> inliers;
     fit.SetPointCloud(*pc);
@@ -29,11 +28,10 @@ std::tuple<Eigen::VectorXd, std::vector<size_t>> FitPlane(
 
 std::tuple<Eigen::VectorXd, std::vector<size_t>> 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<size_t> inliers;
     fit.SetPointCloud(*pc);
@@ -48,11 +46,10 @@ std::tuple<Eigen::VectorXd, std::vector<size_t>> FitSphere(
 
 std::tuple<Eigen::VectorXd, std::vector<size_t>> 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<size_t> inliers;
     fit.SetPointCloud(*pc);
@@ -68,16 +65,13 @@ std::tuple<Eigen::VectorXd, std::vector<size_t>> 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<int, int> shape, int k,