40 #ifndef PCL_ROPS_ESTIMATION_HPP_ 41 #define PCL_ROPS_ESTIMATION_HPP_ 43 #include <pcl/features/rops_estimation.h> 48 template <
typename Po
intInT,
typename Po
intOutT>
51 number_of_rotations_ (3),
52 support_radius_ (1.0f),
53 sqr_support_radius_ (1.0f),
56 triangles_of_the_point_ (0)
61 template <
typename Po
intInT,
typename Po
intOutT>
65 triangles_of_the_point_.clear ();
69 template <
typename Po
intInT,
typename Po
intOutT>
void 72 if (number_of_bins != 0)
73 number_of_bins_ = number_of_bins;
77 template <
typename Po
intInT,
typename Po
intOutT>
unsigned int 80 return (number_of_bins_);
84 template <
typename Po
intInT,
typename Po
intOutT>
void 87 if (number_of_rotations != 0)
89 number_of_rotations_ = number_of_rotations;
90 step_ = 90.0f / static_cast <
float> (number_of_rotations_ + 1);
95 template <
typename Po
intInT,
typename Po
intOutT>
unsigned int 98 return (number_of_rotations_);
102 template <
typename Po
intInT,
typename Po
intOutT>
void 105 if (support_radius > 0.0f)
107 support_radius_ = support_radius;
108 sqr_support_radius_ = support_radius * support_radius;
113 template <
typename Po
intInT,
typename Po
intOutT>
float 116 return (support_radius_);
120 template <
typename Po
intInT,
typename Po
intOutT>
void 123 triangles_ = triangles;
127 template <
typename Po
intInT,
typename Po
intOutT>
void 130 triangles = triangles_;
134 template <
typename Po
intInT,
typename Po
intOutT>
void 137 if (triangles_.empty ())
139 output.points.clear ();
143 buildListOfPointsTriangles ();
146 unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
147 const auto number_of_points = indices_->size ();
148 output.points.clear ();
149 output.points.reserve (number_of_points);
151 for (
const auto& idx: *indices_)
153 std::set <unsigned int> local_triangles;
154 std::vector <int> local_points;
155 getLocalSurface ((*input_)[idx], local_triangles, local_points);
157 Eigen::Matrix3f lrf_matrix;
158 computeLRF ((*input_)[idx], local_triangles, lrf_matrix);
160 PointCloudIn transformed_cloud;
161 transformCloud ((*input_)[idx], lrf_matrix, local_points, transformed_cloud);
163 std::array<PointInT, 3> axes;
164 axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f;
165 axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f;
166 axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f;
167 std::vector <float> feature;
168 for (
const auto &axis : axes)
174 PointCloudIn rotated_cloud;
175 Eigen::Vector3f min, max;
176 rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
179 for (
unsigned int i_proj = 0; i_proj < 3; i_proj++)
181 Eigen::MatrixXf distribution_matrix;
182 distribution_matrix.resize (number_of_bins_, number_of_bins_);
183 getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
186 std::vector <float> moments;
187 computeCentralMoments (distribution_matrix, moments);
189 feature.insert (feature.end (), moments.begin (), moments.end ());
193 }
while (theta < 90.0f);
196 const float norm = std::accumulate(
197 feature.cbegin(), feature.cend(), 0.f, [](
const auto& sum,
const auto& val) {
198 return sum + std::abs(val);
201 if (norm < std::numeric_limits <float>::epsilon ())
204 invert_norm = 1.0f /
norm;
206 output.points.emplace_back ();
207 for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
208 output.points.back().histogram[i_dim] = feature[i_dim] * invert_norm;
213 template <
typename Po
intInT,
typename Po
intOutT>
void 216 triangles_of_the_point_.clear ();
218 std::vector <unsigned int> dummy;
220 triangles_of_the_point_.resize (surface_->points. size (), dummy);
222 for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++)
223 for (
const auto& vertex: triangles_[i_triangle].vertices)
224 triangles_of_the_point_[vertex].push_back (i_triangle);
228 template <
typename Po
intInT,
typename Po
intOutT>
void 231 std::vector <float> distances;
232 tree_->radiusSearch (point, support_radius_, local_points, distances);
234 for (
const auto& pt: local_points)
235 local_triangles.insert (triangles_of_the_point_[pt].begin (),
236 triangles_of_the_point_[pt].end ());
240 template <
typename Po
intInT,
typename Po
intOutT>
void 243 std::size_t number_of_triangles = local_triangles.size ();
245 std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices;
246 std::vector <float> triangle_area (number_of_triangles), distance_weight (number_of_triangles);
248 scatter_matrices.reserve (number_of_triangles);
249 triangle_area.clear ();
250 distance_weight.clear ();
252 float total_area = 0.0f;
253 const float coeff = 1.0f / 12.0f;
254 const float coeff_1_div_3 = 1.0f / 3.0f;
256 Eigen::Vector3f feature_point (point.x, point.y, point.z);
258 for (
const auto& triangle: local_triangles)
260 Eigen::Vector3f pt[3];
261 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
263 const unsigned int index = triangles_[triangle].vertices[i_vertex];
264 pt[i_vertex] (0) = (*surface_)[index].x;
265 pt[i_vertex] (1) = (*surface_)[index].y;
266 pt[i_vertex] (2) = (*surface_)[index].z;
269 const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
270 triangle_area.push_back (curr_area);
271 total_area += curr_area;
273 distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
275 Eigen::Matrix3f curr_scatter_matrix;
276 curr_scatter_matrix.setZero ();
277 for (
const auto &i_pt : pt)
279 Eigen::Vector3f vec = i_pt - feature_point;
280 curr_scatter_matrix += vec * (vec.transpose ());
281 for (
const auto &j_pt : pt)
282 curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
284 scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
287 if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
288 total_area = 1.0f / total_area;
292 Eigen::Matrix3f overall_scatter_matrix;
293 overall_scatter_matrix.setZero ();
294 std::vector<float> total_weight (number_of_triangles);
295 const float denominator = 1.0f / 6.0f;
296 for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
298 const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
299 overall_scatter_matrix += factor * scatter_matrices[i_triangle];
300 total_weight[i_triangle] = factor * denominator;
303 Eigen::Vector3f v1, v2, v3;
304 computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
308 std::size_t i_triangle = 0;
309 for (
const auto& triangle: local_triangles)
311 Eigen::Vector3f pt[3];
312 for (
unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
314 const unsigned int index = triangles_[triangle].vertices[i_vertex];
315 pt[i_vertex] (0) = (*surface_)[index].x;
316 pt[i_vertex] (1) = (*surface_)[index].y;
317 pt[i_vertex] (2) = (*surface_)[index].z;
320 float factor1 = 0.0f;
321 float factor3 = 0.0f;
322 for (
const auto &i_pt : pt)
324 Eigen::Vector3f vec = i_pt - feature_point;
325 factor1 += vec.dot (v1);
326 factor3 += vec.dot (v3);
328 h1 += total_weight[i_triangle] * factor1;
329 h3 += total_weight[i_triangle] * factor3;
333 if (h1 < 0.0f) v1 = -v1;
334 if (h3 < 0.0f) v3 = -v3;
338 lrf_matrix.row (0) = v1;
339 lrf_matrix.row (1) = v2;
340 lrf_matrix.row (2) = v3;
344 template <
typename Po
intInT,
typename Po
intOutT>
void 346 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis)
const 348 Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
351 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
352 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
353 eigen_vectors = eigen_solver.eigenvectors ();
354 eigen_values = eigen_solver.eigenvalues ();
356 unsigned int temp = 0;
357 unsigned int major_index = 0;
358 unsigned int middle_index = 1;
359 unsigned int minor_index = 2;
361 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
364 major_index = middle_index;
368 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
371 major_index = minor_index;
375 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
378 minor_index = middle_index;
382 major_axis = eigen_vectors.col (major_index).real ();
383 middle_axis = eigen_vectors.col (middle_index).real ();
384 minor_axis = eigen_vectors.col (minor_index).real ();
388 template <
typename Po
intInT,
typename Po
intOutT>
void 391 const auto number_of_points = local_points.size ();
392 transformed_cloud.points.clear ();
393 transformed_cloud.points.reserve (number_of_points);
395 for (
const auto& idx: local_points)
397 Eigen::Vector3f transformed_point ((*surface_)[idx].x - point.x,
398 (*surface_)[idx].y - point.y,
399 (*surface_)[idx].z - point.z);
401 transformed_point = matrix * transformed_point;
404 new_point.x = transformed_point (0);
405 new_point.y = transformed_point (1);
406 new_point.z = transformed_point (2);
407 transformed_cloud.points.emplace_back (new_point);
412 template <
typename Po
intInT,
typename Po
intOutT>
void 415 Eigen::Matrix3f rotation_matrix;
416 const float x = axis.x;
417 const float y = axis.y;
418 const float z = axis.z;
419 const float rad =
M_PI / 180.0f;
420 const float cosine = std::cos (angle * rad);
421 const float sine = std::sin (angle * rad);
422 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
423 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
424 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
426 const auto number_of_points = cloud.size ();
428 rotated_cloud.header = cloud.header;
429 rotated_cloud.width = number_of_points;
430 rotated_cloud.height = 1;
431 rotated_cloud.points.clear ();
432 rotated_cloud.points.reserve (number_of_points);
434 min (0) = std::numeric_limits <float>::max ();
435 min (1) = std::numeric_limits <float>::max ();
436 min (2) = std::numeric_limits <float>::max ();
437 max (0) = -std::numeric_limits <float>::max ();
438 max (1) = -std::numeric_limits <float>::max ();
439 max (2) = -std::numeric_limits <float>::max ();
441 for (
const auto& pt: cloud.points)
443 Eigen::Vector3f point (pt.x, pt.y, pt.z);
444 point = rotation_matrix * point;
446 PointInT rotated_point;
447 rotated_point.x = point (0);
448 rotated_point.y = point (1);
449 rotated_point.z = point (2);
450 rotated_cloud.points.emplace_back (rotated_point);
452 for (
int i = 0; i < 3; ++i)
454 min(i) = std::min(min(i), point(i));
455 max(i) = std::max(max(i), point(i));
461 template <
typename Po
intInT,
typename Po
intOutT>
void 466 const unsigned int coord[3][2] = {
471 const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
472 const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
474 for (
const auto& pt: cloud.points)
476 Eigen::Vector3f point (pt.x, pt.y, pt.z);
478 const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
479 const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
481 const float u_ratio = u_length / u_bin_length;
482 unsigned int row = static_cast <
unsigned int> (u_ratio);
483 if (row == number_of_bins_) row--;
485 const float v_ratio = v_length / v_bin_length;
486 unsigned int col = static_cast <
unsigned int> (v_ratio);
487 if (col == number_of_bins_) col--;
489 matrix (row, col) += 1.0f;
492 matrix /= std::max<float> (1, cloud.size ());
496 template <
typename Po
intInT,
typename Po
intOutT>
void 502 for (
unsigned int i = 0; i < number_of_bins_; i++)
503 for (
unsigned int j = 0; j < number_of_bins_; j++)
505 const float m = matrix (i, j);
506 mean_i += static_cast <
float> (i + 1) * m;
507 mean_j += static_cast <
float> (j + 1) * m;
510 const unsigned int number_of_moments_to_compute = 4;
511 const float power[number_of_moments_to_compute][2] = {
517 float entropy = 0.0f;
518 moments.resize (number_of_moments_to_compute + 1, 0.0f);
519 for (
unsigned int i = 0; i < number_of_bins_; i++)
521 const float i_factor = static_cast <
float> (i + 1) - mean_i;
522 for (
unsigned int j = 0; j < number_of_bins_; j++)
524 const float j_factor = static_cast <
float> (j + 1) - mean_j;
525 const float m = matrix (i, j);
527 entropy -= m * std::log (m);
528 for (
unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
529 moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m;
533 moments[number_of_moments_to_compute] = entropy;
536 #endif // PCL_ROPS_ESTIMATION_HPP_ This class implements the method for extracting RoPS features presented in the article "Rotational Pr...
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...