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.
points[p_idx].r, cloud.
points[p_idx].g, cloud.
points[p_idx].b, 0),
56 cloud.
points[q_idx].getVector4fMap (), normals.
points[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 std::vector<int> &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 (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
79 for (
size_t j_idx = 0; j_idx < indices.size (); ++j_idx)
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> (floor (nr_split * ((pfhrgb_tuple_[0] + M_PI) * d_pi_)));
93 if (f_index_[0] < 0) f_index_[0] = 0;
94 if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
96 f_index_[1] =
static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[1] + 1.0) * 0.5)));
97 if (f_index_[1] < 0) f_index_[1] = 0;
98 if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
100 f_index_[2] =
static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[2] + 1.0) * 0.5)));
101 if (f_index_[2] < 0) f_index_[2] = 0;
102 if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
105 f_index_[4] =
static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[4] + 1.0) * 0.5)));
106 if (f_index_[4] < 0) f_index_[4] = 0;
107 if (f_index_[4] >= nr_split) f_index_[4] = nr_split - 1;
109 f_index_[5] =
static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[5] + 1.0) * 0.5)));
110 if (f_index_[5] < 0) f_index_[5] = 0;
111 if (f_index_[5] >= nr_split) f_index_[5] = nr_split - 1;
113 f_index_[6] =
static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[6] + 1.0) * 0.5)));
114 if (f_index_[6] < 0) f_index_[6] = 0;
115 if (f_index_[6] >= nr_split) f_index_[6] = nr_split - 1;
121 for (
int d = 0; d < 3; ++d)
123 h_index += h_p * f_index_[d];
126 pfhrgb_histogram[h_index] += hist_incr;
131 for (
int d = 4; d < 7; ++d)
133 h_index += h_p * f_index_[d];
136 pfhrgb_histogram[h_index] += hist_incr;
142 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 146 pfhrgb_histogram_.setZero (2 * nr_subdiv_ * nr_subdiv_ * nr_subdiv_);
147 pfhrgb_tuple_.setZero (7);
151 std::vector<int> nn_indices (k_);
152 std::vector<float> nn_dists (k_);
155 for (
size_t idx = 0; idx < indices_->size (); ++idx)
157 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
160 computePointPFHRGBSignature (*surface_, *normals_, nn_indices, nr_subdiv_, pfhrgb_histogram_);
163 for (
int d = 0; d < pfhrgb_histogram_.size (); ++d) {
164 output.
points[idx].histogram[d] = pfhrgb_histogram_[d];
171 #define PCL_INSTANTIATE_PFHRGBEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::PFHRGBEstimation<T,NT,OutT>; std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PCL_EXPORTS bool computeRGBPairFeatures(const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &colors1, const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &colors2, float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
void computePointPFHRGBSignature(const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, const std::vector< int > &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram)
void computeFeature(PointCloudOut &output)
Abstract feature estimation method.
bool computeRGBPairFeatures(const pcl::PointCloud< PointInT > &cloud, const pcl::PointCloud< PointNT > &normals, int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)