use random sampler in teaser

yuecideng · Apr 19, 2022 · 4c07588 · 4c07588
1 parent d60b0f9
commit 4c07588
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 5 deletions.
diff --git a/app/reconstruction/README.md b/app/reconstruction/README.md
@@ -39,7 +39,7 @@ Create a `json` file with the following elements:
 "max_depth_diff": 0.05,
 "voxel_size": 0.01,
 "integration_voxel_size": 0.008,
-"enable_slac": true
+"enable_slac": false
 ```
 These are the whole parameters of the reconstruction pipeline that can be tuned. If you do not specify part of these parameters, the default value will be used.
 

diff --git a/app/reconstruction/example/config.json b/app/reconstruction/example/config.json
@@ -25,5 +25,5 @@
     "max_depth_diff": 0.05,
     "voxel_size": 0.02,
     "integration_voxel_size": 0.008,
-    "enable_slac": true
+    "enable_slac": false
 }
diff --git a/include/misc3d/utils.h b/include/misc3d/utils.h
@@ -380,6 +380,13 @@ inline void EigenMat4x4ToArray(const Eigen::Matrix<T, 4, 4> &mat,
     }
 }
 
+/**
+ * @brief Convert 16 array to 4x4 Mat
+ *
+ * @tparam T
+ * @param array
+ * @param mat
+ */
 template <typename T>
 inline void ArrayToEigenMat4x4(const std::array<T, 16> &array,
                                Eigen::Matrix<T, 4, 4> &mat) {
@@ -390,4 +397,23 @@ inline void ArrayToEigenMat4x4(const std::array<T, 16> &array,
     }
 }
 
+/**
+ * @brief Get new eigen matrix by indices
+ * 
+ * @param mat 
+ * @param index 
+ * @return Eigen::Matrix3Xd 
+ */
+inline Eigen::Matrix3Xd SelectByIndexEigenMat(const Eigen::Matrix3Xd &mat,
+                                       const std::vector<size_t> &indices) {
+    Eigen::Matrix3Xd new_mat;
+    new_mat.resize(3, indices.size());
+#pragma omp parallel for
+    for (int i = 0; i < indices.size(); i++) {
+        new_mat.col(i) = mat.col(indices[i]);
+    }
+
+    return new_mat;
+}
+
 }  // namespace misc3d
diff --git a/src/pipeline.cpp b/src/pipeline.cpp
@@ -93,7 +93,7 @@ PipelineConfig::PipelineConfig() {
     max_depth_diff_ = 0.05;
     voxel_size_ = 0.01;
     integration_voxel_size_ = 0.005;
-    enable_slac_ = true;
+    enable_slac_ = false;
     make_fragment_param_ = {PipelineConfig::DescriptorType::ORB, 100, 40, 0.2};
     local_refine_method_ = LocalRefineMethod::ColoredICP;
     global_registration_method_ = GlobalRegistrationMethod::TeaserPlusPlus;

diff --git a/src/transform_estimation.cpp b/src/transform_estimation.cpp
@@ -100,10 +100,15 @@ Eigen::Matrix4d TeaserSolver::Solve(const Eigen::Matrix3Xd& src,
     teaser::RobustRegistrationSolver solver(params);
     if (src.cols() > max_num) {
         misc3d::LogWarning(
-            "The number of correspondences is too large, use only first "
+            "The number of correspondences is too large, random selecting "
             "{} correspondences instead.",
             max_num);
-        solver.solve(src.block<3, max_num>(0, 0), dst.block<3, max_num>(0, 0));
+        const size_t num = src.cols();
+        RandomSampler<size_t> sampler(num);
+        const std::vector<size_t> indices =
+            sampler.SampleWithoutDuplicate(max_num);
+        solver.solve(SelectByIndexEigenMat(src, indices),
+                     SelectByIndexEigenMat(dst, indices));
     } else {
         solver.solve(src, dst);
     }