Revise get functions & (minor) fix typos in the comments (#185)

* Change `getNormals` to be an inline public function

* (minor) Add a line break between functions

* Add `getInputOrderedTranslationInliers` function for visualization purposes

* (minor) Fix typos in the comments

teaser/include/teaser/certification.h

@@ -31,7 +31,7 @@ class AbstractRotationCertifier {
 
   /**
    * Abstract function for certifying rotation estimation results
-   * @param rotation_solution [in] a solution to the rotatoin registration problem
+   * @param rotation_solution [in] a solution to the rotation registration problem
    * @param src [in] Assume dst = R * src
    * @param dst [in] Assume dst = R * src
    * @param theta [in] a binary vector indicating inliers vs. outliers
@@ -185,7 +185,7 @@ class DRSCertifier : public AbstractRotationCertifier {
     (1) off-diagonal blocks must be skew-symmetric
     (2) diagonal blocks must satisfy W_00 = - sum(W_ii)
     (3) W_dual must also satisfy complementary slackness (because M_init satisfies complementary
-   slackness) This projection is optimal in the sense of minimum Frobenious norm
+   slackness) This projection is optimal in the sense of minimum Frobenius norm
    */
   void getOptimalDualProjection(const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& W,
                                 const Eigen::Matrix<double, 1, Eigen::Dynamic>& theta_prepended,

teaser/include/teaser/fpfh.h

@@ -48,6 +48,12 @@ class FPFHEstimation {
     return fpfh_estimation_;
   }
 
+   /**
+   * Return the normal vectors of the input cloud that are used in the calculation of FPFH
+   * @return
+   */
+  inline pcl::PointCloud<pcl::Normal> getNormals() { return *normals_; }
+
 private:
   // pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfh_estimation_;
   pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33>::Ptr fpfh_estimation_;
@@ -82,12 +88,6 @@ 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/graph.h

@@ -35,7 +35,7 @@ class Graph {
    * vertices v1 & v2, we assume that v2 exists in v1's list, and v1 also exists in v2's list. This
    * condition is not enforced. If violated, removeEdge() function might exhibit undefined
    * behaviors.
-   * @param [in] adj_list an map representing an adjacency list
+   * @param [in] adj_list a map representing an adjacency list
    */
   explicit Graph(const std::map<int, std::vector<int>>& adj_list) {
     adj_list_.resize(adj_list.size());

teaser/include/teaser/linalg.h

@@ -78,7 +78,7 @@ Eigen::Matrix<NumT, Eigen::Dynamic, 1> vectorKron(const Eigen::Matrix<NumT, N, 1
  *
  * @tparam NumT numerical type for Eigen matrices (double, float, etc.)
  * @param A [in] input matrix
- * @param nearestPSD [out] output neaest positive semi-definite matrix
+ * @param nearestPSD [out] output nearest positive semi-definite matrix
  * @param eig_threshold [in] optional threshold of determining the smallest eigen values
  */
 template <typename NumT>

teaser/include/teaser/registration.h

@@ -22,7 +22,7 @@
 #include "teaser/geometry.h"
 
 // TODO: might be a good idea to template Eigen::Vector3f and Eigen::VectorXf such that later on we
-// can decide to use doulbe if we want. Double vs float might give nontrivial differences..
+// can decide to use double if we want. Double vs float might give nontrivial differences..
 
 namespace teaser {
 
@@ -283,7 +283,7 @@ class GNCTLSRotationSolver : public GNCRotationSolver {
  * For more information, please see the original paper on FGR:
  * Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in Computer Vision – ECCV 2016,
  * Cham, 2016, vol. 9906, pp. 766–782.
- * Notice that our implementation differ from the paper on the estimation of T matrix. We
+ * Notice that our implementation differs from the paper on the estimation of T matrix. We
  * only estimate rotation, instead of rotation and translation.
  *
  */
@@ -317,7 +317,7 @@ class FastGlobalRegistrationSolver : public GNCRotationSolver {
 /**
  * Use Quatro to solve for pairwise registration problems avoiding degeneracy
  *
- * For more information, please see the original paper on FGR:
+ * For more information, please see the original paper on Quatro:
  * H. Lim et al., "A Single Correspondence Is Enough: Robust Global Registration
  * to Avoid Degeneracy in Urban Environments," in Robotics - ICRA 2022,
  * Accepted. To appear. arXiv:2203.06612 [cs], Mar. 2022.
@@ -714,12 +714,31 @@ class RobustRegistrationSolver {
   }
 
   /**
-   * Return inliers from rotation estimation
+   * Return inliers from translation estimation
    *
-   * @return a vector of indices of measurements deemed as inliers by rotation estimation
+   * @return a vector of indices of measurements deemed as inliers by translation estimation
    */
   inline std::vector<int> getTranslationInliers() { return translation_inliers_; }
 
+  /**
+   * Return input-ordered inliers from translation estimation
+   *
+   * @return a vector of indices of given input correspondences deemed as inliers
+   * by translation estimation.
+   */
+  inline std::vector<int> getInputOrderedTranslationInliers() {
+    if (params_.rotation_estimation_algorithm == ROTATION_ESTIMATION_ALGORITHM::FGR) {
+      throw std::runtime_error(
+          "This function is not supported when using FGR since FGR does not use max clique.");
+    }
+    std::vector<int> translation_inliers;
+    translation_inliers.reserve(translation_inliers_.size());
+    for (const auto& i : translation_inliers_) {
+      translation_inliers.emplace_back(max_clique_[i]);
+    }
+    return translation_inliers;
+  }
+
   /**
    * Return a boolean Eigen row vector indicating whether specific measurements are inliers
    * according to translation measurements.

teaser/include/teaser/utils.h

@@ -22,7 +22,7 @@ namespace utils {
  * randsample()
  * @tparam T A number type
  * @tparam URBG A UniformRandomBitGenerator type
- * @param input A input vector containing the whole population
+ * @param input An input vector containing the whole population
  * @param num_samples Number of samples we want
  * @param g
  * @return
@@ -56,7 +56,7 @@ std::vector<T> randomSample(std::vector<T> input, size_t num_samples, URBG&& g)
 }
 
 /**
- * Remove one row from matrix.
+ * Remove one row from a matrix.
  * Credit to: https://stackoverflow.com/questions/13290395
  * @param matrix an Eigen::Matrix.
  * @param rowToRemove index of row to remove. If >= matrix.rows(), no operation will be taken
@@ -78,7 +78,7 @@ void removeRow(Eigen::Matrix<T, R, C>& matrix, unsigned int rowToRemove) {
 }
 
 /**
- * Remove one column from matrix.
+ * Remove one column from a matrix.
  * Credit to: https://stackoverflow.com/questions/13290395
  * @param matrix
  * @param colToRemove index of col to remove. If >= matrix.cols(), no operation will be taken
@@ -112,7 +112,7 @@ template <class T, int D> float calculateDiameter(const Eigen::Matrix<T, D, Eige
 }
 
 /**
- * Helper function to use svd to estimate rotation.
+ * Helper function to use SVD to estimate rotation.
  * Method described here: http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
  * @param X
  * @param Y
@@ -160,7 +160,7 @@ inline Eigen::Matrix2d svdRot2d(const Eigen::Matrix<double, 2, Eigen::Dynamic>&
 }
 
 /**
- * Use an boolean Eigen matrix to mask a vector
+ * Use a boolean Eigen matrix to mask a vector
  * @param mask a 1-by-N boolean Eigen matrix
  * @param elements vector to be masked
  * @return

teaser/src/fpfh.cc

@@ -58,6 +58,5 @@ void teaser::FPFHEstimation::setSearchMethod(
 void teaser::FPFHEstimation::compute(pcl::PointCloud<pcl::FPFHSignature33>& output_cloud) {
   fpfh_estimation_->compute(output_cloud);
 }
-void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); }
 
-pcl::PointCloud<pcl::Normal> teaser::FPFHEstimation::getNormals() { return *normals_; }
+void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); }