Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
centroid.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-present, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/common/centroid.h>
44#include <pcl/conversions.h>
45#include <pcl/common/point_tests.h> // for pcl::isFinite
46
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
858CentroidPoint<PointT>::add (const PointT& point)
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
881computeCentroid (const pcl::PointCloud<PointInT>& cloud,
882 PointOutT& centroid)
883{
884 pcl::CentroidPoint<PointInT> cp;
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
900computeCentroid (const pcl::PointCloud<PointInT>& cloud,
901 const Indices& indices,
902 PointOutT& centroid)
903{
904 pcl::CentroidPoint<PointInT> cp;
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.
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: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
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.