custom_function_sorter.hpp

Go to the documentation of this file.

#pragma once
 
#include <functional>
#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 custom_function_sorter_t
    : public base_correspondence_sorter_t<custom_function_sorter_t<DataType>,
                                           DataType>
{
public:
  using base_type =
      base_correspondence_sorter_t<custom_function_sorter_t<DataType>,
                                   DataType>;
  using typename base_type::point_cloud;
  using typename base_type::quality_scores_t;
 
  using quality_function_t = std::function<DataType(
      const correspondence_t& correspondence, std::size_t index,
      const point_cloud& source_cloud, const point_cloud& target_cloud)>;
 
  using simple_quality_function_t =
      std::function<DataType(const correspondence_t& correspondence)>;
 
  custom_function_sorter_t() = default;
  ~custom_function_sorter_t() = default;
 
  void set_quality_function(quality_function_t func)
  {
    m_quality_function = std::move(func);
    m_simple_function = nullptr;
    this->m_cached = false;
  }
 
  void set_simple_quality_function(simple_quality_function_t func)
  {
    m_simple_function = std::move(func);
    m_quality_function = nullptr;
    this->m_cached = false;
  }
 
  [[nodiscard]] bool has_quality_function() const
  {
    return m_quality_function || m_simple_function;
  }
 
protected:
  // Friend declaration to allow base class access
  friend class base_correspondence_sorter_t<custom_function_sorter_t<DataType>,
                                            DataType>;
 
  void compute_quality_scores_impl(quality_scores_t& scores)
  {
    if (!has_quality_function()) {
      LOG_ERROR_S
          << "错误：未设置质量函数 / Error: Quality function not set";
      std::fill(scores.begin(), scores.end(), 0);
      return;
    }
 
    const auto& corrs = *this->m_correspondences;
 
    if (m_simple_function) {
      // 使用简化的质量函数 / Use simplified quality function
      for (std::size_t i = 0; i < corrs.size(); ++i) {
        scores[i] = m_simple_function(corrs[i]);
      }
    } else {
      // 使用完整的质量函数 / Use full quality function
      const auto& src_cloud = *this->m_source_cloud;
      const auto& tgt_cloud = *this->m_target_cloud;
 
      for (std::size_t i = 0; i < corrs.size(); ++i) {
        scores[i] = m_quality_function(corrs[i], i, src_cloud, tgt_cloud);
      }
    }
  }
 
  [[nodiscard]] bool validate_input_impl() const
  {
    // 如果使用完整的质量函数，需要点云数据 / If using full quality function,
    // need point cloud data
    if (m_quality_function && (!this->m_source_cloud || !this->m_target_cloud)) {
      LOG_ERROR_S << "错误：使用完整质量函数时需要点云数据 / Error: Point "
                     "cloud data required when using full quality function";
      return false;
    }
    return true;
  }
 
  [[nodiscard]] std::string get_sorter_name_impl() const
  {
    return "CustomFunction";
  }
 
private:
  quality_function_t m_quality_function;  
  simple_quality_function_t
      m_simple_function;  
};
 
}  // namespace toolbox::pcl