point_to_plane_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 point_to_plane_icp_t : public base_fine_registration_t<point_to_plane_icp_t<DataType, KNNSearcher>, DataType>
{
public:
  using base_type = base_fine_registration_t<point_to_plane_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;
  
  // 法线结构体定义
  struct normal_t {
    DataType normal_x;
    DataType normal_y;
    DataType normal_z;
    DataType curvature;
  };
  using normal_type = normal_t;
  
  explicit point_to_plane_icp_t(bool enable_parallel = false)
      : m_enable_parallel(enable_parallel),
        m_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::POINT_TO_PLANE;
  }
  
  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_regularization(DataType lambda) {
    m_regularization = std::max(static_cast<DataType>(0), lambda);
  }
  
  DataType get_regularization() const { return m_regularization; }
  
protected:
  friend class base_fine_registration_t<point_to_plane_icp_t<DataType, KNNSearcher>, DataType>;
  
  bool validate_input_impl() const {
    // TODO: 验证目标点云是否具有法线信息
    // 目前假设用户已提供正确的带法线的点云
    return true;
  }
  
  void preprocess_impl() {
    if (this->m_target_cloud) {
      LOG_INFO_S << "构建目标点云KD树 / Building target cloud KD-tree";
      m_knn_searcher->set_input(*this->m_target_cloud);
    }
  }
  
  bool align_impl(const transformation_t& initial_guess, result_type& result);
  
  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);
  
  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);
  
  Eigen::Matrix<DataType, 6, 1> transformation_to_vector(const transformation_t& transform) const;
  
  transformation_t vector_to_transformation(const Eigen::Matrix<DataType, 6, 1>& vec) const;
  
private:
  bool m_enable_parallel;                              
  std::unique_ptr<knn_searcher_type> m_knn_searcher;  
  DataType m_outlier_rejection_ratio = 0.1;           
  DataType m_regularization = 1e-4;                   
};
 
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/point_to_plane_icp_impl.hpp>