base_correspondence_generator.hpp

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <vector>
#include <sstream>
#include <algorithm>
#include <limits>
#include <cmath>
#include <cpp-toolbox/types/point.hpp>
#include <cpp-toolbox/cpp-toolbox_export.hpp>
#include <cpp-toolbox/logger/thread_logger.hpp>
 
// Logger macros
#define LOG_ERROR_S toolbox::logger::thread_logger_t::instance().error_s()
#define LOG_WARN_S toolbox::logger::thread_logger_t::instance().warn_s()
 
namespace toolbox::pcl
{
 
struct correspondence_t
{
  std::size_t src_idx;  
  std::size_t dst_idx;  
  float distance;       
};
 
template<typename Derived, typename DataType, typename Signature>
class CPP_TOOLBOX_EXPORT base_correspondence_generator_t
{
public:
  using point_cloud = toolbox::types::point_cloud_t<DataType>;
  using point_cloud_ptr = std::shared_ptr<point_cloud>;
  using signatures_ptr = std::shared_ptr<std::vector<Signature>>;
  using keypoint_indices_ptr = std::shared_ptr<std::vector<std::size_t>>;
 
  base_correspondence_generator_t() = default;
  virtual ~base_correspondence_generator_t() = default;
 
  // 删除拷贝和移动操作 / Delete copy and move operations
  base_correspondence_generator_t(const base_correspondence_generator_t&) = delete;
  base_correspondence_generator_t& operator=(const base_correspondence_generator_t&) = delete;
  base_correspondence_generator_t(base_correspondence_generator_t&&) = default;
  base_correspondence_generator_t& operator=(base_correspondence_generator_t&&) = default;
 
  void set_source(const point_cloud_ptr& src_cloud,
                  const signatures_ptr& src_descriptors,
                  const keypoint_indices_ptr& src_keypoint_indices)
  {
    m_src_cloud = src_cloud;
    m_src_descriptors = src_descriptors;
    m_src_keypoint_indices = src_keypoint_indices;
  }
 
  void set_destination(const point_cloud_ptr& dst_cloud,
                       const signatures_ptr& dst_descriptors,
                       const keypoint_indices_ptr& dst_keypoint_indices)
  {
    m_dst_cloud = dst_cloud;
    m_dst_descriptors = dst_descriptors;
    m_dst_keypoint_indices = dst_keypoint_indices;
  }
 
  void set_ratio(float ratio) { m_ratio = ratio; }
 
  float get_ratio() const { return m_ratio; }
 
  void set_mutual_verification(bool mutual_verification)
  {
    m_mutual_verification = mutual_verification;
  }
 
  bool get_mutual_verification() const { return m_mutual_verification; }
 
  void set_distance_threshold(float distance_threshold)
  {
    m_distance_threshold = distance_threshold;
  }
 
  float get_distance_threshold() const { return m_distance_threshold; }
 
  void compute(std::vector<correspondence_t>& correspondences)
  {
    // 清空输出和统计信息 / Clear output and statistics
    correspondences.clear();
    reset_statistics();
 
    // 验证输入 / Validate input
    if (!validate_input()) {
      return;
    }
 
    // 调用派生类的实现 / Call derived class implementation
    static_cast<Derived*>(this)->compute_impl(correspondences);
  }
 
  std::string get_statistics() const
  {
    std::stringstream ss;
    ss << "对应点生成统计 / Correspondence Generation Statistics:\n";
    ss << "  候选对应点总数 / Total candidates: " << m_total_candidates << "\n";
    ss << "  通过比率测试 / Passed ratio test: " << m_ratio_test_passed << "\n";
    if (m_mutual_verification) {
      ss << "  通过双向验证 / Passed mutual verification: " << m_mutual_test_passed << "\n";
    }
    ss << "  通过距离阈值 / Passed distance threshold: " << m_distance_test_passed << "\n";
    
    if (m_src_descriptors) {
      float match_rate = static_cast<float>(m_distance_test_passed) / m_src_descriptors->size();
      ss << "  匹配率 / Match rate: " << (match_rate * 100) << "%\n";
    }
    
    return ss.str();
  }
 
protected:
  bool validate_input() const
  {
    // 检查源数据 / Check source data
    if (!m_src_cloud || !m_src_descriptors || !m_src_keypoint_indices) {
      LOG_ERROR_S << "错误：源数据不完整 / Error: Source data incomplete";
      return false;
    }
 
    // 检查目标数据 / Check target data
    if (!m_dst_cloud || !m_dst_descriptors || !m_dst_keypoint_indices) {
      LOG_ERROR_S << "错误：目标数据不完整 / Error: Target data incomplete";
      return false;
    }
 
    // 检查描述子和关键点数量是否匹配 / Check if descriptors and keypoints match
    if (m_src_descriptors->size() != m_src_keypoint_indices->size()) {
      LOG_ERROR_S << "错误：源描述子数量与关键点数量不匹配 / Error: Source descriptor count doesn't match keypoint count";
      return false;
    }
 
    if (m_dst_descriptors->size() != m_dst_keypoint_indices->size()) {
      LOG_ERROR_S << "错误：目标描述子数量与关键点数量不匹配 / Error: Target descriptor count doesn't match keypoint count";
      return false;
    }
 
    // 检查参数有效性 / Check parameter validity
    if (m_ratio <= 0.0f || m_ratio >= 1.0f) {
      LOG_WARN_S << "警告：比率测试阈值应在(0,1)之间 / Warning: Ratio test threshold should be in (0,1)";
    }
 
    return true;
  }
 
  bool apply_ratio_test(const std::vector<correspondence_t>& candidates) const
  {
    // Lowe's ratio test: 检查最佳匹配是否显著优于次佳匹配 / Check if best match is significantly better than second best
    if (candidates.size() < 2) {
      return true;  // 如果只有一个匹配，默认通过 / If only one match, pass by default
    }
    
    return (candidates[0].distance < m_ratio * candidates[1].distance);
  }
 
  void apply_distance_threshold(std::vector<correspondence_t>& correspondences) const
  {
    // 移除距离超过阈值的对应关系 / Remove correspondences exceeding distance threshold
    correspondences.erase(
        std::remove_if(correspondences.begin(), correspondences.end(),
                       [this](const correspondence_t& corr) {
                         return corr.distance > m_distance_threshold;
                       }),
        correspondences.end());
  }
 
  float compute_geometric_consistency(const std::vector<correspondence_t>& correspondences) const
  {
    if (correspondences.size() < 3) {
      return 0.0f;
    }
    
    // 简单的几何一致性检查：比较点对之间的距离比率 / Simple geometric consistency: compare distance ratios between point pairs
    float consistency_score = 0.0f;
    std::size_t valid_pairs = 0;
    
    for (std::size_t i = 0; i < correspondences.size() - 1; ++i) {
      for (std::size_t j = i + 1; j < correspondences.size(); ++j) {
        // 计算源点对之间的距离 / Compute distance between source point pair
        const auto& src_p1 = m_src_cloud->points[correspondences[i].src_idx];
        const auto& src_p2 = m_src_cloud->points[correspondences[j].src_idx];
        DataType src_dist = std::sqrt(
            (src_p1.x - src_p2.x) * (src_p1.x - src_p2.x) +
            (src_p1.y - src_p2.y) * (src_p1.y - src_p2.y) +
            (src_p1.z - src_p2.z) * (src_p1.z - src_p2.z));
        
        // 计算目标点对之间的距离 / Compute distance between target point pair
        const auto& dst_p1 = m_dst_cloud->points[correspondences[i].dst_idx];
        const auto& dst_p2 = m_dst_cloud->points[correspondences[j].dst_idx];
        DataType dst_dist = std::sqrt(
            (dst_p1.x - dst_p2.x) * (dst_p1.x - dst_p2.x) +
            (dst_p1.y - dst_p2.y) * (dst_p1.y - dst_p2.y) +
            (dst_p1.z - dst_p2.z) * (dst_p1.z - dst_p2.z));
        
        // 避免除零 / Avoid division by zero
        if (src_dist > 0.001 && dst_dist > 0.001) {
          DataType ratio = src_dist / dst_dist;
          // 如果比率接近1，说明几何关系保持良好 / If ratio is close to 1, geometry is well preserved
          if (ratio > 0.8 && ratio < 1.2) {
            consistency_score += 1.0f;
          }
          valid_pairs++;
        }
      }
    }
    
    return valid_pairs > 0 ? consistency_score / valid_pairs : 0.0f;
  }
 
  void reset_statistics()
  {
    m_total_candidates = 0;
    m_ratio_test_passed = 0;
    m_mutual_test_passed = 0;
    m_distance_test_passed = 0;
  }
 
  // 数据成员 / Data members
  point_cloud_ptr m_src_cloud;
  point_cloud_ptr m_dst_cloud;
  signatures_ptr m_src_descriptors;
  signatures_ptr m_dst_descriptors;
  keypoint_indices_ptr m_src_keypoint_indices;
  keypoint_indices_ptr m_dst_keypoint_indices;
 
  float m_ratio = 0.8F;  
  float m_distance_threshold = std::numeric_limits<float>::max();  
  bool m_mutual_verification = true;  
 
  // 统计信息 / Statistics
  mutable std::size_t m_total_candidates = 0;
  mutable std::size_t m_ratio_test_passed = 0;
  mutable std::size_t m_mutual_test_passed = 0;
  mutable std::size_t m_distance_test_passed = 0;
};
 
}  // namespace toolbox::pcl