Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
rops_estimation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 *
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author : Sergey Ushakov
36 * Email : sergey.s.ushakov@mail.ru
37 *
38 */
39
40#ifndef PCL_ROPS_ESTIMATION_HPP_
41#define PCL_ROPS_ESTIMATION_HPP_
42
43#include <pcl/features/rops_estimation.h>
44
45#include <array>
46#include <numeric> // for accumulate
47#include <Eigen/Eigenvalues> // for EigenSolver
48
49//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50template <typename PointInT, typename PointOutT>
52 number_of_bins_ (5),
53 number_of_rotations_ (3),
54 support_radius_ (1.0f),
55 sqr_support_radius_ (1.0f),
56 step_ (22.5f),
57 triangles_ (0),
58 triangles_of_the_point_ (0)
59{
60}
61
62//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
63template <typename PointInT, typename PointOutT>
64pcl::ROPSEstimation <PointInT, PointOutT>::~ROPSEstimation ()
65{
66 triangles_.clear ();
67 triangles_of_the_point_.clear ();
68}
69
70//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71template <typename PointInT, typename PointOutT> void
73{
74 if (number_of_bins != 0)
75 number_of_bins_ = number_of_bins;
76}
77
78//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
79template <typename PointInT, typename PointOutT> unsigned int
81{
82 return (number_of_bins_);
83}
84
85//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86template <typename PointInT, typename PointOutT> void
88{
89 if (number_of_rotations != 0)
90 {
91 number_of_rotations_ = number_of_rotations;
92 step_ = 90.0f / static_cast <float> (number_of_rotations_ + 1);
93 }
94}
95
96//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
97template <typename PointInT, typename PointOutT> unsigned int
99{
100 return (number_of_rotations_);
101}
102
103//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104template <typename PointInT, typename PointOutT> void
106{
107 if (support_radius > 0.0f)
108 {
109 support_radius_ = support_radius;
110 sqr_support_radius_ = support_radius * support_radius;
111 }
112}
113
114//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
115template <typename PointInT, typename PointOutT> float
117{
118 return (support_radius_);
119}
120
121//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
122template <typename PointInT, typename PointOutT> void
123pcl::ROPSEstimation <PointInT, PointOutT>::setTriangles (const std::vector <pcl::Vertices>& triangles)
124{
125 triangles_ = triangles;
126}
127
128//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
129template <typename PointInT, typename PointOutT> void
130pcl::ROPSEstimation <PointInT, PointOutT>::getTriangles (std::vector <pcl::Vertices>& triangles) const
131{
132 triangles = triangles_;
133}
134
135//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
136template <typename PointInT, typename PointOutT> void
137pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output)
138{
139 if (triangles_.empty ())
140 {
141 output.clear ();
142 return;
143 }
144
145 buildListOfPointsTriangles ();
146
147 //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
148 unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
149 const auto number_of_points = indices_->size ();
150 output.clear ();
151 output.reserve (number_of_points);
152
153 for (const auto& idx: *indices_)
154 {
155 std::set <unsigned int> local_triangles;
156 pcl::Indices local_points;
157 getLocalSurface ((*input_)[idx], local_triangles, local_points);
158
159 Eigen::Matrix3f lrf_matrix;
160 computeLRF ((*input_)[idx], local_triangles, lrf_matrix);
161
162 PointCloudIn transformed_cloud;
163 transformCloud ((*input_)[idx], lrf_matrix, local_points, transformed_cloud);
164
165 std::array<PointInT, 3> axes;
166 axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f;
167 axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f;
168 axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f;
169 std::vector <float> feature;
170 for (const auto &axis : axes)
171 {
172 float theta = step_;
173 do
174 {
175 //rotate local surface and get bounding box
176 PointCloudIn rotated_cloud;
177 Eigen::Vector3f min, max;
178 rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
179
180 //for each projection (XY, XZ and YZ) compute distribution matrix and central moments
181 for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
182 {
183 Eigen::MatrixXf distribution_matrix;
184 distribution_matrix.resize (number_of_bins_, number_of_bins_);
185 getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
186
187 // TODO remove this needless copy due to API design
188 std::vector <float> moments;
189 computeCentralMoments (distribution_matrix, moments);
190
191 feature.insert (feature.end (), moments.begin (), moments.end ());
192 }
193
194 theta += step_;
195 } while (theta < 90.0f);
196 }
197
198 const float norm = std::accumulate(
199 feature.cbegin(), feature.cend(), 0.f, [](const auto& sum, const auto& val) {
200 return sum + std::abs(val);
201 });
202 float invert_norm;
203 if (norm < std::numeric_limits <float>::epsilon ())
204 invert_norm = 1.0f;
205 else
206 invert_norm = 1.0f / norm;
207
208 output.emplace_back ();
209 for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
210 output.back().histogram[i_dim] = feature[i_dim] * invert_norm;
211 }
212}
213
214//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
215template <typename PointInT, typename PointOutT> void
216pcl::ROPSEstimation <PointInT, PointOutT>::buildListOfPointsTriangles ()
217{
218 triangles_of_the_point_.clear ();
219
220 std::vector <unsigned int> dummy;
221 dummy.reserve (100);
222 triangles_of_the_point_.resize (surface_->points. size (), dummy);
223
224 for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++)
225 for (const auto& vertex: triangles_[i_triangle].vertices)
226 triangles_of_the_point_[vertex].push_back (i_triangle);
227}
228
229//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
230template <typename PointInT, typename PointOutT> void
231pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, pcl::Indices& local_points) const
232{
233 std::vector <float> distances;
234 tree_->radiusSearch (point, support_radius_, local_points, distances);
235
236 for (const auto& pt: local_points)
237 local_triangles.insert (triangles_of_the_point_[pt].begin (),
238 triangles_of_the_point_[pt].end ());
239}
240
241//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
242template <typename PointInT, typename PointOutT> void
243pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
244{
245 std::size_t number_of_triangles = local_triangles.size ();
246
247 std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices;
248 std::vector <float> triangle_area (number_of_triangles), distance_weight (number_of_triangles);
249
250 scatter_matrices.reserve (number_of_triangles);
251 triangle_area.clear ();
252 distance_weight.clear ();
253
254 float total_area = 0.0f;
255 const float coeff = 1.0f / 12.0f;
256 const float coeff_1_div_3 = 1.0f / 3.0f;
257
258 Eigen::Vector3f feature_point (point.x, point.y, point.z);
259
260 for (const auto& triangle: local_triangles)
261 {
262 Eigen::Vector3f pt[3];
263 for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
264 {
265 const unsigned int index = triangles_[triangle].vertices[i_vertex];
266 pt[i_vertex] (0) = (*surface_)[index].x;
267 pt[i_vertex] (1) = (*surface_)[index].y;
268 pt[i_vertex] (2) = (*surface_)[index].z;
269 }
270
271 const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
272 triangle_area.push_back (curr_area);
273 total_area += curr_area;
274
275 distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
276
277 Eigen::Matrix3f curr_scatter_matrix;
278 curr_scatter_matrix.setZero ();
279 for (const auto &i_pt : pt)
280 {
281 Eigen::Vector3f vec = i_pt - feature_point;
282 curr_scatter_matrix += vec * (vec.transpose ());
283 for (const auto &j_pt : pt)
284 curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
285 }
286 scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
287 }
288
289 if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
290 total_area = 1.0f / total_area;
291 else
292 total_area = 1.0f;
293
294 Eigen::Matrix3f overall_scatter_matrix;
295 overall_scatter_matrix.setZero ();
296 std::vector<float> total_weight (number_of_triangles);
297 const float denominator = 1.0f / 6.0f;
298 for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
299 {
300 const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
301 overall_scatter_matrix += factor * scatter_matrices[i_triangle];
302 total_weight[i_triangle] = factor * denominator;
303 }
304
305 Eigen::Vector3f v1, v2, v3;
306 computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
307
308 float h1 = 0.0f;
309 float h3 = 0.0f;
310 std::size_t i_triangle = 0;
311 for (const auto& triangle: local_triangles)
312 {
313 Eigen::Vector3f pt[3];
314 for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
315 {
316 const unsigned int index = triangles_[triangle].vertices[i_vertex];
317 pt[i_vertex] (0) = (*surface_)[index].x;
318 pt[i_vertex] (1) = (*surface_)[index].y;
319 pt[i_vertex] (2) = (*surface_)[index].z;
320 }
321
322 float factor1 = 0.0f;
323 float factor3 = 0.0f;
324 for (const auto &i_pt : pt)
325 {
326 Eigen::Vector3f vec = i_pt - feature_point;
327 factor1 += vec.dot (v1);
328 factor3 += vec.dot (v3);
329 }
330 h1 += total_weight[i_triangle] * factor1;
331 h3 += total_weight[i_triangle] * factor3;
332 i_triangle++;
333 }
334
335 if (h1 < 0.0f) v1 = -v1;
336 if (h3 < 0.0f) v3 = -v3;
337
338 v2 = v3.cross (v1);
339
340 lrf_matrix.row (0) = v1;
341 lrf_matrix.row (1) = v2;
342 lrf_matrix.row (2) = v3;
343}
344
345//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
346template <typename PointInT, typename PointOutT> void
347pcl::ROPSEstimation <PointInT, PointOutT>::computeEigenVectors (const Eigen::Matrix3f& matrix,
348 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const
349{
350 Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
351 eigen_solver.compute (matrix);
352
353 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
354 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
355 eigen_vectors = eigen_solver.eigenvectors ();
356 eigen_values = eigen_solver.eigenvalues ();
357
358 unsigned int temp = 0;
359 unsigned int major_index = 0;
360 unsigned int middle_index = 1;
361 unsigned int minor_index = 2;
362
363 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
364 {
365 temp = major_index;
366 major_index = middle_index;
367 middle_index = temp;
368 }
369
370 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
371 {
372 temp = major_index;
373 major_index = minor_index;
374 minor_index = temp;
375 }
376
377 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
378 {
379 temp = minor_index;
380 minor_index = middle_index;
381 middle_index = temp;
382 }
383
384 major_axis = eigen_vectors.col (major_index).real ();
385 middle_axis = eigen_vectors.col (middle_index).real ();
386 minor_axis = eigen_vectors.col (minor_index).real ();
387}
388
389//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
390template <typename PointInT, typename PointOutT> void
391pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const pcl::Indices& local_points, PointCloudIn& transformed_cloud) const
392{
393 const auto number_of_points = local_points.size ();
394 transformed_cloud.clear ();
395 transformed_cloud.reserve (number_of_points);
396
397 for (const auto& idx: local_points)
398 {
399 Eigen::Vector3f transformed_point ((*surface_)[idx].x - point.x,
400 (*surface_)[idx].y - point.y,
401 (*surface_)[idx].z - point.z);
402
403 transformed_point = matrix * transformed_point;
404
405 PointInT new_point;
406 new_point.x = transformed_point (0);
407 new_point.y = transformed_point (1);
408 new_point.z = transformed_point (2);
409 transformed_cloud.emplace_back (new_point);
410 }
411}
412
413//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
414template <typename PointInT, typename PointOutT> void
415pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const
416{
417 Eigen::Matrix3f rotation_matrix;
418 const float x = axis.x;
419 const float y = axis.y;
420 const float z = axis.z;
421 const float rad = M_PI / 180.0f;
422 const float cosine = std::cos (angle * rad);
423 const float sine = std::sin (angle * rad);
424 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
425 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
426 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
427
428 const auto number_of_points = cloud.size ();
429
430 rotated_cloud.header = cloud.header;
431 rotated_cloud.width = number_of_points;
432 rotated_cloud.height = 1;
433 rotated_cloud.clear ();
434 rotated_cloud.reserve (number_of_points);
435
436 min (0) = std::numeric_limits <float>::max ();
437 min (1) = std::numeric_limits <float>::max ();
438 min (2) = std::numeric_limits <float>::max ();
439 max (0) = -std::numeric_limits <float>::max ();
440 max (1) = -std::numeric_limits <float>::max ();
441 max (2) = -std::numeric_limits <float>::max ();
442
443 for (const auto& pt: cloud.points)
444 {
445 Eigen::Vector3f point (pt.x, pt.y, pt.z);
446 point = rotation_matrix * point;
447
448 PointInT rotated_point;
449 rotated_point.x = point (0);
450 rotated_point.y = point (1);
451 rotated_point.z = point (2);
452 rotated_cloud.emplace_back (rotated_point);
453
454 for (int i = 0; i < 3; ++i)
455 {
456 min(i) = std::min(min(i), point(i));
457 max(i) = std::max(max(i), point(i));
458 }
459 }
460}
461
462//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
463template <typename PointInT, typename PointOutT> void
464pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
465{
466 matrix.setZero ();
467
468 const unsigned int coord[3][2] = {
469 {0, 1},
470 {0, 2},
471 {1, 2}};
472
473 const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
474 const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
475
476 for (const auto& pt: cloud.points)
477 {
478 Eigen::Vector3f point (pt.x, pt.y, pt.z);
479
480 const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
481 const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
482
483 const float u_ratio = u_length / u_bin_length;
484 auto row = static_cast <unsigned int> (u_ratio);
485 if (row == number_of_bins_) row--;
486
487 const float v_ratio = v_length / v_bin_length;
488 auto col = static_cast <unsigned int> (v_ratio);
489 if (col == number_of_bins_) col--;
490
491 matrix (row, col) += 1.0f;
492 }
493
494 matrix /= std::max<float> (1, cloud.size ());
495}
496
497//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
498template <typename PointInT, typename PointOutT> void
499pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector <float>& moments) const
500{
501 float mean_i = 0.0f;
502 float mean_j = 0.0f;
503
504 for (unsigned int i = 0; i < number_of_bins_; i++)
505 for (unsigned int j = 0; j < number_of_bins_; j++)
506 {
507 const float m = matrix (i, j);
508 mean_i += static_cast <float> (i + 1) * m;
509 mean_j += static_cast <float> (j + 1) * m;
510 }
511
512 const unsigned int number_of_moments_to_compute = 4;
513 const float power[number_of_moments_to_compute][2] = {
514 {1.0f, 1.0f},
515 {2.0f, 1.0f},
516 {1.0f, 2.0f},
517 {2.0f, 2.0f}};
518
519 float entropy = 0.0f;
520 moments.resize (number_of_moments_to_compute + 1, 0.0f);
521 for (unsigned int i = 0; i < number_of_bins_; i++)
522 {
523 const float i_factor = static_cast <float> (i + 1) - mean_i;
524 for (unsigned int j = 0; j < number_of_bins_; j++)
525 {
526 const float j_factor = static_cast <float> (j + 1) - mean_j;
527 const float m = matrix (i, j);
528 if (m > 0.0f)
529 entropy -= m * std::log (m);
530 for (unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
531 moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m;
532 }
533 }
534
535 moments[number_of_moments_to_compute] = entropy;
536}
537
538#endif // PCL_ROPS_ESTIMATION_HPP_
float getSupportRadius() const
Returns the support radius.
void setNumberOfRotations(unsigned int number_of_rotations)
This method sets the number of rotations.
ROPSEstimation()
Simple constructor.
void setNumberOfPartitionBins(unsigned int number_of_bins)
Allows to set the number of partition bins that is used for distribution matrix calculation.
unsigned int getNumberOfPartitionBins() const
Returns the number of partition bins.
unsigned int getNumberOfRotations() const
returns the number of rotations.
void setSupportRadius(float support_radius)
Allows to set the support radius that is used to crop the local surface of the point.
void setTriangles(const std::vector< pcl::Vertices > &triangles)
This method sets the triangles of the mesh.
void getTriangles(std::vector< pcl::Vertices > &triangles) const
Returns the triangles of the mesh.
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
#define M_PI
Definition pcl_macros.h:201