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