generalized_icp.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/pcl/registration/base_fine_registration.hpp>
#include <cpp-toolbox/pcl/knn/kdtree.hpp>
#include <cpp-toolbox/base/thread_pool_singleton.hpp>
#include <cpp-toolbox/logger/thread_logger.hpp>
 
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
 
namespace toolbox::pcl
{
 
template<typename DataType, typename KNNSearcher = kdtree_t<DataType>>
class generalized_icp_t : public base_fine_registration_t<generalized_icp_t<DataType, KNNSearcher>, DataType>
{
public:
  using base_type = base_fine_registration_t<generalized_icp_t<DataType, KNNSearcher>, DataType>;
  using typename base_type::point_cloud;
  using typename base_type::point_cloud_ptr;
  using typename base_type::transformation_t;
  using typename base_type::result_type;
  
  using point_type = toolbox::types::point_t<DataType>;
  using knn_searcher_type = KNNSearcher;
  using Matrix3 = Eigen::Matrix<DataType, 3, 3>;
  using Vector3 = Eigen::Matrix<DataType, 3, 1>;
  using Vector6 = Eigen::Matrix<DataType, 6, 1>;
  using Matrix6 = Eigen::Matrix<DataType, 6, 6>;
  
  explicit generalized_icp_t(bool enable_parallel = false)
      : m_enable_parallel(enable_parallel),
        m_source_knn_searcher(std::make_unique<knn_searcher_type>()),
        m_target_knn_searcher(std::make_unique<knn_searcher_type>())
  {
  }
  
  void set_enable_parallel(bool enable) { m_enable_parallel = enable; }
  
  bool get_enable_parallel() const { return m_enable_parallel; }
  
  correspondence_type_e get_correspondence_type_impl() const {
    return correspondence_type_e::PLANE_TO_PLANE;
  }
  
  void set_k_correspondences(std::size_t k) { m_k_correspondences = k; }
  
  std::size_t get_k_correspondences() const { return m_k_correspondences; }
  
  void set_covariance_epsilon(DataType epsilon) {
    m_covariance_epsilon = std::max(static_cast<DataType>(0), epsilon);
  }
  
  DataType get_covariance_epsilon() const { return m_covariance_epsilon; }
  
  void set_outlier_rejection_ratio(DataType ratio) {
    m_outlier_rejection_ratio = std::clamp(ratio, static_cast<DataType>(0), static_cast<DataType>(1));
  }
  
  DataType get_outlier_rejection_ratio() const { return m_outlier_rejection_ratio; }
  
  void set_optimizer_max_iterations(std::size_t max_iter) {
    m_optimizer_max_iterations = max_iter;
  }
  
  std::size_t get_optimizer_max_iterations() const { return m_optimizer_max_iterations; }
  
protected:
  friend class base_fine_registration_t<generalized_icp_t<DataType, KNNSearcher>, DataType>;
  
  void preprocess_impl();
  
  bool align_impl(const transformation_t& initial_guess, result_type& result);
  
  void compute_covariances(const point_cloud& cloud,
                          knn_searcher_type& searcher,
                          std::vector<Matrix3>& covariances);
  
  void find_correspondences(const point_cloud& transformed_source,
                           std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                           std::vector<DataType>& distances);
  
  transformation_t compute_transformation(const point_cloud& source,
                                        const point_cloud& target,
                                        const std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                                        const transformation_t& current_transform);
  
  DataType compute_mahalanobis_distance(const Vector3& p_src,
                                       const Vector3& p_tgt,
                                       const Matrix3& C_src,
                                       const Matrix3& C_tgt,
                                       const transformation_t& transform,
                                       Vector6* gradient = nullptr) const;
  
  DataType compute_error(const point_cloud& source,
                        const point_cloud& target,
                        const std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                        const transformation_t& transform) const;
  
  void reject_outliers(std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                      std::vector<DataType>& distances);
  
  DataType objective_function(const Vector6& x,
                             const point_cloud& source,
                             const point_cloud& target,
                             const std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                             const transformation_t& base_transform,
                             Vector6* gradient = nullptr) const;
  
  Vector6 lbfgs_optimize(const Vector6& x0,
                        const point_cloud& source,
                        const point_cloud& target,
                        const std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                        const transformation_t& base_transform);
  
  Vector6 transformation_to_vector(const transformation_t& transform) const;
  
  transformation_t vector_to_transformation(const Vector6& vec) const;
  
private:
  bool m_enable_parallel;                                    
  std::unique_ptr<knn_searcher_type> m_source_knn_searcher; 
  std::unique_ptr<knn_searcher_type> m_target_knn_searcher; 
  
  std::size_t m_k_correspondences = 20;                     
  DataType m_covariance_epsilon = 0.001;                   
  DataType m_outlier_rejection_ratio = 0.1;                
  std::size_t m_optimizer_max_iterations = 10;             
  
  std::vector<Matrix3> m_source_covariances;               
  std::vector<Matrix3> m_target_covariances;               
};
 
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/generalized_icp_impl.hpp>