prosac_registration.hpp

Go to the documentation of this file.

#pragma once
 
#include <algorithm>
#include <chrono>
#include <cmath>
#include <random>
#include <vector>
 
#include <Eigen/Dense>
#include <Eigen/SVD>
 
#include <cpp-toolbox/cpp-toolbox_export.hpp>
#include <cpp-toolbox/pcl/registration/base_coarse_registration.hpp>
 
namespace toolbox::pcl
{
 
template<typename DataType>
class CPP_TOOLBOX_EXPORT prosac_registration_t
    : public base_coarse_registration_t<prosac_registration_t<DataType>,
                                        DataType>
{
  // Allow base class to access protected members
  friend class base_coarse_registration_t<prosac_registration_t<DataType>,
                                          DataType>;
 
public:
  using base_type =
      base_coarse_registration_t<prosac_registration_t<DataType>, DataType>;
  using typename base_type::point_cloud;
  using typename base_type::point_cloud_ptr;
  using typename base_type::correspondences_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>;
 
  prosac_registration_t() = default;
  ~prosac_registration_t() = default;
 
  void set_confidence(DataType confidence)
  {
    m_confidence = std::clamp(confidence, static_cast<DataType>(0.0),
                              static_cast<DataType>(1.0));
  }
 
  [[nodiscard]] DataType get_confidence() const { return m_confidence; }
 
  void set_sample_size(std::size_t size)
  {
    m_sample_size = std::max(size, std::size_t(3));
  }
 
  [[nodiscard]] std::size_t get_sample_size() const { return m_sample_size; }
 
  void set_refine_result(bool refine) { m_refine_result = refine; }
 
  [[nodiscard]] bool get_refine_result() const { return m_refine_result; }
 
  void set_early_stop_ratio(DataType ratio)
  {
    m_early_stop_ratio = std::clamp(ratio, static_cast<DataType>(0.0),
                                    static_cast<DataType>(1.0));
  }
 
  [[nodiscard]] DataType get_early_stop_ratio() const
  {
    return m_early_stop_ratio;
  }
 
  void set_initial_inlier_ratio(DataType ratio)
  {
    m_initial_inlier_ratio =
        std::clamp(ratio, static_cast<DataType>(0.01), static_cast<DataType>(0.9));
  }
 
  [[nodiscard]] DataType get_initial_inlier_ratio() const
  {
    return m_initial_inlier_ratio;
  }
 
  void set_non_randomness_threshold(DataType threshold)
  {
    m_non_randomness_threshold = threshold;
  }
 
  [[nodiscard]] DataType get_non_randomness_threshold() const
  {
    return m_non_randomness_threshold;
  }
 
  void set_sorted_correspondences(
      const correspondences_ptr& correspondences,
      const std::vector<std::size_t>& sorted_indices = {},
      const std::vector<DataType>& quality_scores = {})
  {
    this->set_correspondences(correspondences);
 
    if (!sorted_indices.empty()) {
      m_sorted_indices = sorted_indices;
    } else {
      // 如果未提供排序索引，假设对应关系已按顺序排序 / If no sorted indices
      // provided, assume correspondences are already sorted
      m_sorted_indices.resize(correspondences->size());
      std::iota(m_sorted_indices.begin(), m_sorted_indices.end(), 0);
    }
 
    m_quality_scores = quality_scores;
  }
 
  [[nodiscard]] const std::vector<std::size_t>& get_sorted_indices() const
  {
    return m_sorted_indices;
  }
 
  // 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 "PROSAC"; }
 
  [[nodiscard]] bool validate_input_impl() const;
 
  void set_correspondences_impl(const correspondences_ptr& /*correspondences*/)
  {
    // PROSAC需要排序的对应关系 / PROSAC requires sorted correspondences
    // 清除之前的排序信息 / Clear previous sorting information
    m_sorted_indices.clear();
    m_quality_scores.clear();
    m_T_n.clear();
  }
 
private:
  void precompute_sampling_schedule(std::size_t n_correspondences);
 
  void progressive_sample(std::vector<correspondence_t>& sample, std::size_t n,
                          std::size_t t, std::mt19937& generator) const;
 
  [[nodiscard]] transformation_t estimate_transformation(
      const std::vector<correspondence_t>& sample) const;
 
  [[nodiscard]] std::size_t count_inliers(const transformation_t& transform,
                                           std::vector<std::size_t>& inliers) const;
 
  [[nodiscard]] bool check_non_randomness(std::size_t inlier_count,
                                           std::size_t n) const;
 
  [[nodiscard]] bool check_maximality(std::size_t inlier_count, std::size_t n,
                                      std::size_t t) const;
 
  [[nodiscard]] transformation_t
  refine_transformation(const std::vector<std::size_t>& inlier_indices) const;
 
  [[nodiscard]] std::size_t compute_binomial_coefficient(std::size_t n,
                                                          std::size_t k) const;
 
  [[nodiscard]] DataType compute_beta(std::size_t i, std::size_t m,
                                      std::size_t n) const;
 
  [[nodiscard]] bool is_sample_valid(
      const std::vector<correspondence_t>& sample) const;
 
  [[nodiscard]] DataType compute_fitness_score(
      const transformation_t& transform,
      const std::vector<std::size_t>& inliers) const;
 
  // PROSAC特定参数 / PROSAC-specific parameters
  DataType m_confidence = static_cast<DataType>(0.99);  
  std::size_t m_sample_size = 3;  
  bool m_refine_result = true;    
  DataType m_early_stop_ratio = static_cast<DataType>(
      0.9);  
  DataType m_initial_inlier_ratio = static_cast<DataType>(
      0.1);  
  DataType m_non_randomness_threshold = static_cast<DataType>(
      0.05);  
 
  // 排序信息 / Sorting information
  std::vector<std::size_t> m_sorted_indices;  
  std::vector<DataType> m_quality_scores;     
 
  // 预计算的采样调度 / Precomputed sampling schedule
  std::vector<std::size_t> m_T_n;  
 
  // 统计信息 / Statistics
  mutable std::size_t m_total_samples = 0;      
  mutable std::size_t m_best_inlier_count = 0;  
};
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/prosac_registration_impl.hpp>