point_to_point_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/SVD>
#include <Eigen/Geometry>
 
namespace toolbox::pcl
{
 
template<typename DataType, typename KNNSearcher = kdtree_t<DataType>>
class point_to_point_icp_t : public base_fine_registration_t<point_to_point_icp_t<DataType, KNNSearcher>, DataType>
{
public:
  using base_type = base_fine_registration_t<point_to_point_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;
  
  explicit point_to_point_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_POINT;
  }
  
  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; }
  
protected:
  friend class base_fine_registration_t<point_to_point_icp_t<DataType, KNNSearcher>, DataType>;
  
  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,
                                        const std::vector<DataType>& distances);
  
  DataType compute_error(const std::vector<DataType>& distances) const;
  
  void reject_outliers(std::vector<std::pair<std::size_t, std::size_t>>& correspondences,
                      std::vector<DataType>& distances);
  
private:
  bool m_enable_parallel;                              
  std::unique_ptr<knn_searcher_type> m_knn_searcher;  
  DataType m_outlier_rejection_ratio = 0.1;           
};
 
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/point_to_point_icp_impl.hpp>