shot_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/math/matrix.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 shot_signature_t : public base_signature_t<DataType, shot_signature_t<DataType>>
{
  using value_type = DataType;  // 为了兼容KNN接口 / For KNN interface compatibility
  static constexpr std::size_t HISTOGRAM_SIZE = 352;  // 32 spatial � 11 angular
  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 shot_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 shot_extractor_t
    : public base_descriptor_extractor_t<shot_extractor_t<DataType, KNN>,
                                         DataType,
                                         shot_signature_t<DataType>>
{
public:
  using base_type = base_descriptor_extractor_t<shot_extractor_t<DataType, KNN>,
                                                DataType,
                                                shot_signature_t<DataType>>;
  using data_type = DataType;
  using signature_type = shot_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>;
 
  shot_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 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:
  struct local_rf_t
  {
    point_t<data_type> x_axis;
    point_t<data_type> y_axis;
    point_t<data_type> z_axis;
  };
 
  void compute_local_reference_frame(
      const point_cloud& cloud,
      const point_cloud& normals,
      std::size_t index,
      const std::vector<std::size_t>& neighbor_indices,
      local_rf_t& lrf) const;
 
  void compute_shot_feature(const point_cloud& cloud,
                            const point_cloud& normals,
                            std::size_t index,
                            const std::vector<std::size_t>& neighbor_indices,
                            const local_rf_t& lrf,
                            signature_type& shot) const;
 
  void compute_weighted_covariance(const point_cloud& cloud,
                                   std::size_t center_idx,
                                   const std::vector<std::size_t>& indices,
                                   const std::vector<data_type>& weights,
                                   data_type cov[9]) const;
 
  std::size_t compute_spatial_bin(const point_t<data_type>& point,
                                  const point_t<data_type>& center,
                                  const local_rf_t& lrf,
                                  data_type radius) const;
 
  std::size_t compute_angular_bin(const point_t<data_type>& normal,
                                  const local_rf_t& lrf) const;
 
  bool m_enable_parallel = false;
  data_type m_search_radius = 0.1;
  std::size_t m_num_neighbors = 100;
  point_cloud_ptr m_cloud;
  point_cloud_ptr m_normals;
  knn_type* m_knn = nullptr;
};
 
}  // namespace toolbox::pcl
 
#include <cpp-toolbox/pcl/descriptors/impl/shot_extractor_impl.hpp>