Point Cloud Library (PCL) 1.15.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
53 triangles_ (0),
54 triangles_of_the_point_ (0)
55{
56}
57
58//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59template <typename PointInT, typename PointOutT>
60pcl::ROPSEstimation <PointInT, PointOutT>::~ROPSEstimation ()
61{
62 triangles_.clear ();
63 triangles_of_the_point_.clear ();
64}
65
66//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67template <typename PointInT, typename PointOutT> void
69{
70 if (number_of_bins != 0)
71 number_of_bins_ = number_of_bins;
72}
73
74//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointInT, typename PointOutT> unsigned int
77{
78 return (number_of_bins_);
79}
80
81//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
82template <typename PointInT, typename PointOutT> void
84{
85 if (number_of_rotations != 0)
86 {
87 number_of_rotations_ = number_of_rotations;
88 step_ = 90.0f / static_cast <float> (number_of_rotations_ + 1);
89 }
90}
91
92//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
93template <typename PointInT, typename PointOutT> unsigned int
95{
96 return (number_of_rotations_);
97}
98
99//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
100template <typename PointInT, typename PointOutT> void
102{
103 if (support_radius > 0.0f)
104 {
105 support_radius_ = support_radius;
106 sqr_support_radius_ = support_radius * support_radius;
107 }
108}
109
110//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
111template <typename PointInT, typename PointOutT> float
113{
114 return (support_radius_);
115}
116
117//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
118template <typename PointInT, typename PointOutT> void
119pcl::ROPSEstimation <PointInT, PointOutT>::setTriangles (const std::vector <pcl::Vertices>& triangles)
120{
121 triangles_ = triangles;
122}
123
124//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
125template <typename PointInT, typename PointOutT> void
126pcl::ROPSEstimation <PointInT, PointOutT>::getTriangles (std::vector <pcl::Vertices>& triangles) const
127{
128 triangles = triangles_;
129}
130
131//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
132template <typename PointInT, typename PointOutT> void
133pcl::ROPSEstimation <PointInT, PointOutT>::computeFeature (PointCloudOut &output)
134{
135 if (triangles_.empty ())
136 {
137 output.clear ();
138 return;
139 }
140
141 buildListOfPointsTriangles ();
142
143 //feature size = number_of_rotations * number_of_axis_to_rotate_around * number_of_projections * number_of_central_moments
144 unsigned int feature_size = number_of_rotations_ * 3 * 3 * 5;
145 const auto number_of_points = indices_->size ();
146 output.clear ();
147 output.reserve (number_of_points);
148
149 for (const auto& idx: *indices_)
150 {
151 std::set <unsigned int> local_triangles;
152 pcl::Indices local_points;
153 getLocalSurface ((*input_)[idx], local_triangles, local_points);
154
155 Eigen::Matrix3f lrf_matrix;
156 computeLRF ((*input_)[idx], local_triangles, lrf_matrix);
157
158 PointCloudIn transformed_cloud;
159 transformCloud ((*input_)[idx], lrf_matrix, local_points, transformed_cloud);
160
161 std::array<PointInT, 3> axes;
162 axes[0].x = 1.0f; axes[0].y = 0.0f; axes[0].z = 0.0f;
163 axes[1].x = 0.0f; axes[1].y = 1.0f; axes[1].z = 0.0f;
164 axes[2].x = 0.0f; axes[2].y = 0.0f; axes[2].z = 1.0f;
165 std::vector <float> feature;
166 for (const auto &axis : axes)
167 {
168 float theta = step_;
169 do
170 {
171 //rotate local surface and get bounding box
172 PointCloudIn rotated_cloud;
173 Eigen::Vector3f min, max;
174 rotateCloud (axis, theta, transformed_cloud, rotated_cloud, min, max);
175
176 //for each projection (XY, XZ and YZ) compute distribution matrix and central moments
177 for (unsigned int i_proj = 0; i_proj < 3; i_proj++)
178 {
179 Eigen::MatrixXf distribution_matrix;
180 distribution_matrix.resize (number_of_bins_, number_of_bins_);
181 getDistributionMatrix (i_proj, min, max, rotated_cloud, distribution_matrix);
182
183 // TODO remove this needless copy due to API design
184 std::vector <float> moments;
185 computeCentralMoments (distribution_matrix, moments);
186
187 feature.insert (feature.end (), moments.begin (), moments.end ());
188 }
189
190 theta += step_;
191 } while (theta < 90.0f);
192 }
193
194 const float norm = std::accumulate(
195 feature.cbegin(), feature.cend(), 0.f, [](const auto& sum, const auto& val) {
196 return sum + std::abs(val);
197 });
198 float invert_norm;
199 if (norm < std::numeric_limits <float>::epsilon ())
200 invert_norm = 1.0f;
201 else
202 invert_norm = 1.0f / norm;
203
204 output.emplace_back ();
205 for (std::size_t i_dim = 0; i_dim < feature_size; i_dim++)
206 output.back().histogram[i_dim] = feature[i_dim] * invert_norm;
207 }
208}
209
210//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
211template <typename PointInT, typename PointOutT> void
212pcl::ROPSEstimation <PointInT, PointOutT>::buildListOfPointsTriangles ()
213{
214 triangles_of_the_point_.clear ();
215
216 std::vector <unsigned int> dummy;
217 dummy.reserve (100);
218 triangles_of_the_point_.resize (surface_->points. size (), dummy);
219
220 for (std::size_t i_triangle = 0; i_triangle < triangles_.size (); i_triangle++)
221 for (const auto& vertex: triangles_[i_triangle].vertices)
222 triangles_of_the_point_[vertex].push_back (i_triangle);
223}
224
225//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
226template <typename PointInT, typename PointOutT> void
227pcl::ROPSEstimation <PointInT, PointOutT>::getLocalSurface (const PointInT& point, std::set <unsigned int>& local_triangles, pcl::Indices& local_points) const
228{
229 std::vector <float> distances;
230 tree_->radiusSearch (point, support_radius_, local_points, distances);
231
232 for (const auto& pt: local_points)
233 local_triangles.insert (triangles_of_the_point_[pt].begin (),
234 triangles_of_the_point_[pt].end ());
235}
236
237//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
238template <typename PointInT, typename PointOutT> void
239pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
240{
241 std::size_t number_of_triangles = local_triangles.size ();
242
243 std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices;
244 std::vector <float> triangle_area (number_of_triangles), distance_weight (number_of_triangles);
245
246 scatter_matrices.reserve (number_of_triangles);
247 triangle_area.clear ();
248 distance_weight.clear ();
249
250 float total_area = 0.0f;
251 const float coeff = 1.0f / 12.0f;
252 const float coeff_1_div_3 = 1.0f / 3.0f;
253
254 Eigen::Vector3f feature_point (point.x, point.y, point.z);
255
256 for (const auto& triangle: local_triangles)
257 {
258 Eigen::Vector3f pt[3];
259 for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
260 {
261 const unsigned int index = triangles_[triangle].vertices[i_vertex];
262 pt[i_vertex] (0) = (*surface_)[index].x;
263 pt[i_vertex] (1) = (*surface_)[index].y;
264 pt[i_vertex] (2) = (*surface_)[index].z;
265 }
266
267 const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
268 triangle_area.push_back (curr_area);
269 total_area += curr_area;
270
271 distance_weight.push_back (std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f));
272
273 Eigen::Matrix3f curr_scatter_matrix;
274 curr_scatter_matrix.setZero ();
275 for (const auto &i_pt : pt)
276 {
277 Eigen::Vector3f vec = i_pt - feature_point;
278 curr_scatter_matrix += vec * (vec.transpose ());
279 for (const auto &j_pt : pt)
280 curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
281 }
282 scatter_matrices.emplace_back (coeff * curr_scatter_matrix);
283 }
284
285 if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
286 total_area = 1.0f;
287 else
288 total_area = 1.0f / total_area;
289
290 Eigen::Matrix3f overall_scatter_matrix;
291 overall_scatter_matrix.setZero ();
292 std::vector<float> total_weight (number_of_triangles);
293 const float denominator = 1.0f / 6.0f;
294 for (std::size_t i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
295 {
296 const float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
297 overall_scatter_matrix += factor * scatter_matrices[i_triangle];
298 total_weight[i_triangle] = factor * denominator;
299 }
300
301 Eigen::Vector3f v1, v2, v3;
302 computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
303
304 float h1 = 0.0f;
305 float h3 = 0.0f;
306 std::size_t i_triangle = 0;
307 for (const auto& triangle: local_triangles)
308 {
309 Eigen::Vector3f pt[3];
310 for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
311 {
312 const unsigned int index = triangles_[triangle].vertices[i_vertex];
313 pt[i_vertex] (0) = (*surface_)[index].x;
314 pt[i_vertex] (1) = (*surface_)[index].y;
315 pt[i_vertex] (2) = (*surface_)[index].z;
316 }
317
318 float factor1 = 0.0f;
319 float factor3 = 0.0f;
320 for (const auto &i_pt : pt)
321 {
322 Eigen::Vector3f vec = i_pt - feature_point;
323 factor1 += vec.dot (v1);
324 factor3 += vec.dot (v3);
325 }
326 h1 += total_weight[i_triangle] * factor1;
327 h3 += total_weight[i_triangle] * factor3;
328 i_triangle++;
329 }
330
331 if (h1 < 0.0f) v1 = -v1;
332 if (h3 < 0.0f) v3 = -v3;
333
334 v2 = v3.cross (v1);
335
336 lrf_matrix.row (0) = v1;
337 lrf_matrix.row (1) = v2;
338 lrf_matrix.row (2) = v3;
339}
340
341//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
342template <typename PointInT, typename PointOutT> void
343pcl::ROPSEstimation <PointInT, PointOutT>::computeEigenVectors (const Eigen::Matrix3f& matrix,
344 Eigen::Vector3f& major_axis, Eigen::Vector3f& middle_axis, Eigen::Vector3f& minor_axis) const
345{
346 Eigen::EigenSolver <Eigen::Matrix3f> eigen_solver;
347 eigen_solver.compute (matrix);
348
349 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvectorsType eigen_vectors;
350 Eigen::EigenSolver <Eigen::Matrix3f>::EigenvalueType eigen_values;
351 eigen_vectors = eigen_solver.eigenvectors ();
352 eigen_values = eigen_solver.eigenvalues ();
353
354 unsigned int temp = 0;
355 unsigned int major_index = 0;
356 unsigned int middle_index = 1;
357 unsigned int minor_index = 2;
358
359 if (eigen_values.real () (major_index) < eigen_values.real () (middle_index))
360 {
361 temp = major_index;
362 major_index = middle_index;
363 middle_index = temp;
364 }
365
366 if (eigen_values.real () (major_index) < eigen_values.real () (minor_index))
367 {
368 temp = major_index;
369 major_index = minor_index;
370 minor_index = temp;
371 }
372
373 if (eigen_values.real () (middle_index) < eigen_values.real () (minor_index))
374 {
375 temp = minor_index;
376 minor_index = middle_index;
377 middle_index = temp;
378 }
379
380 major_axis = eigen_vectors.col (major_index).real ();
381 middle_axis = eigen_vectors.col (middle_index).real ();
382 minor_axis = eigen_vectors.col (minor_index).real ();
383}
384
385//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
386template <typename PointInT, typename PointOutT> void
387pcl::ROPSEstimation <PointInT, PointOutT>::transformCloud (const PointInT& point, const Eigen::Matrix3f& matrix, const pcl::Indices& local_points, PointCloudIn& transformed_cloud) const
388{
389 const auto number_of_points = local_points.size ();
390 transformed_cloud.clear ();
391 transformed_cloud.reserve (number_of_points);
392
393 for (const auto& idx: local_points)
394 {
395 Eigen::Vector3f transformed_point ((*surface_)[idx].x - point.x,
396 (*surface_)[idx].y - point.y,
397 (*surface_)[idx].z - point.z);
398
399 transformed_point = matrix * transformed_point;
400
401 PointInT new_point;
402 new_point.x = transformed_point (0);
403 new_point.y = transformed_point (1);
404 new_point.z = transformed_point (2);
405 transformed_cloud.emplace_back (new_point);
406 }
407}
408
409//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
410template <typename PointInT, typename PointOutT> void
411pcl::ROPSEstimation <PointInT, PointOutT>::rotateCloud (const PointInT& axis, const float angle, const PointCloudIn& cloud, PointCloudIn& rotated_cloud, Eigen::Vector3f& min, Eigen::Vector3f& max) const
412{
413 Eigen::Matrix3f rotation_matrix;
414 const float x = axis.x;
415 const float y = axis.y;
416 const float z = axis.z;
417 const float rad = M_PI / 180.0f;
418 const float cosine = std::cos (angle * rad);
419 const float sine = std::sin (angle * rad);
420 rotation_matrix << cosine + (1 - cosine) * x * x, (1 - cosine) * x * y - sine * z, (1 - cosine) * x * z + sine * y,
421 (1 - cosine) * y * x + sine * z, cosine + (1 - cosine) * y * y, (1 - cosine) * y * z - sine * x,
422 (1 - cosine) * z * x - sine * y, (1 - cosine) * z * y + sine * x, cosine + (1 - cosine) * z * z;
423
424 const auto number_of_points = cloud.size ();
425
426 rotated_cloud.header = cloud.header;
427 rotated_cloud.width = number_of_points;
428 rotated_cloud.height = 1;
429 rotated_cloud.clear ();
430 rotated_cloud.reserve (number_of_points);
431
432 min (0) = std::numeric_limits <float>::max ();
433 min (1) = std::numeric_limits <float>::max ();
434 min (2) = std::numeric_limits <float>::max ();
435 max (0) = -std::numeric_limits <float>::max ();
436 max (1) = -std::numeric_limits <float>::max ();
437 max (2) = -std::numeric_limits <float>::max ();
438
439 for (const auto& pt: cloud.points)
440 {
441 Eigen::Vector3f point (pt.x, pt.y, pt.z);
442 point = rotation_matrix * point;
443
444 PointInT rotated_point;
445 rotated_point.x = point (0);
446 rotated_point.y = point (1);
447 rotated_point.z = point (2);
448 rotated_cloud.emplace_back (rotated_point);
449
450 for (int i = 0; i < 3; ++i)
451 {
452 min(i) = std::min(min(i), point(i));
453 max(i) = std::max(max(i), point(i));
454 }
455 }
456}
457
458//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
459template <typename PointInT, typename PointOutT> void
460pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
461{
462 matrix.setZero ();
463
464 const unsigned int coord[3][2] = {
465 {0, 1},
466 {0, 2},
467 {1, 2}};
468
469 const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
470 const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
471
472 for (const auto& pt: cloud.points)
473 {
474 Eigen::Vector3f point (pt.x, pt.y, pt.z);
475
476 const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
477 const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
478
479 const float u_ratio = u_length / u_bin_length;
480 auto row = static_cast <unsigned int> (u_ratio);
481 if (row == number_of_bins_) row--;
482
483 const float v_ratio = v_length / v_bin_length;
484 auto col = static_cast <unsigned int> (v_ratio);
485 if (col == number_of_bins_) col--;
486
487 matrix (row, col) += 1.0f;
488 }
489
490 matrix /= std::max<float> (1, cloud.size ());
491}
492
493//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
494template <typename PointInT, typename PointOutT> void
495pcl::ROPSEstimation <PointInT, PointOutT>::computeCentralMoments (const Eigen::MatrixXf& matrix, std::vector <float>& moments) const
496{
497 float mean_i = 0.0f;
498 float mean_j = 0.0f;
499
500 for (unsigned int i = 0; i < number_of_bins_; i++)
501 for (unsigned int j = 0; j < number_of_bins_; j++)
502 {
503 const float m = matrix (i, j);
504 mean_i += static_cast <float> (i + 1) * m;
505 mean_j += static_cast <float> (j + 1) * m;
506 }
507
508 const unsigned int number_of_moments_to_compute = 4;
509 const float power[number_of_moments_to_compute][2] = {
510 {1.0f, 1.0f},
511 {2.0f, 1.0f},
512 {1.0f, 2.0f},
513 {2.0f, 2.0f}};
514
515 float entropy = 0.0f;
516 moments.resize (number_of_moments_to_compute + 1, 0.0f);
517 for (unsigned int i = 0; i < number_of_bins_; i++)
518 {
519 const float i_factor = static_cast <float> (i + 1) - mean_i;
520 for (unsigned int j = 0; j < number_of_bins_; j++)
521 {
522 const float j_factor = static_cast <float> (j + 1) - mean_j;
523 const float m = matrix (i, j);
524 if (m > 0.0f)
525 entropy -= m * std::log (m);
526 for (unsigned int i_moment = 0; i_moment < number_of_moments_to_compute; i_moment++)
527 moments[i_moment] += std::pow (i_factor, power[i_moment][0]) * std::pow (j_factor, power[i_moment][1]) * m;
528 }
529 }
530
531 moments[number_of_moments_to_compute] = entropy;
532}
533
534#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:203