refactor RegistrationPCL

include/small_gicp/ann/traits.hpp

@@ -12,7 +12,7 @@ namespace traits {
 template <typename T>
 struct Traits;
 
-/// @brief Find k-nearest neighbors
+/// @brief Find k-nearest neighbors.
 /// @param tree       Nearest neighbor search (e.g., KdTree)
 /// @param point      Query point
 /// @param k          Number of neighbors
@@ -33,7 +33,7 @@ struct has_nearest_neighbor_search {
   static constexpr bool value = decltype(test((T*)nullptr))::value;
 };
 
-/// @brief Find the nearest neighbor
+/// @brief Find the nearest neighbor.
 /// @param tree       Nearest neighbor search (e.g., KdTree)
 /// @param point      Query point
 /// @param k_index    [out] Index of the nearest neighbor
@@ -44,6 +44,12 @@ size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size
   return Traits<T>::nearest_neighbor_search(tree, point, k_index, k_sq_dist);
 }
 
+/// @brief Find the nearest neighbor. If Traits<T>::nearest_neighbor_search is not defined, fallback to knn_search with k=1.
+/// @param tree       Nearest neighbor search (e.g., KdTree)
+/// @param point      Query point
+/// @param k_index    [out] Index of the nearest neighbor
+/// @param k_sq_dist  [out] Squared distance to the nearest neighbor
+/// @return 1 if a neighbor is found else 0
 template <typename T, std::enable_if_t<!has_nearest_neighbor_search<T>::value, bool> = true>
 size_t nearest_neighbor_search(const T& tree, const Eigen::Vector4d& point, size_t* k_index, double* k_sq_dist) {
   return Traits<T>::knn_search(tree, point, 1, k_index, k_sq_dist);

include/small_gicp/pcl/pcl_registration.hpp

@@ -6,9 +6,11 @@
 #include <pcl/registration/registration.h>
 #include <small_gicp/ann/kdtree_omp.hpp>
 #include <small_gicp/ann/gaussian_voxelmap.hpp>
+#include <small_gicp/registration/registration_result.hpp>
 
 namespace small_gicp {
 
+/// @brief PCL registration interfaces.
 template <typename PointSource, typename PointTarget>
 class RegistrationPCL : public pcl::Registration<PointSource, PointTarget, float> {
 public:
@@ -43,42 +45,70 @@ class RegistrationPCL : public pcl::Registration<PointSource, PointTarget, float
   RegistrationPCL();
   virtual ~RegistrationPCL();
 
-  void setNumThreads(int n) { num_threads_ = n; }
-  void setCorrespondenceRandomness(int k) { k_correspondences_ = k; }
-  void setVoxelResolution(double r) { voxel_resolution_ = r; }
-  void setRotationEpsilon(double eps) { rotation_epsilon_ = eps; }
+  /// @brief Set the number of threads to use.
+  void setNumThreads(int n);
+  /// @brief Set the number of neighbors for covariance estimation.
+  /// @note  This is equivalent to `setNumNeighborsForCovariance`. Just exists for compatibility with pcl::GICP.
+  void setCorrespondenceRandomness(int k);
+  /// @brief Set the number of neighbors for covariance estimation.
+  void setNumNeighborsForCovariance(int k);
+  /// @brief Set the voxel resolution for VGICP.
+  void setVoxelResolution(double r);
+  /// @brief Set rotation epsilon for convergence check.
+  void setRotationEpsilon(double eps);
+  /// @brief Set registration type ("GICP" or "VGICP").
   void setRegistrationType(const std::string& type);
+  /// @brief Set the verbosity flag.
+  void setVerbosity(bool verbose);
 
-  const Eigen::Matrix<double, 6, 6>& getFinalHessian() const { return final_hessian_; }
+  /// @brief Get the final Hessian matrix ([rx, ry, rz, tx, ty, tz]).
+  const Eigen::Matrix<double, 6, 6>& getFinalHessian() const;
 
+  /// @brief Get the detailed registration result.
+  const RegistrationResult& getRegistrationResult() const;
+
+  /// @brief  Set the input source (aligned) point cloud.
   void setInputSource(const PointCloudSourceConstPtr& cloud) override;
+  /// @brief  Set the input target (fixed) point cloud.
   void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
 
+  /// @brief Set source point covariances.
+  void setSourceCovariances(const std::vector<Eigen::Matrix4d>& covs);
+  /// @brief Set target point covariances.
+  void setTargetCovariances(const std::vector<Eigen::Matrix4d>& covs);
+  /// @brief Get source point covariances.
+  const std::vector<Eigen::Matrix4d>& getSourceCovariances() const;
+  /// @brief Get target point covariances.
+  const std::vector<Eigen::Matrix4d>& getTargetCovariances() const;
+
+  /// @brief Swap source and target point clouds and their augmented data (KdTrees, covariances, and voxelmaps).
   void swapSourceAndTarget();
+  /// @brief Clear source point cloud.
   void clearSource();
+  /// @brief Clear target point cloud.
   void clearTarget();
 
 protected:
   virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
 
 protected:
-  int num_threads_;
-  int k_correspondences_;
-  double rotation_epsilon_;
-  double voxel_resolution_;
-  bool verbose_;
-  std::string registration_type_;
+  int num_threads_;                ///< Number of threads to use.
+  int k_correspondences_;          ///< Number of neighbors for covariance estimation.
+  double rotation_epsilon_;        ///< Rotation epsilon for convergence check.
+  double voxel_resolution_;        ///< Voxel resolution for VGICP.
+  std::string registration_type_;  ///< Registration type ("GICP" or "VGICP").
+  bool verbose_;                   ///< Verbosity flag.
 
-  std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> target_tree_;
-  std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> source_tree_;
+  std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> target_tree_;  ///< KdTree for target point cloud.
+  std::shared_ptr<KdTreeOMP<pcl::PointCloud<PointSource>>> source_tree_;  ///< KdTree for source point cloud.
 
-  std::shared_ptr<GaussianVoxelMap> target_voxelmap_;
-  std::shared_ptr<GaussianVoxelMap> source_voxelmap_;
+  std::shared_ptr<GaussianVoxelMap> target_voxelmap_;  ///< VoxelMap for target point cloud.
+  std::shared_ptr<GaussianVoxelMap> source_voxelmap_;  ///< VoxelMap for source point cloud.
 
-  std::vector<Eigen::Matrix4d> target_covs_;
-  std::vector<Eigen::Matrix4d> source_covs_;
+  std::vector<Eigen::Matrix4d> target_covs_;  ///< Covariances of target points
+  std::vector<Eigen::Matrix4d> source_covs_;  ///< Covariances of source points.
 
-  Eigen::Matrix<double, 6, 6> final_hessian_;
+  RegistrationResult result_;  ///< Registration result.
 };
 
 }  // namespace small_gicp

include/small_gicp/pcl/pcl_registration_impl.hpp

@@ -33,8 +33,6 @@ RegistrationPCL<PointSource, PointTarget>::RegistrationPCL() {
   voxel_resolution_ = 1.0;
   verbose_ = false;
   registration_type_ = "GICP";
-
-  final_hessian_.setIdentity();
 }
 
 template <typename PointSource, typename PointTarget>
@@ -64,6 +62,54 @@ void RegistrationPCL<PointSource, PointTarget>::setInputTarget(const PointCloudT
   target_voxelmap_.reset();
 }
 
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setSourceCovariances(const std::vector<Eigen::Matrix4d>& covs) {
+  if (input_ == nullptr) {
+    PCL_ERROR("[RegistrationPCL::setSourceCovariances] Target cloud is not set\n");
+    return;
+  }
+
+  if (covs.size() != input_->size()) {
+    PCL_ERROR("[RegistrationPCL::setSourceCovariances] Invalid number of covariances: %lu\n", covs.size());
+    return;
+  }
+
+  source_covs_ = covs;
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setTargetCovariances(const std::vector<Eigen::Matrix4d>& covs) {
+  if (target_ == nullptr) {
+    PCL_ERROR("[RegistrationPCL::setTargetCovariances] Target cloud is not set\n");
+    return;
+  }
+
+  if (covs.size() != target_->size()) {
+    PCL_ERROR("[RegistrationPCL::setTargetCovariances] Invalid number of covariances: %lu\n", covs.size());
+    return;
+  }
+
+  target_covs_ = covs;
+}
+
+template <typename PointSource, typename PointTarget>
+const std::vector<Eigen::Matrix4d>& RegistrationPCL<PointSource, PointTarget>::getSourceCovariances() const {
+  if (source_covs_.empty()) {
+    PCL_WARN("[RegistrationPCL::getSourceCovariances] Covariances are not estimated\n");
+  }
+
+  return source_covs_;
+}
+
+template <typename PointSource, typename PointTarget>
+const std::vector<Eigen::Matrix4d>& RegistrationPCL<PointSource, PointTarget>::getTargetCovariances() const {
+  if (target_covs_.empty()) {
+    PCL_WARN("[RegistrationPCL::getTargetCovariances] Covariances are not estimated\n");
+  }
+
+  return target_covs_;
+}
+
 template <typename PointSource, typename PointTarget>
 void RegistrationPCL<PointSource, PointTarget>::swapSourceAndTarget() {
   input_.swap(target_);
@@ -88,6 +134,45 @@ void RegistrationPCL<PointSource, PointTarget>::clearTarget() {
   target_voxelmap_.reset();
 }
 
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setNumThreads(int n) {
+  if (n <= 0) {
+    PCL_ERROR("[RegistrationPCL::setNumThreads] Invalid number of threads: %d\n", n);
+    n = 1;
+  }
+
+  num_threads_ = n;
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
+  setNumNeighborsForCovariance(k);
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setNumNeighborsForCovariance(int k) {
+  if (k < 5) {
+    PCL_ERROR("[RegistrationPCL::setNumNeighborsForCovariance] Invalid number of neighbors: %d\n", k);
+    k = 5;
+  }
+  k_correspondences_ = k;
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setVoxelResolution(double r) {
+  if (voxel_resolution_ <= 0) {
+    PCL_ERROR("[RegistrationPCL::setVoxelResolution] Invalid voxel resolution: %f\n", r);
+    r = 1.0;
+  }
+
+  voxel_resolution_ = r;
+}
+
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setRotationEpsilon(double eps) {
+  rotation_epsilon_ = eps;
+}
+
 template <typename PointSource, typename PointTarget>
 void RegistrationPCL<PointSource, PointTarget>::setRegistrationType(const std::string& type) {
   if (type == "GICP") {
@@ -99,6 +184,21 @@ void RegistrationPCL<PointSource, PointTarget>::setRegistrationType(const std::s
   }
 }
 
+template <typename PointSource, typename PointTarget>
+void RegistrationPCL<PointSource, PointTarget>::setVerbosity(bool verbose) {
+  verbose_ = verbose;
+}
+
+template <typename PointSource, typename PointTarget>
+const Eigen::Matrix<double, 6, 6>& RegistrationPCL<PointSource, PointTarget>::getFinalHessian() const {
+  return result_.H;
+}
+
+template <typename PointSource, typename PointTarget>
+const RegistrationResult& RegistrationPCL<PointSource, PointTarget>::getRegistrationResult() const {
+  return result_;
+}
+
 template <typename PointSource, typename PointTarget>
 void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
   if (output.points.data() == input_->points.data() || output.points.data() == target_->points.data()) {
@@ -123,9 +223,8 @@ void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloud
   registration.optimizer.verbose = verbose_;
   registration.optimizer.max_iterations = max_iterations_;
 
-  RegistrationResult result(Eigen::Isometry3d::Identity());
   if (registration_type_ == "GICP") {
-    result = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast<double>()));
+    result_ = registration.align(target_proxy, source_proxy, *target_tree_, Eigen::Isometry3d(guess.template cast<double>()));
   } else if (registration_type_ == "VGICP") {
     if (!target_voxelmap_) {
       target_voxelmap_ = std::make_shared<GaussianVoxelMap>(voxel_resolution_);
@@ -136,14 +235,14 @@ void RegistrationPCL<PointSource, PointTarget>::computeTransformation(PointCloud
       source_voxelmap_->insert(source_proxy);
     }
 
-    result = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast<double>()));
+    result_ = registration.align(*target_voxelmap_, source_proxy, *target_voxelmap_, Eigen::Isometry3d(guess.template cast<double>()));
   } else {
     PCL_ERROR("[RegistrationPCL::computeTransformation] Invalid registration type: %s\n", registration_type_.c_str());
     return;
   }
 
-  final_transformation_ = result.T_target_source.matrix().template cast<float>();
-  final_hessian_ = result.H;
+  converged_ = result_.converged;
+  final_transformation_ = result_.T_target_source.matrix().template cast<float>();
   pcl::transformPointCloud(*input_, output, final_transformation_);
 }
 

include/small_gicp/registration/optimizer.hpp

@@ -38,8 +38,11 @@ struct GaussNewtonOptimizer {
 
     RegistrationResult result(init_T);
     for (int i = 0; i < max_iterations && !result.converged; i++) {
+      // Linearize
       auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
       general_factor.update_linearized_system(target, source, target_tree, result.T_target_source, &H, &b, &e);
+
+      // Solve linear system
       const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
 
       if (verbose) {
@@ -95,12 +98,17 @@ struct LevenbergMarquardtOptimizer {
     double lambda = init_lambda;
     RegistrationResult result(init_T);
     for (int i = 0; i < max_iterations && !result.converged; i++) {
+      // Linearize
       auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors);
       general_factor.update_linearized_system(target, source, target_tree, result.T_target_source, &H, &b, &e);
 
+      // Lambda iteration
       bool success = false;
       for (int j = 0; j < max_inner_iterations; j++) {
+        // Solve with damping
         const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
+
+        // Validte new solution
         const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta);
         double new_e = reduction.error(target, source, new_T, factors);
         general_factor.update_error(target, source, new_T, &e);
@@ -111,13 +119,15 @@ struct LevenbergMarquardtOptimizer {
         }
 
         if (new_e < e) {
+          // Error decreased, decrease lambda
           result.converged = criteria.converged(delta);
           result.T_target_source = new_T;
           lambda /= lambda_factor;
           success = true;
 
           break;
         } else {
+          // Failed to decrease error, increase lambda
           lambda *= lambda_factor;
         }
       }

include/small_gicp/registration/reduction_omp.hpp

@@ -15,7 +15,7 @@ inline int omp_get_thread_num() {
 
 /// @brief Parallel reduction with OpenMP backend
 struct ParallelReductionOMP {
-  ParallelReductionOMP() : num_threads(8) {}
+  ParallelReductionOMP() : num_threads(4) {}
 
   template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree, typename CorrespondenceRejector, typename Factor>
   std::tuple<Eigen::Matrix<double, 6, 6>, Eigen::Matrix<double, 6, 1>, double> linearize(

include/small_gicp/registration/registration.hpp

@@ -31,6 +31,13 @@ struct Registration {
   template <typename TargetPointCloud, typename SourcePointCloud, typename TargetTree>
   RegistrationResult
   align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T = Eigen::Isometry3d::Identity()) const {
+    if (traits::size(target) <= 10) {
+      std::cerr << "warning: target point cloud is too small. |target|=" << traits::size(target) << std::endl;
+    }
+    if (traits::size(source) <= 10) {
+      std::cerr << "warning: source point cloud is too small. |source|=" << traits::size(source) << std::endl;
+    }
+
     std::vector<Factor> factors(traits::size(source));
     return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors, general_factor);
   }

include/small_gicp/registration/registration_result.hpp

@@ -9,7 +9,7 @@ namespace small_gicp {
 
 /// @brief Registration result
 struct RegistrationResult {
-  RegistrationResult(const Eigen::Isometry3d& T)
+  RegistrationResult(const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity())
   : T_target_source(T),
     converged(false),
     iterations(0),