Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
centroid.h
Go to the documentation of this file.
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010, 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 */
38
39#pragma once
40
41#include <pcl/memory.h>
42#include <pcl/pcl_macros.h>
43#include <pcl/point_cloud.h>
44#include <pcl/type_traits.h>
45#include <pcl/PointIndices.h>
46#include <pcl/cloud_iterator.h>
47
48/**
49 * \file pcl/common/centroid.h
50 * Define methods for centroid estimation and covariance matrix calculus
51 * \ingroup common
52 */
53
54/*@{*/
55namespace pcl
56{
57 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
58 * \param[in] cloud_iterator an iterator over the input point cloud
59 * \param[out] centroid the output centroid
60 * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
61 * \note if return value is 0, the centroid is not changed, thus not valid.
62 * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices.
63 * \ingroup common
64 */
65 template <typename PointT, typename Scalar> inline unsigned int
66 compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
67 Eigen::Matrix<Scalar, 4, 1> &centroid);
68
69 template <typename PointT> inline unsigned int
71 Eigen::Vector4f &centroid)
72 {
74 }
75
76 template <typename PointT> inline unsigned int
78 Eigen::Vector4d &centroid)
79 {
81 }
82
83 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
84 * \param[in] cloud the input point cloud
85 * \param[out] centroid the output centroid
86 * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
87 * \note if return value is 0, the centroid is not changed, thus not valid.
88 * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices.
89 * \ingroup common
90 */
91 template <typename PointT, typename Scalar> inline unsigned int
93 Eigen::Matrix<Scalar, 4, 1> &centroid);
94
95 template <typename PointT> inline unsigned int
97 Eigen::Vector4f &centroid)
98 {
99 return (compute3DCentroid <PointT, float> (cloud, centroid));
100 }
101
102 template <typename PointT> inline unsigned int
104 Eigen::Vector4d &centroid)
105 {
106 return (compute3DCentroid <PointT, double> (cloud, centroid));
107 }
108
109 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and
110 * return it as a 3D vector.
111 * \param[in] cloud the input point cloud
112 * \param[in] indices the point cloud indices that need to be used
113 * \param[out] centroid the output centroid
114 * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input indices.
115 * \note if return value is 0, the centroid is not changed, thus not valid.
116 * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices.
117 * \ingroup common
118 */
119 template <typename PointT, typename Scalar> inline unsigned int
121 const Indices &indices,
122 Eigen::Matrix<Scalar, 4, 1> &centroid);
123
124 template <typename PointT> inline unsigned int
126 const Indices &indices,
127 Eigen::Vector4f &centroid)
128 {
129 return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
130 }
131
132 template <typename PointT> inline unsigned int
134 const Indices &indices,
135 Eigen::Vector4d &centroid)
136 {
137 return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
138 }
139
140 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and
141 * return it as a 3D vector.
142 * \param[in] cloud the input point cloud
143 * \param[in] indices the point cloud indices that need to be used
144 * \param[out] centroid the output centroid
145 * \return number of valid points used to determine the centroid. In case of dense point clouds, this is the same as the size of input indices.
146 * \note if return value is 0, the centroid is not changed, thus not valid.
147 * The last component of the vector is set to 1, this allows to transform the centroid vector with 4x4 matrices.
148 * \ingroup common
149 */
150 template <typename PointT, typename Scalar> inline unsigned int
152 const pcl::PointIndices &indices,
153 Eigen::Matrix<Scalar, 4, 1> &centroid);
154
155 template <typename PointT> inline unsigned int
157 const pcl::PointIndices &indices,
158 Eigen::Vector4f &centroid)
159 {
160 return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
161 }
162
163 template <typename PointT> inline unsigned int
165 const pcl::PointIndices &indices,
166 Eigen::Vector4d &centroid)
167 {
168 return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
169 }
170
171 /** \brief Compute the 3x3 covariance matrix of a given set of points.
172 * The result is returned as a Eigen::Matrix3f.
173 * Note: the covariance matrix is not normalized with the number of
174 * points. For a normalized covariance, please use
175 * computeCovarianceMatrixNormalized.
176 * \param[in] cloud the input point cloud
177 * \param[in] centroid the centroid of the set of points in the cloud
178 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
179 * \return number of valid points used to determine the covariance matrix.
180 * In case of dense point clouds, this is the same as the size of input cloud.
181 * \note if return value is 0, the covariance matrix is not changed, thus not valid.
182 * \ingroup common
183 */
184 template <typename PointT, typename Scalar> inline unsigned int
186 const Eigen::Matrix<Scalar, 4, 1> &centroid,
187 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
188
189 template <typename PointT> inline unsigned int
191 const Eigen::Vector4f &centroid,
192 Eigen::Matrix3f &covariance_matrix)
193 {
195 }
196
197 template <typename PointT> inline unsigned int
199 const Eigen::Vector4d &centroid,
200 Eigen::Matrix3d &covariance_matrix)
201 {
203 }
204
205 /** \brief Compute normalized the 3x3 covariance matrix of a given set of points.
206 * The result is returned as a Eigen::Matrix3f.
207 * Normalized means that every entry has been divided by the number of points in the point cloud.
208 * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix
209 * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
210 * the covariance matrix and is returned by the computeCovarianceMatrix function.
211 * \param[in] cloud the input point cloud
212 * \param[in] centroid the centroid of the set of points in the cloud
213 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
214 * \return number of valid points used to determine the covariance matrix.
215 * In case of dense point clouds, this is the same as the size of input cloud.
216 * \ingroup common
217 */
218 template <typename PointT, typename Scalar> inline unsigned int
220 const Eigen::Matrix<Scalar, 4, 1> &centroid,
221 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
222
223 template <typename PointT> inline unsigned int
225 const Eigen::Vector4f &centroid,
226 Eigen::Matrix3f &covariance_matrix)
227 {
229 }
230
231 template <typename PointT> inline unsigned int
233 const Eigen::Vector4d &centroid,
234 Eigen::Matrix3d &covariance_matrix)
235 {
237 }
238
239 /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
240 * The result is returned as a Eigen::Matrix3f.
241 * Note: the covariance matrix is not normalized with the number of
242 * points. For a normalized covariance, please use
243 * computeCovarianceMatrixNormalized.
244 * \param[in] cloud the input point cloud
245 * \param[in] indices the point cloud indices that need to be used
246 * \param[in] centroid the centroid of the set of points in the cloud
247 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
248 * \return number of valid points used to determine the covariance matrix.
249 * In case of dense point clouds, this is the same as the size of input indices.
250 * \ingroup common
251 */
252 template <typename PointT, typename Scalar> inline unsigned int
254 const Indices &indices,
255 const Eigen::Matrix<Scalar, 4, 1> &centroid,
256 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
257
258 template <typename PointT> inline unsigned int
260 const Indices &indices,
261 const Eigen::Vector4f &centroid,
262 Eigen::Matrix3f &covariance_matrix)
263 {
264 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
265 }
266
267 template <typename PointT> inline unsigned int
269 const Indices &indices,
270 const Eigen::Vector4d &centroid,
271 Eigen::Matrix3d &covariance_matrix)
272 {
273 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
274 }
275
276 /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
277 * The result is returned as a Eigen::Matrix3f.
278 * Note: the covariance matrix is not normalized with the number of
279 * points. For a normalized covariance, please use
280 * computeCovarianceMatrixNormalized.
281 * \param[in] cloud the input point cloud
282 * \param[in] indices the point cloud indices that need to be used
283 * \param[in] centroid the centroid of the set of points in the cloud
284 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
285 * \return number of valid points used to determine the covariance matrix.
286 * In case of dense point clouds, this is the same as the size of input indices.
287 * \ingroup common
288 */
289 template <typename PointT, typename Scalar> inline unsigned int
291 const pcl::PointIndices &indices,
292 const Eigen::Matrix<Scalar, 4, 1> &centroid,
293 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
294
295 template <typename PointT> inline unsigned int
297 const pcl::PointIndices &indices,
298 const Eigen::Vector4f &centroid,
299 Eigen::Matrix3f &covariance_matrix)
300 {
301 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
302 }
303
304 template <typename PointT> inline unsigned int
306 const pcl::PointIndices &indices,
307 const Eigen::Vector4d &centroid,
308 Eigen::Matrix3d &covariance_matrix)
309 {
310 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
311 }
312
313 /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using
314 * their indices.
315 * The result is returned as a Eigen::Matrix3f.
316 * Normalized means that every entry has been divided by the number of entries in indices.
317 * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix
318 * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
319 * the covariance matrix and is returned by the computeCovarianceMatrix function.
320 * \param[in] cloud the input point cloud
321 * \param[in] indices the point cloud indices that need to be used
322 * \param[in] centroid the centroid of the set of points in the cloud
323 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
324 * \return number of valid points used to determine the covariance matrix.
325 * In case of dense point clouds, this is the same as the size of input indices.
326 * \ingroup common
327 */
328 template <typename PointT, typename Scalar> inline unsigned int
330 const Indices &indices,
331 const Eigen::Matrix<Scalar, 4, 1> &centroid,
332 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
333
334 template <typename PointT> inline unsigned int
336 const Indices &indices,
337 const Eigen::Vector4f &centroid,
338 Eigen::Matrix3f &covariance_matrix)
339 {
340 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
341 }
342
343 template <typename PointT> inline unsigned int
345 const Indices &indices,
346 const Eigen::Vector4d &centroid,
347 Eigen::Matrix3d &covariance_matrix)
348 {
349 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
350 }
351
352 /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using
353 * their indices. The result is returned as a Eigen::Matrix3f.
354 * Normalized means that every entry has been divided by the number of entries in indices.
355 * For small number of points, or if you want explicitly the sample-variance, use computeCovarianceMatrix
356 * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
357 * the covariance matrix and is returned by the computeCovarianceMatrix function.
358 * \param[in] cloud the input point cloud
359 * \param[in] indices the point cloud indices that need to be used
360 * \param[in] centroid the centroid of the set of points in the cloud
361 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
362 * \return number of valid points used to determine the covariance matrix.
363 * In case of dense point clouds, this is the same as the size of input indices.
364 * \ingroup common
365 */
366 template <typename PointT, typename Scalar> inline unsigned int
368 const pcl::PointIndices &indices,
369 const Eigen::Matrix<Scalar, 4, 1> &centroid,
370 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
371
372 template <typename PointT> inline unsigned int
374 const pcl::PointIndices &indices,
375 const Eigen::Vector4f &centroid,
376 Eigen::Matrix3f &covariance_matrix)
377 {
378 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
379 }
380
381 template <typename PointT> inline unsigned int
383 const pcl::PointIndices &indices,
384 const Eigen::Vector4d &centroid,
385 Eigen::Matrix3d &covariance_matrix)
386 {
387 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
388 }
389
390 /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
391 * Normalized means that every entry has been divided by the number of valid entries in the point cloud.
392 * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix
393 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
394 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
395 * \param[in] cloud the input point cloud
396 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
397 * \param[out] centroid the centroid of the set of points in the cloud
398 * \return number of valid points used to determine the covariance matrix.
399 * In case of dense point clouds, this is the same as the size of input cloud.
400 * \ingroup common
401 */
402 template <typename PointT, typename Scalar> inline unsigned int
404 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
405 Eigen::Matrix<Scalar, 4, 1> &centroid);
406
407 template <typename PointT> inline unsigned int
409 Eigen::Matrix3f &covariance_matrix,
410 Eigen::Vector4f &centroid)
411 {
413 }
414
415 template <typename PointT> inline unsigned int
417 Eigen::Matrix3d &covariance_matrix,
418 Eigen::Vector4d &centroid)
419 {
421 }
422
423 /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
424 * Normalized means that every entry has been divided by the number of entries in indices.
425 * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix
426 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
427 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
428 * \param[in] cloud the input point cloud
429 * \param[in] indices subset of points given by their indices
430 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
431 * \param[out] centroid the centroid of the set of points in the cloud
432 * \return number of valid points used to determine the covariance matrix.
433 * In case of dense point clouds, this is the same as the size of input indices.
434 * \ingroup common
435 */
436 template <typename PointT, typename Scalar> inline unsigned int
438 const Indices &indices,
439 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
440 Eigen::Matrix<Scalar, 4, 1> &centroid);
441
442 template <typename PointT> inline unsigned int
444 const Indices &indices,
445 Eigen::Matrix3f &covariance_matrix,
446 Eigen::Vector4f &centroid)
447 {
448 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
449 }
450
451 template <typename PointT> inline unsigned int
453 const Indices &indices,
454 Eigen::Matrix3d &covariance_matrix,
455 Eigen::Vector4d &centroid)
456 {
457 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
458 }
459
460 /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
461 * Normalized means that every entry has been divided by the number of entries in indices.
462 * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix
463 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
464 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
465 * \param[in] cloud the input point cloud
466 * \param[in] indices subset of points given by their indices
467 * \param[out] centroid the centroid of the set of points in the cloud
468 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
469 * \return number of valid points used to determine the covariance matrix.
470 * In case of dense point clouds, this is the same as the size of input indices.
471 * \ingroup common
472 */
473 template <typename PointT, typename Scalar> inline unsigned int
475 const pcl::PointIndices &indices,
476 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
477 Eigen::Matrix<Scalar, 4, 1> &centroid);
478
479 template <typename PointT> inline unsigned int
481 const pcl::PointIndices &indices,
482 Eigen::Matrix3f &covariance_matrix,
483 Eigen::Vector4f &centroid)
484 {
485 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
486 }
487
488 template <typename PointT> inline unsigned int
490 const pcl::PointIndices &indices,
491 Eigen::Matrix3d &covariance_matrix,
492 Eigen::Vector4d &centroid)
493 {
494 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
495 }
496
497 /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
498 * Normalized means that every entry has been divided by the number of entries in the input point cloud.
499 * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix
500 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
501 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
502 * \param[in] cloud the input point cloud
503 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
504 * \return number of valid points used to determine the covariance matrix.
505 * In case of dense point clouds, this is the same as the size of input cloud.
506 * \ingroup common
507 */
508 template <typename PointT, typename Scalar> inline unsigned int
510 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
511
512 template <typename PointT> inline unsigned int
518
519 template <typename PointT> inline unsigned int
525
526 /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
527 * Normalized means that every entry has been divided by the number of entries in indices.
528 * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix
529 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
530 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
531 * \param[in] cloud the input point cloud
532 * \param[in] indices subset of points given by their indices
533 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
534 * \return number of valid points used to determine the covariance matrix.
535 * In case of dense point clouds, this is the same as the size of input indices.
536 * \ingroup common
537 */
538 template <typename PointT, typename Scalar> inline unsigned int
540 const Indices &indices,
541 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
542
543 template <typename PointT> inline unsigned int
545 const Indices &indices,
546 Eigen::Matrix3f &covariance_matrix)
547 {
549 }
550
551 template <typename PointT> inline unsigned int
553 const Indices &indices,
554 Eigen::Matrix3d &covariance_matrix)
555 {
557 }
558
559 /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
560 * Normalized means that every entry has been divided by the number of entries in indices.
561 * For small number of points, or if you want explicitly the sample-variance, scale the covariance matrix
562 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
563 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
564 * \param[in] cloud the input point cloud
565 * \param[in] indices subset of points given by their indices
566 * \param[out] covariance_matrix the resultant 3x3 covariance matrix
567 * \return number of valid points used to determine the covariance matrix.
568 * In case of dense point clouds, this is the same as the size of input indices.
569 * \ingroup common
570 */
571 template <typename PointT, typename Scalar> inline unsigned int
573 const pcl::PointIndices &indices,
574 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
575
576 template <typename PointT> inline unsigned int
578 const pcl::PointIndices &indices,
579 Eigen::Matrix3f &covariance_matrix)
580 {
582 }
583
584 template <typename PointT> inline unsigned int
586 const pcl::PointIndices &indices,
587 Eigen::Matrix3d &covariance_matrix)
588 {
590 }
591
592 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
593 * \param[in] cloud_iterator an iterator over the input point cloud
594 * \param[in] centroid the centroid of the point cloud
595 * \param[out] cloud_out the resultant output point cloud
596 * \param[in] npts the number of samples guaranteed to be left in the input cloud, accessible by the iterator. If not given, it will be calculated.
597 * \ingroup common
598 */
599 template <typename PointT, typename Scalar> void
600 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
601 const Eigen::Matrix<Scalar, 4, 1> &centroid,
602 pcl::PointCloud<PointT> &cloud_out,
603 int npts = 0);
604
605 template <typename PointT> void
607 const Eigen::Vector4f &centroid,
609 int npts = 0)
610 {
612 }
613
614 template <typename PointT> void
616 const Eigen::Vector4d &centroid,
618 int npts = 0)
619 {
621 }
622
623 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
624 * \param[in] cloud_in the input point cloud
625 * \param[in] centroid the centroid of the point cloud
626 * \param[out] cloud_out the resultant output point cloud
627 * \ingroup common
628 */
629 template <typename PointT, typename Scalar> void
631 const Eigen::Matrix<Scalar, 4, 1> &centroid,
632 pcl::PointCloud<PointT> &cloud_out);
633
634 template <typename PointT> void
641
642 template <typename PointT> void
649
650 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
651 * \param[in] cloud_in the input point cloud
652 * \param[in] indices the set of point indices to use from the input point cloud
653 * \param[out] centroid the centroid of the point cloud
654 * \param cloud_out the resultant output point cloud
655 * \ingroup common
656 */
657 template <typename PointT, typename Scalar> void
659 const Indices &indices,
660 const Eigen::Matrix<Scalar, 4, 1> &centroid,
661 pcl::PointCloud<PointT> &cloud_out);
662
663 template <typename PointT> void
665 const Indices &indices,
666 const Eigen::Vector4f &centroid,
668 {
669 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
670 }
671
672 template <typename PointT> void
674 const Indices &indices,
675 const Eigen::Vector4d &centroid,
677 {
678 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
679 }
680
681 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
682 * \param[in] cloud_in the input point cloud
683 * \param[in] indices the set of point indices to use from the input point cloud
684 * \param[out] centroid the centroid of the point cloud
685 * \param cloud_out the resultant output point cloud
686 * \ingroup common
687 */
688 template <typename PointT, typename Scalar> void
690 const pcl::PointIndices& indices,
691 const Eigen::Matrix<Scalar, 4, 1> &centroid,
692 pcl::PointCloud<PointT> &cloud_out);
693
694 template <typename PointT> void
696 const pcl::PointIndices& indices,
697 const Eigen::Vector4f &centroid,
699 {
700 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
701 }
702
703 template <typename PointT> void
705 const pcl::PointIndices& indices,
706 const Eigen::Vector4d &centroid,
708 {
709 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
710 }
711
712 /** \brief Subtract a centroid from a point cloud and return the de-meaned
713 * representation as an Eigen matrix
714 * \param[in] cloud_iterator an iterator over the input point cloud
715 * \param[in] centroid the centroid of the point cloud
716 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
717 * an Eigen matrix (4 rows, N pts columns)
718 * \param[in] npts the number of samples guaranteed to be left in the input cloud, accessible by the iterator. If not given, it will be calculated.
719 * \ingroup common
720 */
721 template <typename PointT, typename Scalar> void
722 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
723 const Eigen::Matrix<Scalar, 4, 1> &centroid,
724 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
725 int npts = 0);
726
727 template <typename PointT> void
729 const Eigen::Vector4f &centroid,
730 Eigen::MatrixXf &cloud_out,
731 int npts = 0)
732 {
734 }
735
736 template <typename PointT> void
738 const Eigen::Vector4d &centroid,
739 Eigen::MatrixXd &cloud_out,
740 int npts = 0)
741 {
743 }
744
745 /** \brief Subtract a centroid from a point cloud and return the de-meaned
746 * representation as an Eigen matrix
747 * \param[in] cloud_in the input point cloud
748 * \param[in] centroid the centroid of the point cloud
749 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
750 * an Eigen matrix (4 rows, N pts columns)
751 * \ingroup common
752 */
753 template <typename PointT, typename Scalar> void
755 const Eigen::Matrix<Scalar, 4, 1> &centroid,
756 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
757
758 template <typename PointT> void
760 const Eigen::Vector4f &centroid,
761 Eigen::MatrixXf &cloud_out)
762 {
764 }
765
766 template <typename PointT> void
768 const Eigen::Vector4d &centroid,
769 Eigen::MatrixXd &cloud_out)
770 {
772 }
773
774 /** \brief Subtract a centroid from a point cloud and return the de-meaned
775 * representation as an Eigen matrix
776 * \param[in] cloud_in the input point cloud
777 * \param[in] indices the set of point indices to use from the input point cloud
778 * \param[in] centroid the centroid of the point cloud
779 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
780 * an Eigen matrix (4 rows, N pts columns)
781 * \ingroup common
782 */
783 template <typename PointT, typename Scalar> void
785 const Indices &indices,
786 const Eigen::Matrix<Scalar, 4, 1> &centroid,
787 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
788
789 template <typename PointT> void
791 const Indices &indices,
792 const Eigen::Vector4f &centroid,
793 Eigen::MatrixXf &cloud_out)
794 {
795 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
796 }
797
798 template <typename PointT> void
800 const Indices &indices,
801 const Eigen::Vector4d &centroid,
802 Eigen::MatrixXd &cloud_out)
803 {
804 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
805 }
806
807 /** \brief Subtract a centroid from a point cloud and return the de-meaned
808 * representation as an Eigen matrix
809 * \param[in] cloud_in the input point cloud
810 * \param[in] indices the set of point indices to use from the input point cloud
811 * \param[in] centroid the centroid of the point cloud
812 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
813 * an Eigen matrix (4 rows, N pts columns)
814 * \ingroup common
815 */
816 template <typename PointT, typename Scalar> void
818 const pcl::PointIndices& indices,
819 const Eigen::Matrix<Scalar, 4, 1> &centroid,
820 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
821
822 template <typename PointT> void
824 const pcl::PointIndices& indices,
825 const Eigen::Vector4f &centroid,
826 Eigen::MatrixXf &cloud_out)
827 {
828 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
829 }
830
831 template <typename PointT> void
833 const pcl::PointIndices& indices,
834 const Eigen::Vector4d &centroid,
835 Eigen::MatrixXd &cloud_out)
836 {
837 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
838 }
839
840 /** \brief Helper functor structure for n-D centroid estimation. */
841 template<typename PointT, typename Scalar>
843 {
844 using Pod = typename traits::POD<PointT>::type;
845
846 NdCentroidFunctor (const PointT &p, Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
847 : f_idx_ (0),
848 centroid_ (centroid),
849 p_ (reinterpret_cast<const Pod&>(p)) { }
850
851 template<typename Key> inline void operator() ()
852 {
853 using T = typename pcl::traits::datatype<PointT, Key>::type;
854 const std::uint8_t* raw_ptr = reinterpret_cast<const std::uint8_t*>(&p_) + pcl::traits::offset<PointT, Key>::value;
855 const T* data_ptr = reinterpret_cast<const T*>(raw_ptr);
856
857 // Check if the value is invalid
858 if (!std::isfinite (*data_ptr))
859 {
860 f_idx_++;
861 return;
862 }
863
864 centroid_[f_idx_++] += *data_ptr;
865 }
866
867 private:
868 int f_idx_;
869 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid_;
870 const Pod &p_;
871 };
872
873 /** \brief General, all purpose nD centroid estimation for a set of points using their
874 * indices.
875 * \param cloud the input point cloud
876 * \param centroid the output centroid
877 * \ingroup common
878 */
879 template <typename PointT, typename Scalar> inline void
881 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
882
883 template <typename PointT> inline void
885 Eigen::VectorXf &centroid)
886 {
887 return (computeNDCentroid<PointT, float> (cloud, centroid));
888 }
889
890 template <typename PointT> inline void
892 Eigen::VectorXd &centroid)
893 {
894 return (computeNDCentroid<PointT, double> (cloud, centroid));
895 }
896
897 /** \brief General, all purpose nD centroid estimation for a set of points using their
898 * indices.
899 * \param cloud the input point cloud
900 * \param indices the point cloud indices that need to be used
901 * \param centroid the output centroid
902 * \ingroup common
903 */
904 template <typename PointT, typename Scalar> inline void
906 const Indices &indices,
907 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
908
909 template <typename PointT> inline void
911 const Indices &indices,
912 Eigen::VectorXf &centroid)
913 {
914 return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
915 }
916
917 template <typename PointT> inline void
919 const Indices &indices,
920 Eigen::VectorXd &centroid)
921 {
922 return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
923 }
924
925 /** \brief General, all purpose nD centroid estimation for a set of points using their
926 * indices.
927 * \param cloud the input point cloud
928 * \param indices the point cloud indices that need to be used
929 * \param centroid the output centroid
930 * \ingroup common
931 */
932 template <typename PointT, typename Scalar> inline void
934 const pcl::PointIndices &indices,
935 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
936
937 template <typename PointT> inline void
939 const pcl::PointIndices &indices,
940 Eigen::VectorXf &centroid)
941 {
942 return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
943 }
944
945 template <typename PointT> inline void
947 const pcl::PointIndices &indices,
948 Eigen::VectorXd &centroid)
949 {
950 return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
951 }
952
953}
954
955#include <pcl/common/impl/accumulators.hpp>
956
957namespace pcl
958{
959
960 /** A generic class that computes the centroid of points fed to it.
961 *
962 * Here by "centroid" we denote not just the mean of 3D point coordinates,
963 * but also mean of values in the other data fields. The general-purpose
964 * \ref computeNDCentroid() function also implements this sort of
965 * functionality, however it does it in a "dumb" way, i.e. regardless of the
966 * semantics of the data inside a field it simply averages the values. In
967 * certain cases (e.g. for \c x, \c y, \c z, \c intensity fields) this
968 * behavior is reasonable, however in other cases (e.g. \c rgb, \c rgba,
969 * \c label fields) this does not lead to meaningful results.
970 *
971 * This class is capable of computing the centroid in a "smart" way, i.e.
972 * taking into account the meaning of the data inside fields. Currently the
973 * following fields are supported:
974 *
975 * Data | Point fields | Algorithm
976 * --------- | ------------------------------------- | -------------------------------------------------------------------------------------------
977 * XYZ | \c x, \c y, \c z | Average (separate for each field)
978 * Normal | \c normal_x, \c normal_y, \c normal_z | Average (separate for each field), resulting vector is normalized
979 * Curvature | \c curvature | Average
980 * Color | \c rgb or \c rgba | Average (separate for R, G, B, and alpha channels)
981 * Intensity | \c intensity | Average
982 * Label | \c label | Majority vote; if several labels have the same largest support then the smaller label wins
983 *
984 * The template parameter defines the type of points that may be accumulated
985 * with this class. This may be an arbitrary PCL point type, and centroid
986 * computation will happen only for the fields that are present in it and are
987 * supported.
988 *
989 * Current centroid may be retrieved at any time using get(). Note that the
990 * function is templated on point type, so it is possible to fetch the
991 * centroid into a point type that differs from the type of points that are
992 * being accumulated. All the "extra" fields for which the centroid is not
993 * being calculated will be left untouched.
994 *
995 * Example usage:
996 *
997 * \code
998 * // Create and accumulate points
999 * CentroidPoint<pcl::PointXYZ> centroid;
1000 * centroid.add (pcl::PointXYZ (1, 2, 3);
1001 * centroid.add (pcl::PointXYZ (5, 6, 7);
1002 * // Fetch centroid using `get()`
1003 * pcl::PointXYZ c1;
1004 * centroid.get (c1);
1005 * // The expected result is: c1.x == 3, c1.y == 4, c1.z == 5
1006 * // It is also okay to use `get()` with a different point type
1007 * pcl::PointXYZRGB c2;
1008 * centroid.get (c2);
1009 * // The expected result is: c2.x == 3, c2.y == 4, c2.z == 5,
1010 * // and c2.rgb is left untouched
1011 * \endcode
1012 *
1013 * \note Assumes that the points being inserted are valid.
1014 *
1015 * \note This class template can be successfully instantiated for *any*
1016 * PCL point type. Of course, each of the field averages is computed only if
1017 * the point type has the corresponding field.
1018 *
1019 * \ingroup common
1020 * \author Sergey Alexandrov */
1021 template <typename PointT>
1023 {
1024
1025 public:
1026
1027 CentroidPoint () = default;
1028
1029 /** Add a new point to the centroid computation.
1030 *
1031 * In this function only the accumulators and point counter are updated,
1032 * actual centroid computation does not happen until get() is called. */
1033 void
1034 add (const PointT& point);
1035
1036 /** Retrieve the current centroid.
1037 *
1038 * Computation (division of accumulated values by the number of points
1039 * and normalization where applicable) happens here. The result is not
1040 * cached, so any subsequent call to this function will trigger
1041 * re-computation.
1042 *
1043 * If the number of accumulated points is zero, then the point will be
1044 * left untouched. */
1045 template <typename PointOutT> void
1046 get (PointOutT& point) const;
1047
1048 /** Get the total number of points that were added. */
1049 inline std::size_t
1050 getSize () const
1051 {
1052 return (num_points_);
1053 }
1054
1056
1057 private:
1058
1059 std::size_t num_points_ = 0;
1060 typename pcl::detail::Accumulators<PointT>::type accumulators_;
1061
1062 };
1063
1064 /** Compute the centroid of a set of points and return it as a point.
1065 *
1066 * Implementation leverages \ref CentroidPoint class and therefore behaves
1067 * differently from \ref compute3DCentroid() and \ref computeNDCentroid().
1068 * See \ref CentroidPoint documentation for explanation.
1069 *
1070 * \param[in] cloud input point cloud
1071 * \param[out] centroid output centroid
1072 *
1073 * \return number of valid points used to determine the centroid (will be the
1074 * same as the size of the cloud if it is dense)
1075 *
1076 * \note If return value is \c 0, then the centroid is not changed, thus is
1077 * not valid.
1078 *
1079 * \ingroup common */
1080 template <typename PointInT, typename PointOutT> std::size_t
1082 PointOutT& centroid);
1083
1084 /** Compute the centroid of a set of points and return it as a point.
1085 * \param[in] cloud
1086 * \param[in] indices point cloud indices that need to be used
1087 * \param[out] centroid
1088 * This is an overloaded function provided for convenience. See the
1089 * documentation for computeCentroid().
1090 *
1091 * \ingroup common */
1092 template <typename PointInT, typename PointOutT> std::size_t
1094 const Indices& indices,
1095 PointOutT& centroid);
1096
1097}
1098/*@}*/
1099#include <pcl/common/impl/centroid.hpp>
A generic class that computes the centroid of points fed to it.
Definition centroid.h:1023
std::size_t getSize() const
Get the total number of points that were added.
Definition centroid.h:1050
void get(PointOutT &point) const
Retrieve the current centroid.
Definition centroid.hpp:867
void add(const PointT &point)
Add a new point to the centroid computation.
Definition centroid.hpp:858
CentroidPoint()=default
Iterator class for point clouds with or without given indices.
std::size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT &centroid)
Compute the centroid of a set of points and return it as a point.
Definition centroid.hpp:881
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
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
Defines functions, macros and traits for allocating and using memory.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
Defines all the PCL and non-PCL macros used.
Helper functor structure for n-D centroid estimation.
Definition centroid.h:843
NdCentroidFunctor(const PointT &p, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &centroid)
Definition centroid.h:846
typename traits::POD< PointT >::type Pod
Definition centroid.h:844
A point structure representing Euclidean xyz coordinates, and the RGB color.
typename boost::fusion::result_of::as_vector< typename boost::mpl::filter_view< boost::mpl::vector< AccumulatorXYZ, AccumulatorNormal, AccumulatorCurvature, AccumulatorRGBA, AccumulatorIntensity, AccumulatorLabel >, IsAccumulatorCompatible< PointT > > >::type type