Point Cloud Library (PCL) 1.12.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
56compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
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
85compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
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
129compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
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
171compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
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
180computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
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
251computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
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
263computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
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
334computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
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
344computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
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
358computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
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
368computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
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
422computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
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
476computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
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
485computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
486 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
487 Eigen::Matrix<Scalar, 4, 1> &centroid)
488{
489 // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
490 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
491 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
492 Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
493 for(const auto& point: cloud)
494 if(isFinite(point)) {
495 K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
496 }
497 std::size_t point_count;
498 if (cloud.is_dense)
499 {
500 point_count = cloud.size ();
501 // For each point in the cloud
502 for (const auto& point: cloud)
503 {
504 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
505 accu [0] += x * x;
506 accu [1] += x * y;
507 accu [2] += x * z;
508 accu [3] += y * y;
509 accu [4] += y * z;
510 accu [5] += z * z;
511 accu [6] += x;
512 accu [7] += y;
513 accu [8] += z;
514 }
515 }
516 else
517 {
518 point_count = 0;
519 for (const auto& point: cloud)
520 {
521 if (!isFinite (point))
522 continue;
523
524 Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
525 accu [0] += x * x;
526 accu [1] += x * y;
527 accu [2] += x * z;
528 accu [3] += y * y;
529 accu [4] += y * z;
530 accu [5] += z * z;
531 accu [6] += x;
532 accu [7] += y;
533 accu [8] += z;
534 ++point_count;
535 }
536 }
537 if (point_count != 0)
538 {
539 accu /= static_cast<Scalar> (point_count);
540 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
541 centroid[3] = 1;
542 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
543 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
544 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
545 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
546 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
547 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
548 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
549 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
550 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
551 }
552 return (static_cast<unsigned int> (point_count));
553}
554
555
556template <typename PointT, typename Scalar> inline unsigned int
557computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
558 const Indices &indices,
559 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
560 Eigen::Matrix<Scalar, 4, 1> &centroid)
561{
562 // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
563 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
564 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
565 Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
566 for(const auto& index : indices)
567 if(isFinite(cloud[index])) {
568 K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
569 }
570 std::size_t point_count;
571 if (cloud.is_dense)
572 {
573 point_count = indices.size ();
574 for (const auto &index : indices)
575 {
576 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
577 accu [0] += x * x;
578 accu [1] += x * y;
579 accu [2] += x * z;
580 accu [3] += y * y;
581 accu [4] += y * z;
582 accu [5] += z * z;
583 accu [6] += x;
584 accu [7] += y;
585 accu [8] += z;
586 }
587 }
588 else
589 {
590 point_count = 0;
591 for (const auto &index : indices)
592 {
593 if (!isFinite (cloud[index]))
594 continue;
595
596 ++point_count;
597 Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
598 accu [0] += x * x;
599 accu [1] += x * y;
600 accu [2] += x * z;
601 accu [3] += y * y;
602 accu [4] += y * z;
603 accu [5] += z * z;
604 accu [6] += x;
605 accu [7] += y;
606 accu [8] += z;
607 }
608 }
609
610 if (point_count != 0)
611 {
612 accu /= static_cast<Scalar> (point_count);
613 centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
614 centroid[3] = 1;
615 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
616 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
617 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
618 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
619 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
620 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
621 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
622 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
623 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
624 }
625 return (static_cast<unsigned int> (point_count));
626}
627
628
629template <typename PointT, typename Scalar> inline unsigned int
630computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
631 const pcl::PointIndices &indices,
632 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
633 Eigen::Matrix<Scalar, 4, 1> &centroid)
634{
635 return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
636}
637
638
639template <typename PointT, typename Scalar> void
640demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
641 const Eigen::Matrix<Scalar, 4, 1> &centroid,
642 pcl::PointCloud<PointT> &cloud_out,
643 int npts)
644{
645 // Calculate the number of points if not given
646 if (npts == 0)
647 {
648 while (cloud_iterator.isValid ())
649 {
650 ++npts;
651 ++cloud_iterator;
652 }
653 cloud_iterator.reset ();
654 }
655
656 int i = 0;
657 cloud_out.resize (npts);
658 // Subtract the centroid from cloud_in
659 while (cloud_iterator.isValid ())
660 {
661 cloud_out[i].x = cloud_iterator->x - centroid[0];
662 cloud_out[i].y = cloud_iterator->y - centroid[1];
663 cloud_out[i].z = cloud_iterator->z - centroid[2];
664 ++i;
665 ++cloud_iterator;
666 }
667 cloud_out.width = cloud_out.size ();
668 cloud_out.height = 1;
669}
670
671
672template <typename PointT, typename Scalar> void
673demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
674 const Eigen::Matrix<Scalar, 4, 1> &centroid,
675 pcl::PointCloud<PointT> &cloud_out)
676{
677 cloud_out = cloud_in;
678
679 // Subtract the centroid from cloud_in
680 for (auto& point: cloud_out)
681 {
682 point.x -= static_cast<float> (centroid[0]);
683 point.y -= static_cast<float> (centroid[1]);
684 point.z -= static_cast<float> (centroid[2]);
685 }
686}
687
688
689template <typename PointT, typename Scalar> void
690demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
691 const Indices &indices,
692 const Eigen::Matrix<Scalar, 4, 1> &centroid,
693 pcl::PointCloud<PointT> &cloud_out)
694{
695 cloud_out.header = cloud_in.header;
696 cloud_out.is_dense = cloud_in.is_dense;
697 if (indices.size () == cloud_in.size ())
698 {
699 cloud_out.width = cloud_in.width;
700 cloud_out.height = cloud_in.height;
701 }
702 else
703 {
704 cloud_out.width = indices.size ();
705 cloud_out.height = 1;
706 }
707 cloud_out.resize (indices.size ());
708
709 // Subtract the centroid from cloud_in
710 for (std::size_t i = 0; i < indices.size (); ++i)
711 {
712 cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
713 cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
714 cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
715 }
716}
717
718
719template <typename PointT, typename Scalar> void
720demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
721 const pcl::PointIndices& indices,
722 const Eigen::Matrix<Scalar, 4, 1> &centroid,
723 pcl::PointCloud<PointT> &cloud_out)
724{
725 return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
726}
727
728
729template <typename PointT, typename Scalar> void
730demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
731 const Eigen::Matrix<Scalar, 4, 1> &centroid,
732 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
733 int npts)
734{
735 // Calculate the number of points if not given
736 if (npts == 0)
737 {
738 while (cloud_iterator.isValid ())
739 {
740 ++npts;
741 ++cloud_iterator;
742 }
743 cloud_iterator.reset ();
744 }
745
746 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
747
748 int i = 0;
749 while (cloud_iterator.isValid ())
750 {
751 cloud_out (0, i) = cloud_iterator->x - centroid[0];
752 cloud_out (1, i) = cloud_iterator->y - centroid[1];
753 cloud_out (2, i) = cloud_iterator->z - centroid[2];
754 ++i;
755 ++cloud_iterator;
756 }
757}
758
759
760template <typename PointT, typename Scalar> void
761demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
762 const Eigen::Matrix<Scalar, 4, 1> &centroid,
763 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
764{
765 std::size_t npts = cloud_in.size ();
766
767 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
768
769 for (std::size_t i = 0; i < npts; ++i)
770 {
771 cloud_out (0, i) = cloud_in[i].x - centroid[0];
772 cloud_out (1, i) = cloud_in[i].y - centroid[1];
773 cloud_out (2, i) = cloud_in[i].z - centroid[2];
774 // One column at a time
775 //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
776 }
777
778 // Make sure we zero the 4th dimension out (1 row, N columns)
779 //cloud_out.block (3, 0, 1, npts).setZero ();
780}
781
782
783template <typename PointT, typename Scalar> void
784demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
785 const Indices &indices,
786 const Eigen::Matrix<Scalar, 4, 1> &centroid,
787 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
788{
789 std::size_t npts = indices.size ();
790
791 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
792
793 for (std::size_t i = 0; i < npts; ++i)
794 {
795 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
796 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
797 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
798 // One column at a time
799 //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
800 }
801
802 // Make sure we zero the 4th dimension out (1 row, N columns)
803 //cloud_out.block (3, 0, 1, npts).setZero ();
804}
805
806
807template <typename PointT, typename Scalar> void
808demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
809 const pcl::PointIndices &indices,
810 const Eigen::Matrix<Scalar, 4, 1> &centroid,
811 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
812{
813 return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
814}
815
816
817template <typename PointT, typename Scalar> inline void
818computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
819 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
820{
821 using FieldList = typename pcl::traits::fieldList<PointT>::type;
822
823 // Get the size of the fields
824 centroid.setZero (boost::mpl::size<FieldList>::value);
825
826 if (cloud.empty ())
827 return;
828
829 // Iterate over each point
830 for (const auto& pt: cloud)
831 {
832 // Iterate over each dimension
833 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
834 }
835 centroid /= static_cast<Scalar> (cloud.size ());
836}
837
838
839template <typename PointT, typename Scalar> inline void
840computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
841 const Indices &indices,
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 (indices.empty ())
850 return;
851
852 // Iterate over each point
853 for (const auto& index: indices)
854 {
855 // Iterate over each dimension
856 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
857 }
858 centroid /= static_cast<Scalar> (indices.size ());
859}
860
861
862template <typename PointT, typename Scalar> inline void
863computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
864 const pcl::PointIndices &indices,
865 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
866{
867 return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
868}
869
870template <typename PointT> void
871CentroidPoint<PointT>::add (const PointT& point)
872{
873 // Invoke add point on each accumulator
874 boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
875 ++num_points_;
876}
877
878template <typename PointT>
879template <typename PointOutT> void
880CentroidPoint<PointT>::get (PointOutT& point) const
881{
882 if (num_points_ != 0)
883 {
884 // Filter accumulators so that only those that are compatible with
885 // both PointT and requested point type remain
886 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
887 // Invoke get point on each accumulator in filtered list
888 boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
889 }
890}
891
892
893template <typename PointInT, typename PointOutT> std::size_t
894computeCentroid (const pcl::PointCloud<PointInT>& cloud,
895 PointOutT& centroid)
896{
897 pcl::CentroidPoint<PointInT> cp;
898
899 if (cloud.is_dense)
900 for (const auto& point: cloud)
901 cp.add (point);
902 else
903 for (const auto& point: cloud)
904 if (pcl::isFinite (point))
905 cp.add (point);
906
907 cp.get (centroid);
908 return (cp.getSize ());
909}
910
911
912template <typename PointInT, typename PointOutT> std::size_t
913computeCentroid (const pcl::PointCloud<PointInT>& cloud,
914 const Indices& indices,
915 PointOutT& centroid)
916{
917 pcl::CentroidPoint<PointInT> cp;
918
919 if (cloud.is_dense)
920 for (const auto &index : indices)
921 cp.add (cloud[index]);
922 else
923 for (const auto &index : indices)
924 if (pcl::isFinite (cloud[index]))
925 cp.add (cloud[index]);
926
927 cp.get (centroid);
928 return (cp.getSize ());
929}
930
931} // namespace pcl
932
centroid.h
Define methods for centroid estimation and covariance matrix calculus.
pcl::CentroidPoint::get
void get(PointOutT &point) const
Retrieve the current centroid.
Definition centroid.hpp:880
pcl::CentroidPoint::add
void add(const PointT &point)
Add a new point to the centroid computation.
Definition centroid.hpp:871
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition cloud_iterator.h:121
pcl::ConstCloudIterator::size
std::size_t size() const
Size of the range the iterator is going through.
Definition cloud_iterator.hpp:537
pcl::computeCentroid
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:894
pcl::computeNDCentroid
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:818
pcl::computeMeanAndCovarianceMatrix
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
pcl::demeanPointCloud
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:640
pcl::computeCovarianceMatrixNormalized
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
pcl::computeCovarianceMatrix
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
pcl::compute3DCentroid
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
pcl::K
@ K
Definition norms.h:54
pcl::isFinite
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
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:675