pfh_extractor.hpp

Go to the documentation of this file.

#pragma once
 
#include <array>
#include <cmath>
#include <vector>
 
#include <cpp-toolbox/cpp-toolbox_export.hpp>
#include <cpp-toolbox/pcl/descriptors/base_descriptor_extractor.hpp>
#include <cpp-toolbox/pcl/knn/kdtree.hpp>
#include <cpp-toolbox/pcl/norm/pca_norm.hpp>
#include <cpp-toolbox/metrics/vector_metrics.hpp>
 
namespace toolbox::pcl
{
 
template<typename DataType>
struct pfh_signature_t : public base_signature_t<DataType, pfh_signature_t<DataType>>
{
  using value_type = DataType;  // 为了兼容KNN接口 / For KNN interface compatibility
  static constexpr std::size_t HISTOGRAM_SIZE = 125;  // 5 � 5 � 5
  std::array<DataType, HISTOGRAM_SIZE> histogram {};
 
  // 提供data()和size()方法以兼容IMetric接口 / Provide data() and size() for IMetric compatibility
  const DataType* data() const { return histogram.data(); }
  DataType* data() { return histogram.data(); }
  constexpr std::size_t size() const { return HISTOGRAM_SIZE; }
 
  DataType distance_impl(const pfh_signature_t& other) const
  {
    DataType dist = 0;
    for (std::size_t i = 0; i < HISTOGRAM_SIZE; ++i) {
      DataType diff = histogram[i] - other.histogram[i];
      dist += diff * diff;
    }
    return std::sqrt(dist);
  }
};
 
template<typename DataType, 
         typename KNN = kdtree_generic_t<point_t<DataType>, toolbox::metrics::L2Metric<DataType>>>
class CPP_TOOLBOX_EXPORT pfh_extractor_t
    : public base_descriptor_extractor_t<pfh_extractor_t<DataType, KNN>,
                                         DataType,
                                         pfh_signature_t<DataType>>
{
public:
  using base_type = base_descriptor_extractor_t<pfh_extractor_t<DataType, KNN>,
                                                DataType,
                                                pfh_signature_t<DataType>>;
  using data_type = DataType;
  using signature_type = pfh_signature_t<DataType>;
  using knn_type = KNN;
  using point_cloud = toolbox::types::point_cloud_t<DataType>;
  using point_cloud_ptr = std::shared_ptr<point_cloud>;
 
  pfh_extractor_t() = default;
 
  std::size_t set_input(const point_cloud& cloud);
  std::size_t set_input(const point_cloud_ptr& cloud);
 
  std::size_t set_knn(const knn_type& knn);
 
  std::size_t set_search_radius(data_type radius);
 
  std::size_t set_num_neighbors(std::size_t num_neighbors);
 
  void set_normals(const point_cloud_ptr& normals);
 
  void set_num_subdivisions(std::size_t subdivisions);
 
  void enable_parallel_impl(bool enable);
 
  void compute_impl(const point_cloud& cloud,
                    const std::vector<std::size_t>& keypoint_indices,
                    std::vector<signature_type>& descriptors) const;
 
  void compute_impl(
      const point_cloud& cloud,
      const std::vector<std::size_t>& keypoint_indices,
      std::unique_ptr<std::vector<signature_type>>& descriptors) const;
 
private:
  void compute_pfh_feature(const point_cloud& cloud,
                           const point_cloud& normals,
                           std::size_t index,
                           const std::vector<std::size_t>& neighbor_indices,
                           signature_type& pfh) const;
 
  void compute_pair_features(const point_t<data_type>& p1,
                             const point_t<data_type>& n1,
                             const point_t<data_type>& p2,
                             const point_t<data_type>& n2,
                             data_type& f1,
                             data_type& f2,
                             data_type& f3,
                             data_type& f4) const;
 
  std::size_t compute_feature_bin_index(data_type f1,
                                        data_type f2,
                                        data_type f3,
                                        data_type f4) const;
 
  bool m_enable_parallel = false;
  data_type m_search_radius = 0.05;
  std::size_t m_num_neighbors = 30;
  std::size_t m_num_subdivisions = 5;
  point_cloud_ptr m_cloud;
  point_cloud_ptr m_normals;
  knn_type* m_knn = nullptr;
};
 
}  // namespace toolbox::pcl
 
#include <cpp-toolbox/pcl/descriptors/impl/pfh_extractor_impl.hpp>