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