Merge pull request #3 from doggydoggy0101/feature/outlier-rejection

✨ add outlier rejection
doggydoggy0101 · Jan 27, 2025 · 4a960e9 · 4a960e9
2 parents d2e8718 + c073b62
commit 4a960e9
Show file tree

Hide file tree

Showing 10 changed files with 536 additions and 434 deletions.
diff --git a/examples/cpp/registration_example.cpp b/examples/cpp/registration_example.cpp
@@ -10,13 +10,9 @@
 #include <tuple>
 #include <vector>
 
-// #include "registration/fracgm.h"
-#include "registration/mcis.h"
-// #include "registration/qgm.h"
-
+#include "registration/mcis.h"  // maximum clique inlier selection
 #include "registration/solver.h"
-
-#define ENABLE_MAX_CLIQUE_INLIER_SELECTION
+#include "registration/tuple.h"  // tuple test
 
 using PointCloud = Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor>;
 
@@ -69,10 +65,28 @@ std::tuple<PointCloud, PointCloud, Eigen::Matrix<double, 4, 4, Eigen::RowMajor>>
   return std::make_tuple(src, dst, gt);
 }
 
-std::tuple<PointCloud, PointCloud> perform_max_clique_inlier_selection(const PointCloud &pc1, const PointCloud &pc2,
-                                                                       double noise_bound, double pmc_timeout,
-                                                                       int pmc_n_threads) {
-  auto indices = registration::mcis::inlier_selection(pc1, pc2, noise_bound, pmc_timeout, pmc_n_threads);
+std::tuple<PointCloud, PointCloud> perform_mcis(const PointCloud &pc1, const PointCloud &pc2, double noise_bound,
+                                                double pmc_timeout, int pmc_n_threads) {
+  auto indices = registration::outlier_rejection::maximum_clique_inlier_selection(pc1, pc2, noise_bound, pmc_timeout,
+                                                                                  pmc_n_threads);
+
+  if (indices.empty()) return std::make_tuple(pc1, pc2);
+
+  PointCloud inlier_pc1(indices.size(), 3);
+  PointCloud inlier_pc2(indices.size(), 3);
+
+  for (size_t idx = 0; idx < indices.size(); idx++) {
+    auto index = indices[idx];
+    inlier_pc1.row(idx) = pc1.row(index);
+    inlier_pc2.row(idx) = pc2.row(index);
+  }
+
+  return std::make_tuple(inlier_pc1, inlier_pc2);
+}
+
+std::tuple<PointCloud, PointCloud> perform_tuple(const PointCloud &pc1, const PointCloud &pc2, double tuple_scale,
+                                                 int max_tuple_count) {
+  auto indices = registration::outlier_rejection::tuple_test(pc1, pc2, tuple_scale, max_tuple_count);
 
   if (indices.empty()) return std::make_tuple(pc1, pc2);
 
@@ -94,21 +108,31 @@ int main() {
   double c = 1.0;
   double noise_bound = 0.1;
 
+  // outlier rejection method
+  std::string method = "mcis";  // "mcis" or "tuple"
+  // mcis
   double pmc_timeout = 3600.0;
   int pmc_n_threads = 4;
+  // tuple
+  double tuple_scale = 0.95;
+  int max_tuple_count = 1000;
 
   auto [src_reg, dst_reg, gt_reg] = get_toy_data();
 
-#ifdef ENABLE_MAX_CLIQUE_INLIER_SELECTION
-  auto [inlier_src_reg, inlier_dst_reg] =
-      perform_max_clique_inlier_selection(src_reg, dst_reg, noise_bound, pmc_timeout, pmc_n_threads);
+  PointCloud inlier_src_reg, inlier_dst_reg;
+
+  if (method == "mcis") {
+    std::cout << "Debug" << "\n\n";
+    std::tie(inlier_src_reg, inlier_dst_reg) = perform_mcis(src_reg, dst_reg, noise_bound, pmc_timeout, pmc_n_threads);
+  } else if (method == "tuple") {
+    std::tie(inlier_src_reg, inlier_dst_reg) = perform_tuple(src_reg, dst_reg, tuple_scale, max_tuple_count);
+  } else {
+    inlier_src_reg = src_reg;
+    inlier_dst_reg = dst_reg;
+  }
 
   auto fracgm_reg = registration::FracGM(max_iteration, tol, c).solve(inlier_src_reg, inlier_dst_reg, noise_bound);
   auto qgm_reg = registration::QGM(max_iteration, tol, c).solve(inlier_src_reg, inlier_dst_reg, noise_bound);
-#else
-  auto fracgm_reg = registration::FracGM(max_iteration, tol, c).solve(src_reg, dst_reg, noise_bound);
-  auto qgm_reg = registration::QGM(max_iteration, tol, c).solve(src_reg, dst_reg, noise_bound);
-#endif
 
   std::cout << "Ground Truth:" << "\n" << gt_reg << "\n\n";
   std::cout << "FracGM:" << "\n" << fracgm_reg << "\n\n";

diff --git a/examples/python/registration_example.py b/examples/python/registration_example.py
@@ -24,30 +24,43 @@ def get_toy_data():
     return src, dst, gt
 
 
-def perform_max_clique_inlier_selection(
-    src, dst, noise_bound, pmc_timeout, pmc_n_threads
-):
-    indices = registration_python.mcis.inlier_selection(
+def perform_mcis(src, dst, noise_bound, pmc_timeout, pmc_n_threads):
+    indices = registration_python.outlier_rejection.maximum_clique_inlier_selection(
         src, dst, noise_bound, pmc_timeout, pmc_n_threads
     )
     return np.take(src, indices, axis=0), np.take(dst, indices, axis=0)
 
 
+def perform_tuple(src, dst, tuple_scale, max_tuple_count):
+    indices = registration_python.outlier_rejection.tuple_test(
+        src, dst, tuple_scale, max_tuple_count
+    )
+    return np.take(src, indices, axis=0), np.take(dst, indices, axis=0)
+
+
 def main():
     max_iteration = 100
     tol = 1e-6
     c = 1.0
     noise_bound = 0.1
 
-    ENABLE_MAX_CLIQUE_INLIER_SELECTION = True
+    # outlier rejection
+    method = "mcis"  # "mcis" or "tuple"
+    # mcis
     pmc_timeout = 3600.0
     pmc_n_threads = 4
+    # tuple
+    tuple_scale = 0.95
+    max_tuple_count = 1000
+
     src_reg, dst_reg, gt_reg = get_toy_data()
 
-    if ENABLE_MAX_CLIQUE_INLIER_SELECTION:
-        src_reg, dst_reg = perform_max_clique_inlier_selection(
+    if method == "mcis":
+        src_reg, dst_reg = perform_mcis(
             src_reg, dst_reg, noise_bound, pmc_timeout, pmc_n_threads
         )
+    elif method == "tuple":
+        src_reg, dst_reg = perform_tuple(src_reg, dst_reg, tuple_scale, max_tuple_count)
 
     fracgm_reg = registration_python.FracGM(max_iteration, tol, c).solve(
         src_reg, dst_reg, noise_bound

diff --git a/python/registration_python/python_wrapper.cpp b/python/registration_python/python_wrapper.cpp
@@ -8,6 +8,7 @@
 #include <pybind11/stl.h>
 #include <registration/mcis.h>
 #include <registration/solver.h>
+#include <registration/tuple.h>
 
 namespace py = pybind11;
 
@@ -20,6 +21,8 @@ PYBIND11_MODULE(registration_python, module) {
       .def(py::init<const size_t, const double, const double>())
       .def("solve", &registration::QGM::solve);
 
-  py::module_ mcis = module.def_submodule("mcis");
-  mcis.def("inlier_selection", &registration::mcis::inlier_selection);
+  py::module_ outlier_rejection = module.def_submodule("outlier_rejection");
+  outlier_rejection
+      .def("maximum_clique_inlier_selection", &registration::outlier_rejection::maximum_clique_inlier_selection)
+      .def("tuple_test", &registration::outlier_rejection::tuple_test);
 }
diff --git a/registration/CMakeLists.txt b/registration/CMakeLists.txt
@@ -1,7 +1,7 @@
 project(registration_src)
 include(GNUInstallDirs)
 
-add_library(registration SHARED src/solver.cpp src/utils.cpp src/graph.cpp src/mcis.cpp)
+add_library(registration SHARED src/solver.cpp src/utils.cpp src/mcis.cpp src/tuple.cpp)
 target_include_directories(registration PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/include>
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>