aa_icp.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/pcl/registration/point_to_point_icp.hpp>
#include <cpp-toolbox/logger/thread_logger.hpp>
 
#include <deque>
#include <Eigen/QR>
 
namespace toolbox::pcl
{
 
template<typename DataType, 
         typename KNNSearcher = kdtree_t<DataType>,
         typename BaseICP = point_to_point_icp_t<DataType, KNNSearcher>>
class aa_icp_t : public base_fine_registration_t<aa_icp_t<DataType, KNNSearcher, BaseICP>, DataType>
{
public:
  using base_type = base_fine_registration_t<aa_icp_t<DataType, KNNSearcher, BaseICP>, 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 base_icp_type = BaseICP;
  using VectorX = Eigen::Matrix<DataType, Eigen::Dynamic, 1>;
  using MatrixX = Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>;
  
  explicit aa_icp_t(bool enable_parallel = false)
      : m_base_icp(enable_parallel)
  {
    // 传递基类的参数设置函数到基础ICP
    m_base_icp.set_outlier_rejection_ratio(0.1);
  }
  
  void set_anderson_m(std::size_t m) { m_anderson_m = m; }
  
  std::size_t get_anderson_m() const { return m_anderson_m; }
  
  void set_beta(DataType beta) {
    m_beta = std::clamp(beta, static_cast<DataType>(0), static_cast<DataType>(1));
  }
  
  DataType get_beta() const { return m_beta; }
  
  void set_regularization(DataType lambda) {
    m_regularization = std::max(static_cast<DataType>(0), lambda);
  }
  
  DataType get_regularization() const { return m_regularization; }
  
  void set_enable_safeguarding(bool enable) { m_enable_safeguarding = enable; }
  
  bool get_enable_safeguarding() const { return m_enable_safeguarding; }
  
  correspondence_type_e get_correspondence_type_impl() const {
    return m_base_icp.get_correspondence_type_impl();
  }
  
  void set_enable_parallel(bool enable) { 
    m_base_icp.set_enable_parallel(enable);
  }
  
  bool get_enable_parallel() const { 
    return m_base_icp.get_enable_parallel();
  }
  
protected:
  friend class base_fine_registration_t<aa_icp_t<DataType, KNNSearcher, BaseICP>, DataType>;
  
  bool validate_input_impl() const {
    // 基础验证交给base_icp自己处理
    return true;
  }
  
  void preprocess_impl() {
    // 设置基础ICP的点云
    m_base_icp.set_source(this->m_source_cloud);
    m_base_icp.set_target(this->m_target_cloud);
    
    // 传递最大对应距离设置
    m_base_icp.set_max_correspondence_distance(this->m_max_correspondence_distance);
    
    // 基础ICP会在自己的align中调用preprocess
  }
  
  bool align_impl(const transformation_t& initial_guess, result_type& result);
  
  VectorX transformation_to_vector(const transformation_t& transform) const;
  
  transformation_t vector_to_transformation(const VectorX& vec) const;
  
  transformation_t perform_base_icp_step(const transformation_t& current_transform,
                                       DataType& error,
                                       std::size_t& num_correspondences);
  
  VectorX anderson_acceleration_update(const std::deque<VectorX>& g_history,
                                      const std::deque<VectorX>& x_history);
  
  bool is_numerically_stable(const VectorX& vec) const;
  
private:
  base_icp_type m_base_icp;                    
  
  std::size_t m_anderson_m = 5;                
  DataType m_beta = 0.5;                       
  DataType m_regularization = 1e-6;            
  bool m_enable_safeguarding = true;           
  
  // Anderson加速的历史记录
  std::deque<VectorX> m_g_history;             
  std::deque<VectorX> m_x_history;             
  
  bool m_initialized = false;                    
};
 
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/aa_icp_impl.hpp>