Point Cloud Library (PCL) 1.13.0
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
47#include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
48#include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
49#include <boost/mpl/size.hpp> // for boost::mpl::size
50
51
52namespace pcl
53{
54
55template <typename PointT, typename Scalar> inline unsigned int
57 Eigen::Matrix<Scalar, 4, 1> &centroid)
58{
59 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
60
61 unsigned int cp = 0;
62
63 // For each point in the cloud
64 // If the data is dense, we don't need to check for NaN
65 while (cloud_iterator.isValid ())
66 {
67 // Check if the point is invalid
68 if (pcl::isFinite (*cloud_iterator))
69 {
70 accumulator[0] += cloud_iterator->x;
71 accumulator[1] += cloud_iterator->y;
72 accumulator[2] += cloud_iterator->z;
73 ++cp;
74 }
75 ++cloud_iterator;
76 }
77
78 if (cp > 0) {
79 centroid = accumulator;
80 centroid /= static_cast<Scalar> (cp);
81 centroid[3] = 1;
82 }
83 return (cp);
84}
85
86
87template <typename PointT, typename Scalar> inline unsigned int
89 Eigen::Matrix<Scalar, 4, 1> &centroid)
90{
91 if (cloud.empty ())
92 return (0);
93
94 // For each point in the cloud
95 // If the data is dense, we don't need to check for NaN
96 if (cloud.is_dense)
97 {
98 // Initialize to 0
99 centroid.setZero ();
100 for (const auto& point: cloud)
101 {
102 centroid[0] += point.x;
103 centroid[1] += point.y;
104 centroid[2] += point.z;
105 }
106 centroid /= static_cast<Scalar> (cloud.size ());
107 centroid[3] = 1;
108
109 return (static_cast<unsigned int> (cloud.size ()));
110 }
111 // NaN or Inf values could exist => check for them
112 unsigned int cp = 0;
113 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
114 for (const auto& point: cloud)
115 {
116 // Check if the point is invalid
117 if (!isFinite (point))
118 continue;
119
120 accumulator[0] += point.x;
121 accumulator[1] += point.y;
122 accumulator[2] += point.z;
123 ++cp;
124 }
125 if (cp > 0) {
126 centroid = accumulator;
127 centroid /= static_cast<Scalar> (cp);
128 centroid[3] = 1;
129 }
130
131 return (cp);
132}
133
134
135template <typename PointT, typename Scalar> inline unsigned int
137 const Indices &indices,
138 Eigen::Matrix<Scalar, 4, 1> &centroid)
139{
140 if (indices.empty ())
141 return (0);
142
143 // If the data is dense, we don't need to check for NaN
144 if (cloud.is_dense)
145 {
146 // Initialize to 0
147 centroid.setZero ();
148 for (const auto& index : indices)
149 {
150 centroid[0] += cloud[index].x;
151 centroid[1] += cloud[index].y;
152 centroid[2] += cloud[index].z;
153 }
154 centroid /= static_cast<Scalar> (indices.size ());
155 centroid[3] = 1;
156 return (static_cast<unsigned int> (indices.size ()));
157 }
158 // NaN or Inf values could exist => check for them
159 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
160 unsigned int cp = 0;
161 for (const auto& index : indices)
162 {
163 // Check if the point is invalid
164 if (!isFinite (cloud [index]))
165 continue;
166
167 accumulator[0] += cloud[index].x;
168 accumulator[1] += cloud[index].y;
169 accumulator[2] += cloud[index].z;
170 ++cp;
171 }
172 if (cp > 0) {
173 centroid = accumulator;
174 centroid /= static_cast<Scalar> (cp);
175 centroid[3] = 1;
176 }
177 return (cp);
178}
179
180
181template <typename PointT, typename Scalar> inline unsigned int
183 const pcl::PointIndices &indices,
184 Eigen::Matrix<Scalar, 4, 1> &centroid)
185{
186 return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
187}
188
189
190template <typename PointT, typename Scalar> inline unsigned
192 const Eigen::Matrix<Scalar, 4, 1> &centroid,
193 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
194{
195 if (cloud.empty ())
196 return (0);
197
198 unsigned point_count;
199 // If the data is dense, we don't need to check for NaN
200 if (cloud.is_dense)
201 {
202 covariance_matrix.setZero ();
203 point_count = static_cast<unsigned> (cloud.size ());
204 // For each point in the cloud
205 for (const auto& point: cloud)
206 {
207 Eigen::Matrix<Scalar, 4, 1> pt;
208 pt[0] = point.x - centroid[0];
209 pt[1] = point.y - centroid[1];
210 pt[2] = point.z - centroid[2];
211
212 covariance_matrix (1, 1) += pt.y () * pt.y ();
213 covariance_matrix (1, 2) += pt.y () * pt.z ();
214
215 covariance_matrix (2, 2) += pt.z () * pt.z ();
216
217 pt *= pt.x ();
218 covariance_matrix (0, 0) += pt.x ();
219 covariance_matrix (0, 1) += pt.y ();
220 covariance_matrix (0, 2) += pt.z ();
221 }
222 }
223 // NaN or Inf values could exist => check for them
224 else
225 {
226 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
227 temp_covariance_matrix.setZero();
228 point_count = 0;
229 // For each point in the cloud
230 for (const auto& point: cloud)
231 {
232 // Check if the point is invalid
233 if (!isFinite (point))
234 continue;
235
236 Eigen::Matrix<Scalar, 4, 1> pt;
237 pt[0] = point.x - centroid[0];
238 pt[1] = point.y - centroid[1];
239 pt[2] = point.z - centroid[2];
240
241 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
242 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
243
244 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
245
246 pt *= pt.x ();
247 temp_covariance_matrix (0, 0) += pt.x ();
248 temp_covariance_matrix (0, 1) += pt.y ();
249 temp_covariance_matrix (0, 2) += pt.z ();
250 ++point_count;
251 }
252 if (point_count > 0) {
253 covariance_matrix = temp_covariance_matrix;
254 }
255 }
256 if (point_count == 0) {
257 return 0;
258 }
259 covariance_matrix (1, 0) = covariance_matrix (0, 1);
260 covariance_matrix (2, 0) = covariance_matrix (0, 2);
261 covariance_matrix (2, 1) = covariance_matrix (1, 2);
262
263 return (point_count);
264}
265
266
267template <typename PointT, typename Scalar> inline unsigned int
269 const Eigen::Matrix<Scalar, 4, 1> &centroid,
270 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
271{
272 unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
273 if (point_count != 0)
274 covariance_matrix /= static_cast<Scalar> (point_count);
275 return (point_count);
276}
277
278
279template <typename PointT, typename Scalar> inline unsigned int
281 const Indices &indices,
282 const Eigen::Matrix<Scalar, 4, 1> &centroid,
283 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
284{
285 if (indices.empty ())
286 return (0);
287
288 std::size_t point_count;
289 // If the data is dense, we don't need to check for NaN
290 if (cloud.is_dense)
291 {
292 covariance_matrix.setZero ();
293 point_count = indices.size ();
294 // For each point in the cloud
295 for (const auto& idx: indices)
296 {
297 Eigen::Matrix<Scalar, 4, 1> pt;
298 pt[0] = cloud[idx].x - centroid[0];
299 pt[1] = cloud[idx].y - centroid[1];
300 pt[2] = cloud[idx].z - centroid[2];
301
302 covariance_matrix (1, 1) += pt.y () * pt.y ();
303 covariance_matrix (1, 2) += pt.y () * pt.z ();
304
305 covariance_matrix (2, 2) += pt.z () * pt.z ();
306
307 pt *= pt.x ();
308 covariance_matrix (0, 0) += pt.x ();
309 covariance_matrix (0, 1) += pt.y ();
310 covariance_matrix (0, 2) += pt.z ();
311 }
312 }
313 // NaN or Inf values could exist => check for them
314 else
315 {
316 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
317 temp_covariance_matrix.setZero ();
318 point_count = 0;
319 // For each point in the cloud
320 for (const auto &index : indices)
321 {
322 // Check if the point is invalid
323 if (!isFinite (cloud[index]))
324 continue;
325
326 Eigen::Matrix<Scalar, 4, 1> pt;
327 pt[0] = cloud[index].x - centroid[0];
328 pt[1] = cloud[index].y - centroid[1];
329 pt[2] = cloud[index].z - centroid[2];
330
331 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
332 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
333
334 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
335
336 pt *= pt.x ();
337 temp_covariance_matrix (0, 0) += pt.x ();
338 temp_covariance_matrix (0, 1) += pt.y ();
339 temp_covariance_matrix (0, 2) += pt.z ();
340 ++point_count;
341 }
342 if (point_count > 0) {
343 covariance_matrix = temp_covariance_matrix;
344 }
345 }
346 if (point_count == 0) {
347 return 0;
348 }
349 covariance_matrix (1, 0) = covariance_matrix (0, 1);
350 covariance_matrix (2, 0) = covariance_matrix (0, 2);
351 covariance_matrix (2, 1) = covariance_matrix (1, 2);
352 return (static_cast<unsigned int> (point_count));
353}
354
355
356template <typename PointT, typename Scalar> inline unsigned int
358 const pcl::PointIndices &indices,
359 const Eigen::Matrix<Scalar, 4, 1> &centroid,
360 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
361{
362 return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
363}
364
365
366template <typename PointT, typename Scalar> inline unsigned int
368 const Indices &indices,
369 const Eigen::Matrix<Scalar, 4, 1> &centroid,
370 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
371{
372 unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
373 if (point_count != 0)
374 covariance_matrix /= static_cast<Scalar> (point_count);
375
376 return (point_count);
377}
378
379
380template <typename PointT, typename Scalar> inline unsigned int
382 const pcl::PointIndices &indices,
383 const Eigen::Matrix<Scalar, 4, 1> &centroid,
384 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
385{
386 return computeCovarianceMatrixNormalized(cloud, indices.indices, centroid, covariance_matrix);
387}
388
389
390template <typename PointT, typename Scalar> inline unsigned int
392 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
393{
394 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
395 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
396
397 unsigned int point_count;
398 if (cloud.is_dense)
399 {
400 point_count = static_cast<unsigned int> (cloud.size ());
401 // For each point in the cloud
402 for (const auto& point: cloud)
403 {
404 accu [0] += point.x * point.x;
405 accu [1] += point.x * point.y;
406 accu [2] += point.x * point.z;
407 accu [3] += point.y * point.y;
408 accu [4] += point.y * point.z;
409 accu [5] += point.z * point.z;
410 }
411 }
412 else
413 {
414 point_count = 0;
415 for (const auto& point: cloud)
416 {
417 if (!isFinite (point))
418 continue;
419
420 accu [0] += point.x * point.x;
421 accu [1] += point.x * point.y;
422 accu [2] += point.x * point.z;
423 accu [3] += point.y * point.y;
424 accu [4] += point.y * point.z;
425 accu [5] += point.z * point.z;
426 ++point_count;
427 }
428 }
429
430 if (point_count != 0)
431 {
432 accu /= static_cast<Scalar> (point_count);
433 covariance_matrix.coeffRef (0) = accu [0];
434 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
435 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
436 covariance_matrix.coeffRef (4) = accu [3];
437 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
438 covariance_matrix.coeffRef (8) = accu [5];
439 }
440 return (point_count);
441}
442
443
444template <typename PointT, typename Scalar> inline unsigned int
446 const Indices &indices,
447 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
448{
449 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
450 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
451
452 unsigned int point_count;
453 if (cloud.is_dense)
454 {
455 point_count = static_cast<unsigned int> (indices.size ());
456 for (const auto &index : indices)
457 {
458 //const PointT& point = cloud[*iIt];
459 accu [0] += cloud[index].x * cloud[index].x;
460 accu [1] += cloud[index].x * cloud[index].y;
461 accu [2] += cloud[index].x * cloud[index].z;
462 accu [3] += cloud[index].y * cloud[index].y;
463 accu [4] += cloud[index].y * cloud[index].z;
464 accu [5] += cloud[index].z * cloud[index].z;
465 }
466 }
467 else
468 {
469 point_count = 0;
470 for (const auto &index : indices)
471 {
472 if (!isFinite (cloud[index]))
473 continue;
474
475 ++point_count;
476 accu [0] += cloud[index].x * cloud[index].x;
477 accu [1] += cloud[index].x * cloud[index].y;
478 accu [2] += cloud[index].x * cloud[index].z;
479 accu [3] += cloud[index].y * cloud[index].y;
480 accu [4] += cloud[index].y * cloud[index].z;
481 accu [5] += cloud[index].z * cloud[index].z;
482 }
483 }
484 if (point_count != 0)
485 {
486 accu /= static_cast<Scalar> (point_count);
487 covariance_matrix.coeffRef (0) = accu [0];
488 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
489 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
490 covariance_matrix.coeffRef (4) = accu [3];
491 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
492 covariance_matrix.coeffRef (8) = accu [5];
493 }
494 return (point_count);
495}
496
497
498template <typename PointT, typename Scalar> inline unsigned int
500 const pcl::PointIndices &indices,
501 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
502{
503 return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
504}
505
506
507template <typename PointT, typename Scalar> inline unsigned int
509 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
510 Eigen::Matrix<Scalar, 4, 1> &centroid)
511{
512 // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
513 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
514 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
515 Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
516 for(const auto& point: cloud)
517 if(isFinite(point)) {
518 K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
519 }
520 std::size_t point_count;
521 if (cloud.is_dense)
522 {
523 point_count = cloud.size ();
524 // For each point in the cloud
525 for (const auto& point: cloud)
526 {
527 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
528 accu [0] += x * x;
529 accu [1] += x * y;
530 accu [2] += x * z;
531 accu [3] += y * y;
532 accu [4] += y * z;
533 accu [5] += z * z;
534 accu [6] += x;
535 accu [7] += y;
536 accu [8] += z;
537 }
538 }
539 else
540 {
541 point_count = 0;
542 for (const auto& point: cloud)
543 {
544 if (!isFinite (point))
545 continue;
546
547 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
548 accu [0] += x * x;
549 accu [1] += x * y;
550 accu [2] += x * z;
551 accu [3] += y * y;
552 accu [4] += y * z;
553 accu [5] += z * z;
554 accu [6] += x;
555 accu [7] += y;
556 accu [8] += z;
557 ++point_count;
558 }
559 }
560 if (point_count != 0)
561 {
562 accu /= static_cast<Scalar> (point_count);
563 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
564 centroid[3] = 1;
565 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
566 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
567 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
568 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
569 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
570 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
571 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
572 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
573 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
574 }
575 return (static_cast<unsigned int> (point_count));
576}
577
578
579template <typename PointT, typename Scalar> inline unsigned int
581 const Indices &indices,
582 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
583 Eigen::Matrix<Scalar, 4, 1> &centroid)
584{
585 // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
586 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
587 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
588 Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
589 for(const auto& index : indices)
590 if(isFinite(cloud[index])) {
591 K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
592 }
593 std::size_t point_count;
594 if (cloud.is_dense)
595 {
596 point_count = indices.size ();
597 for (const auto &index : indices)
598 {
599 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
600 accu [0] += x * x;
601 accu [1] += x * y;
602 accu [2] += x * z;
603 accu [3] += y * y;
604 accu [4] += y * z;
605 accu [5] += z * z;
606 accu [6] += x;
607 accu [7] += y;
608 accu [8] += z;
609 }
610 }
611 else
612 {
613 point_count = 0;
614 for (const auto &index : indices)
615 {
616 if (!isFinite (cloud[index]))
617 continue;
618
619 ++point_count;
620 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
621 accu [0] += x * x;
622 accu [1] += x * y;
623 accu [2] += x * z;
624 accu [3] += y * y;
625 accu [4] += y * z;
626 accu [5] += z * z;
627 accu [6] += x;
628 accu [7] += y;
629 accu [8] += z;
630 }
631 }
632
633 if (point_count != 0)
634 {
635 accu /= static_cast<Scalar> (point_count);
636 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
637 centroid[3] = 1;
638 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
639 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
640 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
641 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
642 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
643 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
644 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
645 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
646 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
647 }
648 return (static_cast<unsigned int> (point_count));
649}
650
651
652template <typename PointT, typename Scalar> inline unsigned int
654 const pcl::PointIndices &indices,
655 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
656 Eigen::Matrix<Scalar, 4, 1> &centroid)
657{
658 return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
659}
660
661
662template <typename PointT, typename Scalar> void
664 const Eigen::Matrix<Scalar, 4, 1> &centroid,
665 pcl::PointCloud<PointT> &cloud_out,
666 int npts)
667{
668 // Calculate the number of points if not given
669 if (npts == 0)
670 {
671 while (cloud_iterator.isValid ())
672 {
673 ++npts;
674 ++cloud_iterator;
675 }
676 cloud_iterator.reset ();
677 }
678
679 int i = 0;
680 cloud_out.resize (npts);
681 // Subtract the centroid from cloud_in
682 while (cloud_iterator.isValid ())
683 {
684 cloud_out[i].x = cloud_iterator->x - centroid[0];
685 cloud_out[i].y = cloud_iterator->y - centroid[1];
686 cloud_out[i].z = cloud_iterator->z - centroid[2];
687 ++i;
688 ++cloud_iterator;
689 }
690 cloud_out.width = cloud_out.size ();
691 cloud_out.height = 1;
692}
693
694
695template <typename PointT, typename Scalar> void
697 const Eigen::Matrix<Scalar, 4, 1> &centroid,
698 pcl::PointCloud<PointT> &cloud_out)
699{
700 cloud_out = cloud_in;
701
702 // Subtract the centroid from cloud_in
703 for (auto& point: cloud_out)
704 {
705 point.x -= static_cast<float> (centroid[0]);
706 point.y -= static_cast<float> (centroid[1]);
707 point.z -= static_cast<float> (centroid[2]);
708 }
709}
710
711
712template <typename PointT, typename Scalar> void
714 const Indices &indices,
715 const Eigen::Matrix<Scalar, 4, 1> &centroid,
716 pcl::PointCloud<PointT> &cloud_out)
717{
718 cloud_out.header = cloud_in.header;
719 cloud_out.is_dense = cloud_in.is_dense;
720 if (indices.size () == cloud_in.size ())
721 {
722 cloud_out.width = cloud_in.width;
723 cloud_out.height = cloud_in.height;
724 }
725 else
726 {
727 cloud_out.width = indices.size ();
728 cloud_out.height = 1;
729 }
730 cloud_out.resize (indices.size ());
731
732 // Subtract the centroid from cloud_in
733 for (std::size_t i = 0; i < indices.size (); ++i)
734 {
735 cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
736 cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
737 cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
738 }
739}
740
741
742template <typename PointT, typename Scalar> void
744 const pcl::PointIndices& indices,
745 const Eigen::Matrix<Scalar, 4, 1> &centroid,
746 pcl::PointCloud<PointT> &cloud_out)
747{
748 return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
749}
750
751
752template <typename PointT, typename Scalar> void
754 const Eigen::Matrix<Scalar, 4, 1> &centroid,
755 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
756 int npts)
757{
758 // Calculate the number of points if not given
759 if (npts == 0)
760 {
761 while (cloud_iterator.isValid ())
762 {
763 ++npts;
764 ++cloud_iterator;
765 }
766 cloud_iterator.reset ();
767 }
768
769 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
770
771 int i = 0;
772 while (cloud_iterator.isValid ())
773 {
774 cloud_out (0, i) = cloud_iterator->x - centroid[0];
775 cloud_out (1, i) = cloud_iterator->y - centroid[1];
776 cloud_out (2, i) = cloud_iterator->z - centroid[2];
777 ++i;
778 ++cloud_iterator;
779 }
780}
781
782
783template <typename PointT, typename Scalar> void
785 const Eigen::Matrix<Scalar, 4, 1> &centroid,
786 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
787{
788 std::size_t npts = cloud_in.size ();
789
790 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
791
792 for (std::size_t i = 0; i < npts; ++i)
793 {
794 cloud_out (0, i) = cloud_in[i].x - centroid[0];
795 cloud_out (1, i) = cloud_in[i].y - centroid[1];
796 cloud_out (2, i) = cloud_in[i].z - centroid[2];
797 // One column at a time
798 //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
799 }
800
801 // Make sure we zero the 4th dimension out (1 row, N columns)
802 //cloud_out.block (3, 0, 1, npts).setZero ();
803}
804
805
806template <typename PointT, typename Scalar> void
808 const Indices &indices,
809 const Eigen::Matrix<Scalar, 4, 1> &centroid,
810 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
811{
812 std::size_t npts = indices.size ();
813
814 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
815
816 for (std::size_t i = 0; i < npts; ++i)
817 {
818 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
819 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
820 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
821 // One column at a time
822 //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
823 }
824
825 // Make sure we zero the 4th dimension out (1 row, N columns)
826 //cloud_out.block (3, 0, 1, npts).setZero ();
827}
828
829
830template <typename PointT, typename Scalar> void
832 const pcl::PointIndices &indices,
833 const Eigen::Matrix<Scalar, 4, 1> &centroid,
834 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
835{
836 return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
837}
838
839
840template <typename PointT, typename Scalar> inline void
842 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
843{
844 using FieldList = typename pcl::traits::fieldList<PointT>::type;
845
846 // Get the size of the fields
847 centroid.setZero (boost::mpl::size<FieldList>::value);
848
849 if (cloud.empty ())
850 return;
851
852 // Iterate over each point
853 for (const auto& pt: cloud)
854 {
855 // Iterate over each dimension
856 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
857 }
858 centroid /= static_cast<Scalar> (cloud.size ());
859}
860
861
862template <typename PointT, typename Scalar> inline void
864 const Indices &indices,
865 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
866{
867 using FieldList = typename pcl::traits::fieldList<PointT>::type;
868
869 // Get the size of the fields
870 centroid.setZero (boost::mpl::size<FieldList>::value);
871
872 if (indices.empty ())
873 return;
874
875 // Iterate over each point
876 for (const auto& index: indices)
877 {
878 // Iterate over each dimension
879 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
880 }
881 centroid /= static_cast<Scalar> (indices.size ());
882}
883
884
885template <typename PointT, typename Scalar> inline void
887 const pcl::PointIndices &indices,
888 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
889{
890 return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
891}
892
893template <typename PointT> void
895{
896 // Invoke add point on each accumulator
897 boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
898 ++num_points_;
899}
900
901template <typename PointT>
902template <typename PointOutT> void
903CentroidPoint<PointT>::get (PointOutT& point) const
904{
905 if (num_points_ != 0)
906 {
907 // Filter accumulators so that only those that are compatible with
908 // both PointT and requested point type remain
909 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
910 // Invoke get point on each accumulator in filtered list
911 boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
912 }
913}
914
915
916template <typename PointInT, typename PointOutT> std::size_t
918 PointOutT& centroid)
919{
921
922 if (cloud.is_dense)
923 for (const auto& point: cloud)
924 cp.add (point);
925 else
926 for (const auto& point: cloud)
927 if (pcl::isFinite (point))
928 cp.add (point);
929
930 cp.get (centroid);
931 return (cp.getSize ());
932}
933
934
935template <typename PointInT, typename PointOutT> std::size_t
937 const Indices& indices,
938 PointOutT& centroid)
939{
941
942 if (cloud.is_dense)
943 for (const auto &index : indices)
944 cp.add (cloud[index]);
945 else
946 for (const auto &index : indices)
947 if (pcl::isFinite (cloud[index]))
948 cp.add (cloud[index]);
949
950 cp.get (centroid);
951 return (cp.getSize ());
952}
953
954} // namespace pcl
955
Define methods for centroid estimation and covariance matrix calculus.
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1023
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:903
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:894
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
std::size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT &centroid)
Compute the centroid of a set of points and return it as a point.
Definition: centroid.hpp:917
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.
Definition: centroid.hpp:841
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:508
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:663
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:268
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:191
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:56
@ K
Definition: norms.h:54
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:843
A point structure representing Euclidean xyz coordinates, and the RGB color.