40 #ifndef PCL_FEATURES_IMPL_PFHRGB_H_
41 #define PCL_FEATURES_IMPL_PFHRGB_H_
43 #include <pcl/features/pfhrgb.h>
46 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
50 float &f1,
float &f2,
float &f3,
float &f4,
float &f5,
float &f6,
float &f7)
52 Eigen::Vector4i colors1 (cloud[p_idx].r, cloud[p_idx].g, cloud[p_idx].b, 0),
53 colors2 (cloud[q_idx].r, cloud[q_idx].g, cloud[q_idx].b, 0);
56 cloud[q_idx].getVector4fMap (), normals[q_idx].getNormalVector4fMap (),
58 f1, f2, f3, f4, f5, f6, f7);
63 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
66 const pcl::Indices &indices,
int nr_split, Eigen::VectorXf &pfhrgb_histogram)
71 pfhrgb_histogram.setZero ();
74 float hist_incr = 100.0f /
static_cast<float> (indices.size () * (indices.size () - 1) / 2);
77 for (
const auto& index_i: indices)
79 for (
const auto& index_j: indices)
82 if (index_i == index_j)
87 pfhrgb_tuple_[0], pfhrgb_tuple_[1], pfhrgb_tuple_[2], pfhrgb_tuple_[3],
88 pfhrgb_tuple_[4], pfhrgb_tuple_[5], pfhrgb_tuple_[6]))
92 f_index_[0] =
static_cast<int> (std::floor (nr_split * ((pfhrgb_tuple_[0] +
M_PI) * d_pi_)));
94 for (
int i = 1; i < 3; ++i)
96 const float feature_value = nr_split * ((pfhrgb_tuple_[i] + 1.0) * 0.5);
97 f_index_[i] =
static_cast<int> (std::floor (feature_value));
100 for (
int i = 4; i < 7; ++i)
102 const float feature_value = nr_split * ((pfhrgb_tuple_[i] + 1.0) * 0.5);
103 f_index_[i] =
static_cast<int> (std::floor (feature_value));
105 for (
auto& feature: f_index_)
107 feature = std::min(nr_split - 1, std::max(0, feature));
113 for (
int d = 0; d < 3; ++d)
115 h_index += h_p * f_index_[d];
118 pfhrgb_histogram[h_index] += hist_incr;
123 for (
int d = 4; d < 7; ++d)
125 h_index += h_p * f_index_[d];
128 pfhrgb_histogram[h_index] += hist_incr;
134 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
138 pfhrgb_histogram_.setZero (2 * nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
139 pfhrgb_tuple_.setZero (7);
144 std::vector<float> nn_dists (k_);
147 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
149 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
152 computePointPFHRGBSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfhrgb_histogram_);
154 std::copy_n (pfhrgb_histogram_.data (), pfhrgb_histogram_.size (),
155 output[idx].histogram);
159 #define PCL_INSTANTIATE_PFHRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHRGBEstimation<T,NT,OutT>;