descriptor_distance_sorter.hpp

Go to the documentation of this file.

#pragma once
 
#include <algorithm>
#include <vector>
 
#include <cpp-toolbox/cpp-toolbox_export.hpp>
#include <cpp-toolbox/pcl/correspondence/base_correspondence_sorter.hpp>
 
namespace toolbox::pcl
{
 
template<typename DataType>
class CPP_TOOLBOX_EXPORT descriptor_distance_sorter_t
    : public base_correspondence_sorter_t<
          descriptor_distance_sorter_t<DataType>, DataType>
{
public:
  using base_type =
      base_correspondence_sorter_t<descriptor_distance_sorter_t<DataType>,
                                   DataType>;
  using typename base_type::quality_scores_t;
 
  descriptor_distance_sorter_t() = default;
  ~descriptor_distance_sorter_t() = default;
 
  void set_invert_score(bool invert)
  {
    m_invert = invert;
    this->m_cached = false;  // 参数改变，清除缓存 / Parameter changed, clear
                             // cache
  }
 
  [[nodiscard]] bool get_invert_score() const { return m_invert; }
 
  void set_normalize(bool normalize)
  {
    m_normalize = normalize;
    this->m_cached = false;
  }
 
  [[nodiscard]] bool get_normalize() const { return m_normalize; }
 
protected:
  // Friend declaration to allow base class access
  friend class base_correspondence_sorter_t<
      descriptor_distance_sorter_t<DataType>, DataType>;
 
  void compute_quality_scores_impl(quality_scores_t& scores)
  {
    const auto& corrs = *this->m_correspondences;
 
    if (m_normalize) {
      // 找到最大和最小距离用于归一化 / Find max and min distance for
      // normalization
      DataType max_distance = 0;
      DataType min_distance = std::numeric_limits<DataType>::max();
 
      for (const auto& corr : corrs) {
        max_distance =
            std::max(max_distance, static_cast<DataType>(corr.distance));
        min_distance =
            std::min(min_distance, static_cast<DataType>(corr.distance));
      }
 
      // 避免除零 / Avoid division by zero
      DataType range = max_distance - min_distance;
      if (range < std::numeric_limits<DataType>::epsilon()) {
        range = static_cast<DataType>(1.0);
      }
 
      // 计算归一化的质量分数 / Compute normalized quality scores
      for (std::size_t i = 0; i < corrs.size(); ++i) {
        DataType normalized =
            (corrs[i].distance - min_distance) / range;  // [0, 1]
 
        if (m_invert) {
          // 距离越小，质量越高 / Smaller distance, higher quality
          scores[i] = static_cast<DataType>(1.0) - normalized;
        } else {
          // 距离越大，质量越高（不常用） / Larger distance, higher quality
          // (uncommon)
          scores[i] = normalized;
        }
      }
    } else {
      // 不归一化，直接使用距离 / No normalization, use distance directly
      for (std::size_t i = 0; i < corrs.size(); ++i) {
        if (m_invert) {
          // 使用负距离，这样排序时距离小的会排在前面 / Use negative distance,
          // so smaller distances rank first
          scores[i] = -static_cast<DataType>(corrs[i].distance);
        } else {
          scores[i] = static_cast<DataType>(corrs[i].distance);
        }
      }
    }
  }
 
  [[nodiscard]] std::string get_sorter_name_impl() const
  {
    return "DescriptorDistance";
  }
 
private:
  bool m_invert = true;  
  bool m_normalize = true;  
};
 
}  // namespace toolbox::pcl