four_pcs_registration.hpp

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <unordered_map>
#include <unordered_set>
 
#include <cpp-toolbox/cpp-toolbox_export.hpp>
#include <cpp-toolbox/pcl/knn/kdtree.hpp>
#include <cpp-toolbox/pcl/registration/base_coarse_registration.hpp>
 
namespace toolbox::pcl
{
 
template<typename DataType>
class CPP_TOOLBOX_EXPORT four_pcs_registration_t
    : public base_coarse_registration_t<four_pcs_registration_t<DataType>, DataType>
{
  // Allow base class to access protected members
  friend class base_coarse_registration_t<four_pcs_registration_t<DataType>, DataType>;
public:
  using base_type = base_coarse_registration_t<four_pcs_registration_t<DataType>, DataType>;
  using typename base_type::point_cloud;
  using typename base_type::point_cloud_ptr;
  using typename base_type::result_type;
  using transformation_t = Eigen::Matrix<DataType, 4, 4>;
  using vector3_t = Eigen::Matrix<DataType, 3, 1>;
  using matrix3_t = Eigen::Matrix<DataType, 3, 3>;
  using kdtree_t = toolbox::pcl::kdtree_t<DataType>;
 
  struct base_4pcs_t
  {
    std::array<std::size_t, 4> indices;  
    std::array<vector3_t, 4> points;     
    DataType invariant1;  
    DataType invariant2;  
    vector3_t normal;     
    DataType d;           
  };
 
  struct candidate_t
  {
    base_4pcs_t source_base;   
    base_4pcs_t target_base;   
    transformation_t transform;  
    DataType lcp_score;        
    std::size_t num_inliers;   
  };
 
  four_pcs_registration_t() = default;
  ~four_pcs_registration_t() = default;
 
  void set_delta(DataType delta) { m_delta = std::abs(delta); }
 
  [[nodiscard]] DataType get_delta() const { return m_delta; }
 
  void set_overlap(DataType overlap)
  {
    m_overlap = std::clamp(overlap, static_cast<DataType>(0.0), static_cast<DataType>(1.0));
  }
 
  [[nodiscard]] DataType get_overlap() const { return m_overlap; }
 
  void set_sample_size(std::size_t size) { m_sample_size = size; }
 
  [[nodiscard]] std::size_t get_sample_size() const { return m_sample_size; }
 
  void set_use_normals(bool use_normals) { m_use_normals = use_normals; }
 
  [[nodiscard]] bool get_use_normals() const { return m_use_normals; }
 
  void set_max_normal_angle(DataType angle) { m_max_normal_angle = std::abs(angle); }
 
  [[nodiscard]] DataType get_max_normal_angle() const { return m_max_normal_angle; }
 
  void set_num_bases(std::size_t num_bases) { m_num_bases = num_bases; }
 
  [[nodiscard]] std::size_t get_num_bases() const { return m_num_bases; }
 
  // Methods needed by base class (must be accessible via friend)
protected:
  bool align_impl(result_type& result);
 
  [[nodiscard]] std::string get_algorithm_name_impl() const { return "4PCS"; }
 
  [[nodiscard]] bool validate_input_impl() const;
 
  void set_source_impl(const point_cloud_ptr& source);
 
  void set_target_impl(const point_cloud_ptr& target);
 
  [[nodiscard]] const std::shared_ptr<kdtree_t>& get_target_kdtree() const { return m_target_kdtree; }
  [[nodiscard]] const std::vector<std::size_t>& get_source_samples() const { return m_source_samples; }
  [[nodiscard]] const std::vector<std::size_t>& get_target_samples() const { return m_target_samples; }
 
protected:
  void sample_points(const point_cloud_ptr& cloud, std::size_t num_samples,
                     std::vector<std::size_t>& indices);
  
  [[nodiscard]] bool are_coplanar(const std::array<vector3_t, 4>& points,
                                   DataType tolerance) const;
 
  void compute_invariants(base_4pcs_t& base) const;
 
  [[nodiscard]] transformation_t estimate_transformation(const base_4pcs_t& source_base,
                                                         const base_4pcs_t& target_base) const;
 
  [[nodiscard]] DataType compute_lcp_score(const transformation_t& transform,
                                            std::vector<std::size_t>& inliers) const;
 
  void refine_candidate(candidate_t& candidate) const;
 
  [[nodiscard]] std::vector<base_4pcs_t> extract_coplanar_bases(
      const std::vector<std::size_t>& indices, const point_cloud_ptr& cloud,
      std::size_t num_bases) const;
 
  [[nodiscard]] std::vector<base_4pcs_t> find_congruent_sets(
      const base_4pcs_t& source_base, const std::vector<std::size_t>& target_indices,
      const point_cloud_ptr& target_cloud) const;
 
  [[nodiscard]] bool is_valid_transformation(const transformation_t& transform) const;
 
private:
  // 4PCS特定参数 / 4PCS-specific parameters
  DataType m_delta = static_cast<DataType>(0.01);     
  DataType m_overlap = static_cast<DataType>(0.5);    
  std::size_t m_sample_size = 200;                    
  bool m_use_normals = false;                         
  DataType m_max_normal_angle = static_cast<DataType>(0.349);  
  std::size_t m_num_bases = 100;                      
 
  // 内部数据结构 / Internal data structures
  std::shared_ptr<kdtree_t> m_target_kdtree;          
  std::vector<std::size_t> m_source_samples;          
  std::vector<std::size_t> m_target_samples;          
};
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/four_pcs_registration_impl.hpp>