Minor modifications for better usuability (#184)

* Set const in the inputs of the matcher.h and matcher.cc

* Add Quatro mode in a Python wrapper

* Make normal estimation based on OMP as well.

* Add API with normal vectors for easy integration with the `xyznormal` mode of ROBIN

* Clean unused variables

* Fix error in the example (change == to =)

* Remove weird semicolon to cover #99

* Remove [[nodiscard]] to cover #140

* Add 'InsertNewlineAtEOF' in the .clang-format

* Add newline at the end of file using .clang-format

* (Minor) Clean codes, e.g., managing double space, redundant line break, no-space, etc, via .clang-format

.clang-format

@@ -1,7 +1,8 @@
 ---
-Language:        Cpp
+Language: Cpp
 ColumnLimit: 100
 DerivePointerAlignment: false
 PointerAlignment: Left
 SortIncludes: false
+InsertNewlineAtEOF: true
 ...

examples/teaser_cpp_fpfh/quatro_cpp_fpfh.cc

@@ -16,8 +16,8 @@ inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) {
   return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0)));
 }
 
-inline void calcErrors(const Eigen::Matrix4d& T, const Eigen::Matrix3d est_rot, const Eigen::Vector3d est_ts,
-                double &rot_error, double& ts_error) {
+inline void calcErrors(const Eigen::Matrix4d& T, const Eigen::Matrix3d est_rot,
+                       const Eigen::Vector3d est_ts, double& rot_error, double& ts_error) {
   rot_error = getAngularError(T.topLeftCorner(3, 3), est_rot);
   ts_error = (T.topRightCorner(3, 1) - est_ts).norm();
 }
@@ -32,11 +32,11 @@ inline void getParams(const double noise_bound, const std::string reg_type,
   if (reg_type == "Quatro") {
     params.rotation_estimation_algorithm =
         teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO;
-    params.inlier_selection_mode == teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU;
-  } else if  (reg_type == "TEASER") {
+    params.inlier_selection_mode = teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU;
+  } else if (reg_type == "TEASER") {
     params.rotation_estimation_algorithm =
         teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
-    params.inlier_selection_mode == teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT;
+    params.inlier_selection_mode = teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_EXACT;
   } else {
     throw std::invalid_argument("Not implemented!");
   }

examples/teaser_cpp_ply/teaser_cpp_ply.cc

@@ -117,4 +117,4 @@ int main() {
             << std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() /
                    1000000.0
             << std::endl;
-}
+}

examples/teaser_python_3dsmooth/teaser_python_3dsmooth.py

@@ -109,7 +109,7 @@ def visualize_correspondences(
     frag2_temp.translate(translate)
 
     # estimate normals
-    vis_list = [target, source, frag1_temp, frag2_temp];
+    vis_list = [target, source, frag1_temp, frag2_temp]
     for ii in vis_list:
         ii.estimate_normals()
     vis_list.extend([*inlier_line_mesh_geoms, *outlier_line_mesh_geoms])

python/teaserpp_python/teaserpp_python.cc

@@ -80,7 +80,8 @@ PYBIND11_MODULE(teaserpp_python, m) {
   py::enum_<teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM>(
       solver, "ROTATION_ESTIMATION_ALGORITHM")
       .value("GNC_TLS", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS)
-      .value("FGR", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR);
+      .value("FGR", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR)
+      .value("QUATRO", teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO);
 
   // Python bound for teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION
   py::enum_<teaser::RobustRegistrationSolver::INLIER_GRAPH_FORMULATION>(solver,
@@ -125,11 +126,19 @@ PYBIND11_MODULE(teaserpp_python, m) {
       .def("__repr__", [](const teaser::RobustRegistrationSolver::Params& a) {
         std::ostringstream print_string;
 
-        std::string rot_alg =
-            a.rotation_estimation_algorithm ==
-                    teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR
-                ? "FGR"
-                : "GNC-TLS";
+        std::string rot_alg;
+        if (a.rotation_estimation_algorithm ==
+            teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR) {
+          rot_alg = "FGR";
+        }
+        if (a.rotation_estimation_algorithm ==
+            teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS) {
+          rot_alg = "GNC_TLS";
+        }
+        if (a.rotation_estimation_algorithm ==
+            teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO) {
+          rot_alg = "QUATRO";
+        }
 
         std::string inlier_selection_alg;
         if (a.inlier_selection_mode ==
@@ -205,4 +214,4 @@ PYBIND11_MODULE(teaserpp_python, m) {
       .def_readwrite("gamma_tau", &teaser::DRSCertifier::Params::gamma_tau)
       .def_readwrite("eig_decomposition_solver",
                      &teaser::DRSCertifier::Params::eig_decomposition_solver);
-}
+}

teaser/include/teaser/certification.h

@@ -52,15 +52,14 @@ class AbstractRotationCertifier {
  */
 class DRSCertifier : public AbstractRotationCertifier {
 public:
-
   /**
    * Solver for eigendecomposition solver / spectral decomposition.
    *
    * @brief For most cases, the default solvers in Eigen should be used.
    * For extremely large matrices, it may make sense to use Spectra instead.
    */
   enum class EIG_SOLVER_TYPE {
-    EIGEN = 0, ///< Use solvers in the Eigen library
+    EIGEN = 0,   ///< Use solvers in the Eigen library
     SPECTRA = 1, ///< Use solvers in the Spectra library
   };
 
@@ -237,4 +236,4 @@ class DRSCertifier : public AbstractRotationCertifier {
   Params params_;
 };
 
-} // namespace teaser
+} // namespace teaser

teaser/include/teaser/fpfh.h

@@ -11,6 +11,7 @@
 #include <boost/smart_ptr/shared_ptr.hpp>
 #include <pcl/features/fpfh.h>
 #include <pcl/features/fpfh_omp.h>
+#include <pcl/features/normal_3d_omp.h>
 
 #include "teaser/geometry.h"
 
@@ -21,10 +22,11 @@ using FPFHCloudPtr = pcl::PointCloud<pcl::FPFHSignature33>::Ptr;
 
 class FPFHEstimation {
 public:
-  FPFHEstimation()
-      : fpfh_estimation_(
-            new pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>){};
-
+  FPFHEstimation() {
+    fpfh_estimation_.reset(
+        new pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>());
+    normals_.reset(new pcl::PointCloud<pcl::Normal>());
+  }
   /**
    * Compute FPFH features.
    *
@@ -50,6 +52,8 @@ class FPFHEstimation {
   // pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfh_estimation_;
   pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfh_estimation_;
 
+  pcl::PointCloud<pcl::Normal>::Ptr normals_;
+
   /**
    * Wrapper function for the corresponding PCL function.
    * @param output_cloud
@@ -78,6 +82,12 @@ class FPFHEstimation {
    * Wrapper function for the corresponding PCL function.
    */
   void setRadiusSearch(double);
+
+  /**
+   * Return the normal vectors of the input cloud that are used in the calculation of FPFH
+   * @return
+   */
+  pcl::PointCloud<pcl::Normal> getNormals();
 };
 
 } // namespace teaser

teaser/include/teaser/geometry.h

@@ -69,5 +69,4 @@ class PointCloud {
   std::vector<PointXYZ> points_;
 };
 
-
 } // namespace teaser

teaser/include/teaser/graph.h

@@ -128,34 +128,34 @@ class Graph {
    * Get the number of vertices
    * @return total number of vertices
    */
-  [[nodiscard]] int numVertices() const { return adj_list_.size(); }
+  int numVertices() const { return adj_list_.size(); }
 
   /**
    * Get the number of edges
    * @return total number of edges
    */
-  [[nodiscard]] int numEdges() const { return num_edges_; }
+  int numEdges() const { return num_edges_; }
 
   /**
    * Get edges originated from a specific vertex
    * @param [in] id
    * @return an unordered set of edges
    */
-  [[nodiscard]] const std::vector<int>& getEdges(int id) const { return adj_list_[id]; }
+  const std::vector<int>& getEdges(int id) const { return adj_list_[id]; }
 
   /**
    * Get all vertices
    * @return a vector of all vertices
    */
-  [[nodiscard]] std::vector<int> getVertices() const {
+  std::vector<int> getVertices() const {
     std::vector<int> v;
     for (int i = 0; i < adj_list_.size(); ++i) {
       v.push_back(i);
     }
     return v;
   }
 
-  [[nodiscard]] Eigen::MatrixXi getAdjMatrix() const {
+  Eigen::MatrixXi getAdjMatrix() const {
     const int num_v = numVertices();
     Eigen::MatrixXi adj_matrix(num_v, num_v);
     for (size_t i = 0; i < num_v; ++i) {
@@ -171,7 +171,7 @@ class Graph {
     return adj_matrix;
   }
 
-  [[nodiscard]] std::vector<std::vector<int>> getAdjList() const { return adj_list_; }
+  std::vector<std::vector<int>> getAdjList() const { return adj_list_; }
 
   /**
    * Preallocate spaces for vertices
@@ -258,7 +258,7 @@ class MaxCliqueSolver {
     /**
      * Number of threads to use for the solver
      */
-     int num_threads = 1;
+    int num_threads = 1;
   };
 
   MaxCliqueSolver() = default;

teaser/include/teaser/linalg.h

@@ -98,4 +98,4 @@ void getNearestPSD(const Eigen::Matrix<NumT, Eigen::Dynamic, Eigen::Dynamic>& A,
   *nearestPSD = Ve * De_positive * Ve.transpose();
 }
 
-} // namespace teaser
+} // namespace teaser

teaser/include/teaser/macros.h

@@ -65,5 +65,5 @@
   auto t_end_##s = clock_##s.now();                                                                \
   std::chrono::duration<double, std::milli> diff_dur_##s = t_end_##s - t_start_##s;                \
   double diff_##s = diff_dur_##s.count();
-#define TEASER_DEBUG_GET_TIMING(s)(double)(diff_##s / 1000.0)
-#endif
+#define TEASER_DEBUG_GET_TIMING(s) (double)(diff_##s / 1000.0)
+#endif

teaser/include/teaser/matcher.h

@@ -36,11 +36,11 @@ class Matcher {
    * @param use_tuple_test
    * @return
    */
-  std::vector<std::pair<int, int>>
-  calculateCorrespondences(teaser::PointCloud& source_points, teaser::PointCloud& target_points,
-                           teaser::FPFHCloud& source_features, teaser::FPFHCloud& target_features,
-                           bool use_absolute_scale = true, bool use_crosscheck = true,
-                           bool use_tuple_test = true, float tuple_scale = 0);
+  std::vector<std::pair<int, int>> calculateCorrespondences(
+      const teaser::PointCloud& source_points, const teaser::PointCloud& target_points,
+      const teaser::FPFHCloud& source_features, const teaser::FPFHCloud& target_features,
+      bool use_absolute_scale = true, bool use_crosscheck = true, bool use_tuple_test = true,
+      float tuple_scale = 0);
 
 private:
   template <typename T> void buildKDTree(const std::vector<T>& data, KDTree* tree);
@@ -56,7 +56,7 @@ class Matcher {
   std::vector<std::pair<int, int>> corres_;
   std::vector<teaser::PointCloud> pointcloud_;
   std::vector<Feature> features_;
-  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > means_; // for normalization
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> means_; // for normalization
   float global_scale_;
 };
 

teaser/src/certification.cc

@@ -97,7 +97,7 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution,
 
   // this initial guess lives in the affine subspace
   // use 2 separate steps to limit slow evaluation on only the few non-zeros in the sparse matrix
-#if EIGEN_VERSION_AT_LEAST(3,3,0)
+#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
   Eigen::SparseMatrix<double> M_init = Q_bar - mu * J_bar - lambda_bar_init;
 #else
   // fix for this bug in Eigen 3.2: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632
@@ -135,7 +135,7 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution,
     TEASER_DEBUG_INFO_MSG("PSD time: " << TEASER_DEBUG_GET_TIMING(PSD));
 
     // projection to affine space
-#if EIGEN_VERSION_AT_LEAST(3,3,0)
+#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
     temp_W = 2 * M_PSD - M - M_init;
 #else
     // fix for this bug in Eigen 3.2: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632
@@ -147,7 +147,7 @@ teaser::DRSCertifier::certify(const Eigen::Matrix3d& R_solution,
     getOptimalDualProjection(temp_W, theta_prepended, inverse_map, &W_dual);
     TEASER_DEBUG_STOP_TIMING(DualProjection);
     TEASER_DEBUG_INFO_MSG("Dual Projection time: " << TEASER_DEBUG_GET_TIMING(DualProjection));
-#if EIGEN_VERSION_AT_LEAST(3,3,0)
+#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
     M_affine = M_init + W_dual;
 #else
     // fix for this bug in Eigen 3.2: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=632
@@ -604,7 +604,7 @@ void teaser::DRSCertifier::getLinearProjection(
           temp_column.emplace_back(var_i_idx, var_j_idx, entry_val);
           if (var_i_idx == var_j_idx) {
             diag_inserted = true;
-            diag_idx = temp_column.size()-1;
+            diag_idx = temp_column.size() - 1;
           }
         }
       }
@@ -623,7 +623,7 @@ void teaser::DRSCertifier::getLinearProjection(
           temp_column.emplace_back(var_i_idx, var_j_idx, entry_val);
           if (var_i_idx == var_j_idx) {
             diag_inserted = true;
-            diag_idx = temp_column.size()-1;
+            diag_idx = temp_column.size() - 1;
           }
         }
       }

teaser/src/fpfh.cc

@@ -16,7 +16,6 @@ teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures(
     const teaser::PointCloud& input_cloud, double normal_search_radius, double fpfh_search_radius) {
 
   // Intermediate variables
-  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
   teaser::FPFHCloudPtr descriptors(new pcl::PointCloud<pcl::FPFHSignature33>());
   pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
   for (auto& i : input_cloud) {
@@ -25,16 +24,17 @@ teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures(
   }
 
   // Estimate normals
-  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
+  normals_->clear();
+  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normalEstimation;
   normalEstimation.setInputCloud(pcl_input_cloud);
   normalEstimation.setRadiusSearch(normal_search_radius);
   pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
   normalEstimation.setSearchMethod(kdtree);
-  normalEstimation.compute(*normals);
+  normalEstimation.compute(*normals_);
 
   // Estimate FPFH
   setInputCloud(pcl_input_cloud);
-  setInputNormals(normals);
+  setInputNormals(normals_);
   setSearchMethod(kdtree);
   setRadiusSearch(fpfh_search_radius);
   compute(*descriptors);
@@ -59,3 +59,5 @@ void teaser::FPFHEstimation::compute(pcl::PointCloud<pcl::FPFHSignature33>& outp
   fpfh_estimation_->compute(output_cloud);
 }
 void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); }
+
+pcl::PointCloud<pcl::Normal> teaser::FPFHEstimation::getNormals() { return *normals_; }

teaser/src/graph.cc

@@ -74,7 +74,7 @@ vector<int> teaser::MaxCliqueSolver::findMaxClique(teaser::Graph graph) {
     for (int i = 1; i < k_cores->size(); ++i) {
       // Note: k_core has size equals to num vertices + 1
       if ((*k_cores)[i] >= max_core) {
-        C.push_back(i-1);
+        C.push_back(i - 1);
       }
     }
     return C;

teaser/src/matcher.cc

@@ -6,7 +6,6 @@
  * See LICENSE for the license information
  */
 
-
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <flann/flann.hpp>
@@ -20,9 +19,9 @@
 namespace teaser {
 
 std::vector<std::pair<int, int>> Matcher::calculateCorrespondences(
-    teaser::PointCloud& source_points, teaser::PointCloud& target_points,
-    teaser::FPFHCloud& source_features, teaser::FPFHCloud& target_features, bool use_absolute_scale,
-    bool use_crosscheck, bool use_tuple_test, float tuple_scale) {
+    const teaser::PointCloud& source_points, const teaser::PointCloud& target_points,
+    const teaser::FPFHCloud& source_features, const teaser::FPFHCloud& target_features,
+    bool use_absolute_scale, bool use_crosscheck, bool use_tuple_test, float tuple_scale) {
 
   Feature cloud_features;
   pointcloud_.push_back(source_points);
@@ -138,9 +137,8 @@ void Matcher::advancedMatching(bool use_crosscheck, bool use_tuple_test, float t
   KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15));
   buildKDTree(features_[fj], &feature_tree_j);
 
-  std::vector<int> corres_K, corres_K2;
+  std::vector<int> corres_K;
   std::vector<float> dis;
-  std::vector<int> ind;
 
   std::vector<std::pair<int, int>> corres;
   std::vector<std::pair<int, int>> corres_cross;