diff --git a/teaser/include/teaser/certification.h b/teaser/include/teaser/certification.h index f17fee1..d006fa6 100644 --- a/teaser/include/teaser/certification.h +++ b/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& W, const Eigen::Matrix& theta_prepended, diff --git a/teaser/include/teaser/fpfh.h b/teaser/include/teaser/fpfh.h index 720d8d2..9df00e7 100644 --- a/teaser/include/teaser/fpfh.h +++ b/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 getNormals() { return *normals_; } + private: // pcl::FPFHEstimation::Ptr fpfh_estimation_; pcl::FPFHEstimationOMP::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 getNormals(); }; } // namespace teaser diff --git a/teaser/include/teaser/graph.h b/teaser/include/teaser/graph.h index f6e28cd..e7ab542 100644 --- a/teaser/include/teaser/graph.h +++ b/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>& adj_list) { adj_list_.resize(adj_list.size()); diff --git a/teaser/include/teaser/linalg.h b/teaser/include/teaser/linalg.h index 9053076..f1e8d19 100644 --- a/teaser/include/teaser/linalg.h +++ b/teaser/include/teaser/linalg.h @@ -78,7 +78,7 @@ Eigen::Matrix vectorKron(const Eigen::Matrix diff --git a/teaser/include/teaser/registration.h b/teaser/include/teaser/registration.h index 8aa596b..6cf52bf 100644 --- a/teaser/include/teaser/registration.h +++ b/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 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 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 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. diff --git a/teaser/include/teaser/utils.h b/teaser/include/teaser/utils.h index 3063b9c..9419275 100644 --- a/teaser/include/teaser/utils.h +++ b/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 randomSample(std::vector 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& 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 float calculateDiameter(const Eigen::Matrix& } /** - * 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 diff --git a/teaser/src/fpfh.cc b/teaser/src/fpfh.cc index 59396d3..fa046ed 100644 --- a/teaser/src/fpfh.cc +++ b/teaser/src/fpfh.cc @@ -58,6 +58,5 @@ void teaser::FPFHEstimation::setSearchMethod( void teaser::FPFHEstimation::compute(pcl::PointCloud& output_cloud) { fpfh_estimation_->compute(output_cloud); } -void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); } -pcl::PointCloud teaser::FPFHEstimation::getNormals() { return *normals_; } +void teaser::FPFHEstimation::setRadiusSearch(double r) { fpfh_estimation_->setRadiusSearch(r); }