ndt.hpp

Go to the documentation of this file.

#pragma once
 
#include <cpp-toolbox/pcl/registration/base_fine_registration.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>
#include <unordered_map>
#include <array>
 
namespace toolbox::pcl
{
 
template<typename DataType>
class ndt_t : public base_fine_registration_t<ndt_t<DataType>, DataType>
{
public:
  using base_type = base_fine_registration_t<ndt_t<DataType>, 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 Vector3 = Eigen::Matrix<DataType, 3, 1>;
  using Matrix3 = Eigen::Matrix<DataType, 3, 3>;
  using Vector6 = Eigen::Matrix<DataType, 6, 1>;
  using Matrix6 = Eigen::Matrix<DataType, 6, 6>;
  
  struct voxel_cell_t {
    Vector3 mean = Vector3::Zero();           
    Matrix3 covariance = Matrix3::Zero();     
    Matrix3 covariance_inv = Matrix3::Zero(); 
    std::size_t num_points = 0;               
    bool valid = false;                       
  };
  
  explicit ndt_t(bool enable_parallel = false)
      : m_enable_parallel(enable_parallel)
  {
  }
  
  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_DISTRIBUTION;
  }
  
  void set_resolution(DataType resolution) {
    m_resolution = std::max(static_cast<DataType>(0.01), resolution);
    m_voxel_grid_updated = true;
  }
  
  DataType get_resolution() const { return m_resolution; }
  
  void set_step_size(DataType step_size) {
    m_step_size = std::max(static_cast<DataType>(0.001), step_size);
  }
  
  DataType get_step_size() const { return m_step_size; }
  
  void set_outlier_ratio(DataType ratio) {
    m_outlier_ratio = std::clamp(ratio, static_cast<DataType>(0), static_cast<DataType>(1));
  }
  
  DataType get_outlier_ratio() const { return m_outlier_ratio; }
  
  void set_line_search_max_iterations(std::size_t max_iter) {
    m_line_search_max_iterations = max_iter;
  }
  
  std::size_t get_line_search_max_iterations() const { return m_line_search_max_iterations; }
  
protected:
  friend class base_fine_registration_t<ndt_t<DataType>, DataType>;
  
  void preprocess_impl();
  
  bool align_impl(const transformation_t& initial_guess, result_type& result);
  
  void build_voxel_grid();
  
  std::array<int, 3> compute_voxel_index(const Vector3& point) const;
  
  std::size_t get_voxel_key(const std::array<int, 3>& index) const;
  
  DataType compute_objective(const transformation_t& transform,
                            Vector6* gradient = nullptr,
                            Matrix6* hessian = nullptr) const;
  
  DataType compute_point_contribution(const Vector3& point,
                                     const transformation_t& transform,
                                     Vector6* gradient = nullptr,
                                     Matrix6* hessian = nullptr) const;
  
  DataType line_search(const transformation_t& current_transform,
                      const Vector6& direction,
                      DataType initial_step_size);
  
  transformation_t vector_to_transformation(const Vector6& vec) const;
  
  DataType gaussian_d1(DataType x_bar, DataType x, 
                      const Matrix3& c_inv) const;
  
  void compute_jacobian(const Vector3& point, 
                       Eigen::Matrix<DataType, 3, 6>& jacobian) const;
  
private:
  bool m_enable_parallel;                           
  DataType m_resolution = 1.0;                      
  DataType m_step_size = 0.1;                       
  DataType m_outlier_ratio = 0.55;                  
  std::size_t m_line_search_max_iterations = 20;    
  
  bool m_voxel_grid_updated = true;                 
  std::unordered_map<std::size_t, voxel_cell_t> m_voxel_grid; 
  
  // 预计算的常数
  DataType m_gauss_d1 = 0;                          
  DataType m_gauss_d2 = 0;                          
};
 
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/ndt_impl.hpp>