Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
centroid.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-present, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/common/centroid.h>
44#include <pcl/conversions.h>
45#include <pcl/common/point_tests.h> // for pcl::isFinite
46#include <Eigen/Eigenvalues> // for EigenSolver
47
48#include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
49#include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
50#include <boost/mpl/size.hpp> // for boost::mpl::size
51
52
53namespace pcl
54{
55
56template <typename PointT, typename Scalar> inline unsigned int
58 Eigen::Matrix<Scalar, 4, 1> &centroid)
59{
60 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
61
62 unsigned int cp = 0;
63
64 // For each point in the cloud
65 // If the data is dense, we don't need to check for NaN
66 while (cloud_iterator.isValid ())
67 {
68 // Check if the point is invalid
69 if (pcl::isFinite (*cloud_iterator))
70 {
71 accumulator[0] += cloud_iterator->x;
72 accumulator[1] += cloud_iterator->y;
73 accumulator[2] += cloud_iterator->z;
74 ++cp;
75 }
76 ++cloud_iterator;
77 }
78
79 if (cp > 0) {
80 centroid = accumulator;
81 centroid /= static_cast<Scalar> (cp);
82 centroid[3] = 1;
83 }
84 return (cp);
85}
86
87
88template <typename PointT, typename Scalar> inline unsigned int
90 Eigen::Matrix<Scalar, 4, 1> &centroid)
91{
92 if (cloud.empty ())
93 return (0);
94
95 // For each point in the cloud
96 // If the data is dense, we don't need to check for NaN
97 if (cloud.is_dense)
98 {
99 // Initialize to 0
100 centroid.setZero ();
101 for (const auto& point: cloud)
102 {
103 centroid[0] += point.x;
104 centroid[1] += point.y;
105 centroid[2] += point.z;
106 }
107 centroid /= static_cast<Scalar> (cloud.size ());
108 centroid[3] = 1;
109
110 return (static_cast<unsigned int> (cloud.size ()));
111 }
112 // NaN or Inf values could exist => check for them
113 unsigned int cp = 0;
114 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
115 for (const auto& point: cloud)
116 {
117 // Check if the point is invalid
118 if (!isFinite (point))
119 continue;
120
121 accumulator[0] += point.x;
122 accumulator[1] += point.y;
123 accumulator[2] += point.z;
124 ++cp;
125 }
126 if (cp > 0) {
127 centroid = accumulator;
128 centroid /= static_cast<Scalar> (cp);
129 centroid[3] = 1;
130 }
131
132 return (cp);
133}
134
135
136template <typename PointT, typename Scalar> inline unsigned int
138 const Indices &indices,
139 Eigen::Matrix<Scalar, 4, 1> &centroid)
140{
141 if (indices.empty ())
142 return (0);
143
144 // If the data is dense, we don't need to check for NaN
145 if (cloud.is_dense)
146 {
147 // Initialize to 0
148 centroid.setZero ();
149 for (const auto& index : indices)
150 {
151 centroid[0] += cloud[index].x;
152 centroid[1] += cloud[index].y;
153 centroid[2] += cloud[index].z;
154 }
155 centroid /= static_cast<Scalar> (indices.size ());
156 centroid[3] = 1;
157 return (static_cast<unsigned int> (indices.size ()));
158 }
159 // NaN or Inf values could exist => check for them
160 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
161 unsigned int cp = 0;
162 for (const auto& index : indices)
163 {
164 // Check if the point is invalid
165 if (!isFinite (cloud [index]))
166 continue;
167
168 accumulator[0] += cloud[index].x;
169 accumulator[1] += cloud[index].y;
170 accumulator[2] += cloud[index].z;
171 ++cp;
172 }
173 if (cp > 0) {
174 centroid = accumulator;
175 centroid /= static_cast<Scalar> (cp);
176 centroid[3] = 1;
177 }
178 return (cp);
179}
180
181
182template <typename PointT, typename Scalar> inline unsigned int
184 const pcl::PointIndices &indices,
185 Eigen::Matrix<Scalar, 4, 1> &centroid)
186{
187 return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
188}
189
190
191template <typename PointT, typename Scalar> inline unsigned
193 const Eigen::Matrix<Scalar, 4, 1> &centroid,
194 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
195{
196 if (cloud.empty ())
197 return (0);
198
199 unsigned point_count;
200 // If the data is dense, we don't need to check for NaN
201 if (cloud.is_dense)
202 {
203 covariance_matrix.setZero ();
204 point_count = static_cast<unsigned> (cloud.size ());
205 // For each point in the cloud
206 for (const auto& point: cloud)
207 {
208 Eigen::Matrix<Scalar, 4, 1> pt;
209 pt[0] = point.x - centroid[0];
210 pt[1] = point.y - centroid[1];
211 pt[2] = point.z - centroid[2];
212
213 covariance_matrix (1, 1) += pt.y () * pt.y ();
214 covariance_matrix (1, 2) += pt.y () * pt.z ();
215
216 covariance_matrix (2, 2) += pt.z () * pt.z ();
217
218 pt *= pt.x ();
219 covariance_matrix (0, 0) += pt.x ();
220 covariance_matrix (0, 1) += pt.y ();
221 covariance_matrix (0, 2) += pt.z ();
222 }
223 }
224 // NaN or Inf values could exist => check for them
225 else
226 {
227 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
228 temp_covariance_matrix.setZero();
229 point_count = 0;
230 // For each point in the cloud
231 for (const auto& point: cloud)
232 {
233 // Check if the point is invalid
234 if (!isFinite (point))
235 continue;
236
237 Eigen::Matrix<Scalar, 4, 1> pt;
238 pt[0] = point.x - centroid[0];
239 pt[1] = point.y - centroid[1];
240 pt[2] = point.z - centroid[2];
241
242 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
243 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
244
245 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
246
247 pt *= pt.x ();
248 temp_covariance_matrix (0, 0) += pt.x ();
249 temp_covariance_matrix (0, 1) += pt.y ();
250 temp_covariance_matrix (0, 2) += pt.z ();
251 ++point_count;
252 }
253 if (point_count > 0) {
254 covariance_matrix = temp_covariance_matrix;
255 }
256 }
257 if (point_count == 0) {
258 return 0;
259 }
260 covariance_matrix (1, 0) = covariance_matrix (0, 1);
261 covariance_matrix (2, 0) = covariance_matrix (0, 2);
262 covariance_matrix (2, 1) = covariance_matrix (1, 2);
263
264 return (point_count);
265}
266
267
268template <typename PointT, typename Scalar> inline unsigned int
270 const Eigen::Matrix<Scalar, 4, 1> &centroid,
271 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
272{
273 unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
274 if (point_count != 0)
275 covariance_matrix /= static_cast<Scalar> (point_count);
276 return (point_count);
277}
278
279
280template <typename PointT, typename Scalar> inline unsigned int
282 const Indices &indices,
283 const Eigen::Matrix<Scalar, 4, 1> &centroid,
284 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
285{
286 if (indices.empty ())
287 return (0);
288
289 std::size_t point_count;
290 // If the data is dense, we don't need to check for NaN
291 if (cloud.is_dense)
292 {
293 covariance_matrix.setZero ();
294 point_count = indices.size ();
295 // For each point in the cloud
296 for (const auto& idx: indices)
297 {
298 Eigen::Matrix<Scalar, 4, 1> pt;
299 pt[0] = cloud[idx].x - centroid[0];
300 pt[1] = cloud[idx].y - centroid[1];
301 pt[2] = cloud[idx].z - centroid[2];
302
303 covariance_matrix (1, 1) += pt.y () * pt.y ();
304 covariance_matrix (1, 2) += pt.y () * pt.z ();
305
306 covariance_matrix (2, 2) += pt.z () * pt.z ();
307
308 pt *= pt.x ();
309 covariance_matrix (0, 0) += pt.x ();
310 covariance_matrix (0, 1) += pt.y ();
311 covariance_matrix (0, 2) += pt.z ();
312 }
313 }
314 // NaN or Inf values could exist => check for them
315 else
316 {
317 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
318 temp_covariance_matrix.setZero ();
319 point_count = 0;
320 // For each point in the cloud
321 for (const auto &index : indices)
322 {
323 // Check if the point is invalid
324 if (!isFinite (cloud[index]))
325 continue;
326
327 Eigen::Matrix<Scalar, 4, 1> pt;
328 pt[0] = cloud[index].x - centroid[0];
329 pt[1] = cloud[index].y - centroid[1];
330 pt[2] = cloud[index].z - centroid[2];
331
332 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
333 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
334
335 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
336
337 pt *= pt.x ();
338 temp_covariance_matrix (0, 0) += pt.x ();
339 temp_covariance_matrix (0, 1) += pt.y ();
340 temp_covariance_matrix (0, 2) += pt.z ();
341 ++point_count;
342 }
343 if (point_count > 0) {
344 covariance_matrix = temp_covariance_matrix;
345 }
346 }
347 if (point_count == 0) {
348 return 0;
349 }
350 covariance_matrix (1, 0) = covariance_matrix (0, 1);
351 covariance_matrix (2, 0) = covariance_matrix (0, 2);
352 covariance_matrix (2, 1) = covariance_matrix (1, 2);
353 return (static_cast<unsigned int> (point_count));
354}
355
356
357template <typename PointT, typename Scalar> inline unsigned int
359 const pcl::PointIndices &indices,
360 const Eigen::Matrix<Scalar, 4, 1> &centroid,
361 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
362{
363 return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
364}
365
366
367template <typename PointT, typename Scalar> inline unsigned int
369 const Indices &indices,
370 const Eigen::Matrix<Scalar, 4, 1> &centroid,
371 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
372{
373 unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
374 if (point_count != 0)
375 covariance_matrix /= static_cast<Scalar> (point_count);
376
377 return (point_count);
378}
379
380
381template <typename PointT, typename Scalar> inline unsigned int
383 const pcl::PointIndices &indices,
384 const Eigen::Matrix<Scalar, 4, 1> &centroid,
385 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
386{
387 return computeCovarianceMatrixNormalized(cloud, indices.indices, centroid, covariance_matrix);
388}
389
390
391template <typename PointT, typename Scalar> inline unsigned int
393 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
394{
395 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
396 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
397
398 unsigned int point_count;
399 if (cloud.is_dense)
400 {
401 point_count = static_cast<unsigned int> (cloud.size ());
402 // For each point in the cloud
403 for (const auto& point: cloud)
404 {
405 accu [0] += point.x * point.x;
406 accu [1] += point.x * point.y;
407 accu [2] += point.x * point.z;
408 accu [3] += point.y * point.y;
409 accu [4] += point.y * point.z;
410 accu [5] += point.z * point.z;
411 }
412 }
413 else
414 {
415 point_count = 0;
416 for (const auto& point: cloud)
417 {
418 if (!isFinite (point))
419 continue;
420
421 accu [0] += point.x * point.x;
422 accu [1] += point.x * point.y;
423 accu [2] += point.x * point.z;
424 accu [3] += point.y * point.y;
425 accu [4] += point.y * point.z;
426 accu [5] += point.z * point.z;
427 ++point_count;
428 }
429 }
430
431 if (point_count != 0)
432 {
433 accu /= static_cast<Scalar> (point_count);
434 covariance_matrix.coeffRef (0) = accu [0];
435 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
436 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
437 covariance_matrix.coeffRef (4) = accu [3];
438 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
439 covariance_matrix.coeffRef (8) = accu [5];
440 }
441 return (point_count);
442}
443
444
445template <typename PointT, typename Scalar> inline unsigned int
447 const Indices &indices,
448 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
449{
450 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
451 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
452
453 unsigned int point_count;
454 if (cloud.is_dense)
455 {
456 point_count = static_cast<unsigned int> (indices.size ());
457 for (const auto &index : indices)
458 {
459 //const PointT& point = cloud[*iIt];
460 accu [0] += cloud[index].x * cloud[index].x;
461 accu [1] += cloud[index].x * cloud[index].y;
462 accu [2] += cloud[index].x * cloud[index].z;
463 accu [3] += cloud[index].y * cloud[index].y;
464 accu [4] += cloud[index].y * cloud[index].z;
465 accu [5] += cloud[index].z * cloud[index].z;
466 }
467 }
468 else
469 {
470 point_count = 0;
471 for (const auto &index : indices)
472 {
473 if (!isFinite (cloud[index]))
474 continue;
475
476 ++point_count;
477 accu [0] += cloud[index].x * cloud[index].x;
478 accu [1] += cloud[index].x * cloud[index].y;
479 accu [2] += cloud[index].x * cloud[index].z;
480 accu [3] += cloud[index].y * cloud[index].y;
481 accu [4] += cloud[index].y * cloud[index].z;
482 accu [5] += cloud[index].z * cloud[index].z;
483 }
484 }
485 if (point_count != 0)
486 {
487 accu /= static_cast<Scalar> (point_count);
488 covariance_matrix.coeffRef (0) = accu [0];
489 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
490 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
491 covariance_matrix.coeffRef (4) = accu [3];
492 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
493 covariance_matrix.coeffRef (8) = accu [5];
494 }
495 return (point_count);
496}
497
498
499template <typename PointT, typename Scalar> inline unsigned int
501 const pcl::PointIndices &indices,
502 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
503{
504 return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
505}
506
507
508template <typename PointT, typename Scalar> inline unsigned int
510 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
511 Eigen::Matrix<Scalar, 4, 1> &centroid)
512{
513 // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
514 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
515 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
516 Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
517 for(const auto& point: cloud)
518 if(isFinite(point)) {
519 K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
520 }
521 std::size_t point_count;
522 if (cloud.is_dense)
523 {
524 point_count = cloud.size ();
525 // For each point in the cloud
526 for (const auto& point: cloud)
527 {
528 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
529 accu [0] += x * x;
530 accu [1] += x * y;
531 accu [2] += x * z;
532 accu [3] += y * y;
533 accu [4] += y * z;
534 accu [5] += z * z;
535 accu [6] += x;
536 accu [7] += y;
537 accu [8] += z;
538 }
539 }
540 else
541 {
542 point_count = 0;
543 for (const auto& point: cloud)
544 {
545 if (!isFinite (point))
546 continue;
547
548 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
549 accu [0] += x * x;
550 accu [1] += x * y;
551 accu [2] += x * z;
552 accu [3] += y * y;
553 accu [4] += y * z;
554 accu [5] += z * z;
555 accu [6] += x;
556 accu [7] += y;
557 accu [8] += z;
558 ++point_count;
559 }
560 }
561 if (point_count != 0)
562 {
563 accu /= static_cast<Scalar> (point_count);
564 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();//effective mean E[P=(x,y,z)]
565 centroid[3] = 1;
566 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2
567 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky]
568 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//(0,2)xz
569 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//(1,1)yy
570 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//(1,2)yz
571 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//(2,2)zz
572 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx
573 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx
574 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy
575 }
576 return (static_cast<unsigned int> (point_count));
577}
578
579
580template <typename PointT, typename Scalar> inline unsigned int
582 const Indices &indices,
583 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
584 Eigen::Matrix<Scalar, 4, 1> &centroid)
585{
586 // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
587 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
588 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
589 Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
590 for(const auto& index : indices)
591 if(isFinite(cloud[index])) {
592 K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
593 }
594 std::size_t point_count;
595 if (cloud.is_dense)
596 {
597 point_count = indices.size ();
598 for (const auto &index : indices)
599 {
600 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
601 accu [0] += x * x;
602 accu [1] += x * y;
603 accu [2] += x * z;
604 accu [3] += y * y;
605 accu [4] += y * z;
606 accu [5] += z * z;
607 accu [6] += x;
608 accu [7] += y;
609 accu [8] += z;
610 }
611 }
612 else
613 {
614 point_count = 0;
615 for (const auto &index : indices)
616 {
617 if (!isFinite (cloud[index]))
618 continue;
619
620 ++point_count;
621 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
622 accu [0] += x * x;
623 accu [1] += x * y;
624 accu [2] += x * z;
625 accu [3] += y * y;
626 accu [4] += y * z;
627 accu [5] += z * z;
628 accu [6] += x;
629 accu [7] += y;
630 accu [8] += z;
631 }
632 }
633
634 if (point_count != 0)
635 {
636 accu /= static_cast<Scalar> (point_count);
637 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();//effective mean E[P=(x,y,z)]
638 centroid[3] = 1;
639 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2
640 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky]
641 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//(0,2)xz
642 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//(1,1)yy
643 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//(1,2)yz
644 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//(2,2)zz
645 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx
646 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx
647 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy
648 }
649 return (static_cast<unsigned int> (point_count));
650}
651
652
653template <typename PointT, typename Scalar> inline unsigned int
655 const pcl::PointIndices &indices,
656 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
657 Eigen::Matrix<Scalar, 4, 1> &centroid)
658{
659 return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
660}
661
662template <typename PointT, typename Scalar> inline unsigned int
664 Eigen::Matrix<Scalar, 3, 1> &centroid,
665 Eigen::Matrix<Scalar, 3, 1> &obb_center,
666 Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
667 Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
668{
669 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
670 Eigen::Matrix<Scalar, 4, 1> centroid4;
671 const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);
672 if (!point_count)
673 return (0);
674 centroid(0) = centroid4(0);
675 centroid(1) = centroid4(1);
676 centroid(2) = centroid4(2);
677
678 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
679 const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
680 const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);//the eigenvectors do not need to be normalized (they are already)
681 const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
682 // Enforce right hand rule:
683 const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
684
685 obb_rotational_matrix <<
686 major_axis(0), middle_axis(0), minor_axis(0),
687 major_axis(1), middle_axis(1), minor_axis(1),
688 major_axis(2), middle_axis(2), minor_axis(2);
689 //obb_rotational_matrix.col(0)==major_axis
690 //obb_rotational_matrix.col(1)==middle_axis
691 //obb_rotational_matrix.col(2)==minor_axis
692
693 //Trasforming the point cloud in the (Centroid, ma-mi-mi_axis) reference
694 //with homogeneous matrix
695 //[R^t , -R^t*Centroid ]
696 //[0 , 1 ]
697 Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
698 transform.topLeftCorner(3, 3) = obb_rotational_matrix.transpose();
699 transform.topRightCorner(3, 1) =-transform.topLeftCorner(3, 3)*centroid;
700
701 //when Scalar==double on a Windows 10 machine and MSVS:
702 //if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse
703 Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
704 Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
705 obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
706 obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
707
708 if (cloud.is_dense)
709 {
710 const auto& point = cloud[0];
711 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
712 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
713
714 obb_min_pointx = obb_max_pointx = P(0);
715 obb_min_pointy = obb_max_pointy = P(1);
716 obb_min_pointz = obb_max_pointz = P(2);
717
718 for (size_t i=1; i<cloud.size();++i)
719 {
720 const auto& point = cloud[i];
721 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
722 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
723
724 if (P(0) < obb_min_pointx)
725 obb_min_pointx = P(0);
726 else if (P(0) > obb_max_pointx)
727 obb_max_pointx = P(0);
728 if (P(1) < obb_min_pointy)
729 obb_min_pointy = P(1);
730 else if (P(1) > obb_max_pointy)
731 obb_max_pointy = P(1);
732 if (P(2) < obb_min_pointz)
733 obb_min_pointz = P(2);
734 else if (P(2) > obb_max_pointz)
735 obb_max_pointz = P(2);
736 }
737 }
738 else
739 {
740 size_t i = 0;
741 for (; i < cloud.size(); ++i)
742 {
743 const auto& point = cloud[i];
744 if (!isFinite(point))
745 continue;
746 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
747 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
748
749 obb_min_pointx = obb_max_pointx = P(0);
750 obb_min_pointy = obb_max_pointy = P(1);
751 obb_min_pointz = obb_max_pointz = P(2);
752 ++i;
753 break;
754 }
755
756 for (; i<cloud.size();++i)
757 {
758 const auto& point = cloud[i];
759 if (!isFinite(point))
760 continue;
761 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
762 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
763
764 if (P(0) < obb_min_pointx)
765 obb_min_pointx = P(0);
766 else if (P(0) > obb_max_pointx)
767 obb_max_pointx = P(0);
768 if (P(1) < obb_min_pointy)
769 obb_min_pointy = P(1);
770 else if (P(1) > obb_max_pointy)
771 obb_max_pointy = P(1);
772 if (P(2) < obb_min_pointz)
773 obb_min_pointz = P(2);
774 else if (P(2) > obb_max_pointz)
775 obb_max_pointz = P(2);
776 }
777
778 }
779
780 const Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))
781 shift((obb_max_pointx + obb_min_pointx) / 2.0f,
782 (obb_max_pointy + obb_min_pointy) / 2.0f,
783 (obb_max_pointz + obb_min_pointz) / 2.0f);
784
785 obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
786 obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
787 obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
788
789 obb_center = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
790
791 return (point_count);
792}
793
794
795template <typename PointT, typename Scalar> inline unsigned int
797 const Indices &indices,
798 Eigen::Matrix<Scalar, 3, 1> &centroid,
799 Eigen::Matrix<Scalar, 3, 1> &obb_center,
800 Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
801 Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
802{
803 Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
804 Eigen::Matrix<Scalar, 4, 1> centroid4;
805 const auto point_count = computeMeanAndCovarianceMatrix(cloud, indices, covariance_matrix, centroid4);
806 if (!point_count)
807 return (0);
808 centroid(0) = centroid4(0);
809 centroid(1) = centroid4(1);
810 centroid(2) = centroid4(2);
811
812 const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
813 const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
814 const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);//the eigenvectors do not need to be normalized (they are already)
815 const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
816 // Enforce right hand rule:
817 const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
818
819 obb_rotational_matrix <<
820 major_axis(0), middle_axis(0), minor_axis(0),
821 major_axis(1), middle_axis(1), minor_axis(1),
822 major_axis(2), middle_axis(2), minor_axis(2);
823 //obb_rotational_matrix.col(0)==major_axis
824 //obb_rotational_matrix.col(1)==middle_axis
825 //obb_rotational_matrix.col(2)==minor_axis
826
827 //Trasforming the point cloud in the (Centroid, ma-mi-mi_axis) reference
828 //with homogeneous matrix
829 //[R^t , -R^t*Centroid ]
830 //[0 , 1 ]
831 Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
832 transform.topLeftCorner(3, 3) = obb_rotational_matrix.transpose();
833 transform.topRightCorner(3, 1) =-transform.topLeftCorner(3, 3)*centroid;
834
835 //when Scalar==double on a Windows 10 machine and MSVS:
836 //if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse
837 Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
838 Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
839 obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
840 obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
841
842 if (cloud.is_dense)
843 {
844 const auto& point = cloud[indices[0]];
845 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
846 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
847
848 obb_min_pointx = obb_max_pointx = P(0);
849 obb_min_pointy = obb_max_pointy = P(1);
850 obb_min_pointz = obb_max_pointz = P(2);
851
852 for (size_t i=1; i<indices.size();++i)
853 {
854 const auto & point = cloud[indices[i]];
855
856 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
857 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
858
859 if (P(0) < obb_min_pointx)
860 obb_min_pointx = P(0);
861 else if (P(0) > obb_max_pointx)
862 obb_max_pointx = P(0);
863 if (P(1) < obb_min_pointy)
864 obb_min_pointy = P(1);
865 else if (P(1) > obb_max_pointy)
866 obb_max_pointy = P(1);
867 if (P(2) < obb_min_pointz)
868 obb_min_pointz = P(2);
869 else if (P(2) > obb_max_pointz)
870 obb_max_pointz = P(2);
871 }
872 }
873 else
874 {
875 size_t i = 0;
876 for (; i<indices.size();++i)
877 {
878 const auto& point = cloud[indices[i]];
879 if (!isFinite(point))
880 continue;
881 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
882 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
883
884 obb_min_pointx = obb_max_pointx = P(0);
885 obb_min_pointy = obb_max_pointy = P(1);
886 obb_min_pointz = obb_max_pointz = P(2);
887 ++i;
888 break;
889 }
890
891 for (; i<indices.size();++i)
892 {
893 const auto& point = cloud[indices[i]];
894 if (!isFinite(point))
895 continue;
896
897 Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
898 Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
899
900 if (P(0) < obb_min_pointx)
901 obb_min_pointx = P(0);
902 else if (P(0) > obb_max_pointx)
903 obb_max_pointx = P(0);
904 if (P(1) < obb_min_pointy)
905 obb_min_pointy = P(1);
906 else if (P(1) > obb_max_pointy)
907 obb_max_pointy = P(1);
908 if (P(2) < obb_min_pointz)
909 obb_min_pointz = P(2);
910 else if (P(2) > obb_max_pointz)
911 obb_max_pointz = P(2);
912 }
913
914 }
915
916 const Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))
917 shift((obb_max_pointx + obb_min_pointx) / 2.0f,
918 (obb_max_pointy + obb_min_pointy) / 2.0f,
919 (obb_max_pointz + obb_min_pointz) / 2.0f);
920
921 obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
922 obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
923 obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
924
925 obb_center = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
926
927 return (point_count);
928}
929
930
931
932template <typename PointT, typename Scalar> void
934 const Eigen::Matrix<Scalar, 4, 1> &centroid,
935 pcl::PointCloud<PointT> &cloud_out,
936 int npts)
937{
938 // Calculate the number of points if not given
939 if (npts == 0)
940 {
941 while (cloud_iterator.isValid ())
942 {
943 ++npts;
944 ++cloud_iterator;
945 }
946 cloud_iterator.reset ();
947 }
948
949 int i = 0;
950 cloud_out.resize (npts);
951 // Subtract the centroid from cloud_in
952 while (cloud_iterator.isValid ())
953 {
954 cloud_out[i].x = cloud_iterator->x - centroid[0];
955 cloud_out[i].y = cloud_iterator->y - centroid[1];
956 cloud_out[i].z = cloud_iterator->z - centroid[2];
957 ++i;
958 ++cloud_iterator;
959 }
960 cloud_out.width = cloud_out.size ();
961 cloud_out.height = 1;
962}
963
964
965template <typename PointT, typename Scalar> void
967 const Eigen::Matrix<Scalar, 4, 1> &centroid,
968 pcl::PointCloud<PointT> &cloud_out)
969{
970 cloud_out = cloud_in;
971
972 // Subtract the centroid from cloud_in
973 for (auto& point: cloud_out)
974 {
975 point.x -= static_cast<float> (centroid[0]);
976 point.y -= static_cast<float> (centroid[1]);
977 point.z -= static_cast<float> (centroid[2]);
978 }
979}
980
981
982template <typename PointT, typename Scalar> void
984 const Indices &indices,
985 const Eigen::Matrix<Scalar, 4, 1> &centroid,
986 pcl::PointCloud<PointT> &cloud_out)
987{
988 cloud_out.header = cloud_in.header;
989 cloud_out.is_dense = cloud_in.is_dense;
990 if (indices.size () == cloud_in.size ())
991 {
992 cloud_out.width = cloud_in.width;
993 cloud_out.height = cloud_in.height;
994 }
995 else
996 {
997 cloud_out.width = indices.size ();
998 cloud_out.height = 1;
999 }
1000 cloud_out.resize (indices.size ());
1001
1002 // Subtract the centroid from cloud_in
1003 for (std::size_t i = 0; i < indices.size (); ++i)
1004 {
1005 cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
1006 cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
1007 cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
1008 }
1009}
1010
1011
1012template <typename PointT, typename Scalar> void
1014 const pcl::PointIndices& indices,
1015 const Eigen::Matrix<Scalar, 4, 1> &centroid,
1016 pcl::PointCloud<PointT> &cloud_out)
1017{
1018 return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
1019}
1020
1021
1022template <typename PointT, typename Scalar> void
1024 const Eigen::Matrix<Scalar, 4, 1> &centroid,
1025 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
1026 int npts)
1027{
1028 // Calculate the number of points if not given
1029 if (npts == 0)
1030 {
1031 while (cloud_iterator.isValid ())
1032 {
1033 ++npts;
1034 ++cloud_iterator;
1035 }
1036 cloud_iterator.reset ();
1037 }
1038
1039 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
1040
1041 int i = 0;
1042 while (cloud_iterator.isValid ())
1043 {
1044 cloud_out (0, i) = cloud_iterator->x - centroid[0];
1045 cloud_out (1, i) = cloud_iterator->y - centroid[1];
1046 cloud_out (2, i) = cloud_iterator->z - centroid[2];
1047 ++i;
1048 ++cloud_iterator;
1049 }
1050}
1051
1052
1053template <typename PointT, typename Scalar> void
1055 const Eigen::Matrix<Scalar, 4, 1> &centroid,
1056 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1057{
1058 std::size_t npts = cloud_in.size ();
1059
1060 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
1061
1062 for (std::size_t i = 0; i < npts; ++i)
1063 {
1064 cloud_out (0, i) = cloud_in[i].x - centroid[0];
1065 cloud_out (1, i) = cloud_in[i].y - centroid[1];
1066 cloud_out (2, i) = cloud_in[i].z - centroid[2];
1067 // One column at a time
1068 //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
1069 }
1070
1071 // Make sure we zero the 4th dimension out (1 row, N columns)
1072 //cloud_out.block (3, 0, 1, npts).setZero ();
1073}
1074
1075
1076template <typename PointT, typename Scalar> void
1078 const Indices &indices,
1079 const Eigen::Matrix<Scalar, 4, 1> &centroid,
1080 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1081{
1082 std::size_t npts = indices.size ();
1083
1084 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
1085
1086 for (std::size_t i = 0; i < npts; ++i)
1087 {
1088 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
1089 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
1090 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
1091 // One column at a time
1092 //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
1093 }
1094
1095 // Make sure we zero the 4th dimension out (1 row, N columns)
1096 //cloud_out.block (3, 0, 1, npts).setZero ();
1097}
1098
1099
1100template <typename PointT, typename Scalar> void
1102 const pcl::PointIndices &indices,
1103 const Eigen::Matrix<Scalar, 4, 1> &centroid,
1104 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1105{
1106 return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
1107}
1108
1109
1110template <typename PointT, typename Scalar> inline void
1112 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
1113{
1114 using FieldList = typename pcl::traits::fieldList<PointT>::type;
1115
1116 // Get the size of the fields
1117 centroid.setZero (boost::mpl::size<FieldList>::value);
1118
1119 if (cloud.empty ())
1120 return;
1121
1122 // Iterate over each point
1123 for (const auto& pt: cloud)
1124 {
1125 // Iterate over each dimension
1126 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
1127 }
1128 centroid /= static_cast<Scalar> (cloud.size ());
1129}
1130
1131
1132template <typename PointT, typename Scalar> inline void
1134 const Indices &indices,
1135 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
1136{
1137 using FieldList = typename pcl::traits::fieldList<PointT>::type;
1138
1139 // Get the size of the fields
1140 centroid.setZero (boost::mpl::size<FieldList>::value);
1141
1142 if (indices.empty ())
1143 return;
1144
1145 // Iterate over each point
1146 for (const auto& index: indices)
1147 {
1148 // Iterate over each dimension
1149 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
1150 }
1151 centroid /= static_cast<Scalar> (indices.size ());
1152}
1153
1154
1155template <typename PointT, typename Scalar> inline void
1157 const pcl::PointIndices &indices,
1158 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
1159{
1160 return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
1161}
1162
1163template <typename PointT> void
1164CentroidPoint<PointT>::add (const PointT& point)
1165{
1166 // Invoke add point on each accumulator
1167 boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
1168 ++num_points_;
1169}
1170
1171template <typename PointT>
1172template <typename PointOutT> void
1173CentroidPoint<PointT>::get (PointOutT& point) const
1174{
1175 if (num_points_ != 0)
1176 {
1177 // Filter accumulators so that only those that are compatible with
1178 // both PointT and requested point type remain
1179 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
1180 // Invoke get point on each accumulator in filtered list
1181 boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
1182 }
1183}
1184
1185
1186template <typename PointInT, typename PointOutT> std::size_t
1187computeCentroid (const pcl::PointCloud<PointInT>& cloud,
1188 PointOutT& centroid)
1189{
1190 pcl::CentroidPoint<PointInT> cp;
1191
1192 if (cloud.is_dense)
1193 for (const auto& point: cloud)
1194 cp.add (point);
1195 else
1196 for (const auto& point: cloud)
1197 if (pcl::isFinite (point))
1198 cp.add (point);
1199
1200 cp.get (centroid);
1201 return (cp.getSize ());
1202}
1203
1204
1205template <typename PointInT, typename PointOutT> std::size_t
1206computeCentroid (const pcl::PointCloud<PointInT>& cloud,
1207 const Indices& indices,
1208 PointOutT& centroid)
1209{
1210 pcl::CentroidPoint<PointInT> cp;
1211
1212 if (cloud.is_dense)
1213 for (const auto &index : indices)
1214 cp.add (cloud[index]);
1215 else
1216 for (const auto &index : indices)
1217 if (pcl::isFinite (cloud[index]))
1218 cp.add (cloud[index]);
1219
1220 cp.get (centroid);
1221 return (cp.getSize ());
1222}
1223
1224} // namespace pcl
1225
Define methods for centroid estimation and covariance matrix calculus.
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool empty() const
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
unsigned int computeCentroidAndOBB(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 1 > &centroid, Eigen::Matrix< Scalar, 3, 1 > &obb_center, Eigen::Matrix< Scalar, 3, 1 > &obb_dimensions, Eigen::Matrix< Scalar, 3, 3 > &obb_rotational_matrix)
Compute centroid, OBB (Oriented Bounding Box), PCA axes of a given set of points.
Definition centroid.hpp:663
void computeNDCentroid(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &centroid)
General, all purpose nD centroid estimation for a set of points using their indices.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition centroid.hpp:509
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition centroid.hpp:933
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
Definition centroid.hpp:269
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
Definition centroid.hpp:192
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:57
@ K
Definition norms.h:54
int cp(int from, int to)
Returns field copy operation code.
Definition repacks.hpp:56
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
Helper functor structure for n-D centroid estimation.
Definition centroid.h:898
A point structure representing Euclidean xyz coordinates, and the RGB color.