super_four_pcs_registration.hpp

Go to the documentation of this file.

#pragma once
 
#include <optional>
 
#include <unordered_map>
#include <cpp-toolbox/pcl/registration/four_pcs_registration.hpp>
 
namespace toolbox::pcl
{
 
template<typename DataType>
class CPP_TOOLBOX_EXPORT super_four_pcs_registration_t
    : public four_pcs_registration_t<DataType>
{
  // Allow base classes to access protected members
  friend class base_coarse_registration_t<super_four_pcs_registration_t<DataType>, DataType>;
  friend class four_pcs_registration_t<DataType>;
public:
  using base_type = four_pcs_registration_t<DataType>;
  using typename base_type::base_4pcs_t;
  using typename base_type::candidate_t;
  using typename base_type::point_cloud;
  using typename base_type::point_cloud_ptr;
  using typename base_type::result_type;
  using typename base_type::transformation_t;
  using typename base_type::vector3_t;
  using typename base_type::matrix3_t;
 
  struct point_pair_t
  {
    std::size_t idx1;      
    std::size_t idx2;      
    DataType distance;     
  };
 
  struct grid_cell_t
  {
    std::vector<std::size_t> point_indices;  
  };
 
  class smart_index_t
  {
  public:
    smart_index_t(DataType cell_size) : m_cell_size(cell_size) {}
 
    void build(const point_cloud_ptr& cloud, const std::vector<std::size_t>& indices);
 
    [[nodiscard]] std::vector<point_pair_t> find_pairs_in_range(
        DataType distance, DataType epsilon, std::size_t max_pairs = 1000) const;
 
    [[nodiscard]] std::string get_statistics() const;
 
  private:
    [[nodiscard]] std::tuple<int, int, int> compute_grid_key(const vector3_t& pt) const;
 
    [[nodiscard]] std::vector<std::tuple<int, int, int>> get_neighbor_cells(
        const std::tuple<int, int, int>& center_key, int radius) const;
 
    DataType m_cell_size;  
    std::map<std::tuple<int, int, int>, grid_cell_t> m_grid;  
    point_cloud_ptr m_cloud;  
    vector3_t m_min_bound;    
    vector3_t m_max_bound;    
  };
 
  super_four_pcs_registration_t() = default;
  ~super_four_pcs_registration_t() = default;
 
  void set_grid_resolution(DataType resolution) { m_grid_resolution = resolution; }
 
  [[nodiscard]] DataType get_grid_resolution() const { return m_grid_resolution; }
 
  void enable_smart_indexing(bool enable) { m_use_smart_indexing = enable; }
 
  [[nodiscard]] bool is_smart_indexing_enabled() const { return m_use_smart_indexing; }
 
  void set_pair_distance_epsilon(DataType epsilon) { m_pair_distance_epsilon = epsilon; }
 
  [[nodiscard]] DataType get_pair_distance_epsilon() const { return m_pair_distance_epsilon; }
 
protected:
  [[nodiscard]] std::string get_algorithm_name_impl() const { return "Super4PCS"; }
 
  bool align_impl(result_type& result);
 
  [[nodiscard]] std::vector<base_4pcs_t> find_congruent_sets_optimized(
      const base_4pcs_t& source_base) const;
 
private:
  [[nodiscard]] std::vector<point_pair_t> extract_pairs_smart(
      const std::vector<std::size_t>& indices, const point_cloud_ptr& cloud,
      DataType distance, DataType epsilon) const;
 
  [[nodiscard]] std::optional<base_4pcs_t> build_base_from_pairs(
      const point_pair_t& pair1, const point_pair_t& pair2,
      const point_cloud_ptr& cloud) const;
 
  [[nodiscard]] bool verify_pair_compatibility(const point_pair_t& pair1,
                                                const point_pair_t& pair2) const;
 
  [[nodiscard]] DataType compute_adaptive_grid_resolution(
      const point_cloud_ptr& cloud, const std::vector<std::size_t>& indices) const;
 
  // Super4PCS特定参数 / Super4PCS-specific parameters
  DataType m_grid_resolution = 0;  
  bool m_use_smart_indexing = true;  
  DataType m_pair_distance_epsilon = static_cast<DataType>(0.01);  
  
  // 内部数据结构 / Internal data structures
  mutable std::unique_ptr<smart_index_t> m_source_index;  
  mutable std::unique_ptr<smart_index_t> m_target_index;  
};
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/super_four_pcs_registration_impl.hpp>