ransac_registration.hpp

Go to the documentation of this file.

#pragma once
 
#include <algorithm>
#include <chrono>
#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 ransac_registration_t
    : public base_coarse_registration_t<ransac_registration_t<DataType>, DataType>
{
  // Allow base class to access protected members
  friend class base_coarse_registration_t<ransac_registration_t<DataType>, DataType>;
public:
  using base_type = base_coarse_registration_t<ransac_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>;
 
  ransac_registration_t() = default;
  ~ransac_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; }
 
  // 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 "RANSAC"; }
 
  [[nodiscard]] bool validate_input_impl() const;
 
  void set_correspondences_impl(const correspondences_ptr& /*correspondences*/)
  {
    // RANSAC需要对应关系，这里可以添加额外的处理
    // RANSAC requires correspondences, additional processing can be added here
  }
 
private:
  [[nodiscard]] std::size_t calculate_iterations(DataType outlier_ratio) const;
 
  void sample_correspondences(std::vector<correspondence_t>& sample,
                              std::mt19937& generator) const;
 
  [[nodiscard]] transformation_t
  estimate_rigid_transform_svd(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]] transformation_t
  refine_transformation(const std::vector<std::size_t>& inlier_indices) const;
 
  [[nodiscard]] DataType compute_fitness_score(const transformation_t& transform,
                                                const std::vector<std::size_t>& inliers) const;
 
  // RANSAC特定参数 / RANSAC-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);  
};
 
}  // namespace toolbox::pcl
 
// 包含实现文件 / Include implementation file
#include <cpp-toolbox/pcl/registration/impl/ransac_registration_impl.hpp>