Point Cloud Library (PCL) 1.13.1
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
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
894CentroidPoint<PointT>::add (const PointT& point)
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
917computeCentroid (const pcl::PointCloud<PointInT>& cloud,
918 PointOutT& centroid)
919{
920 pcl::CentroidPoint<PointInT> cp;
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
936computeCentroid (const pcl::PointCloud<PointInT>& cloud,
937 const Indices& indices,
938 PointOutT& centroid)
939{
940 pcl::CentroidPoint<PointInT> cp;
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.
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
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
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:843
A point structure representing Euclidean xyz coordinates, and the RGB color.