Point Cloud Library (PCL)  1.9.0-dev
centroid.h
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 #ifndef PCL_COMMON_CENTROID_H_
40 #define PCL_COMMON_CENTROID_H_
41 
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  typedef typename traits::POD<PointT>::type Pod;
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  typedef typename pcl::traits::datatype<PointT, Key>::type T;
853  const uint8_t* raw_ptr = reinterpret_cast<const 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 (!pcl_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  * - XYZ (\c x, \c y, \c z)
975  *
976  * Separate average for each field.
977  *
978  * - Normal (\c normal_x, \c normal_y, \c normal_z)
979  *
980  * Separate average for each field, and the resulting vector is normalized.
981  *
982  * - Curvature (\c curvature)
983  *
984  * Average.
985  *
986  * - RGB/RGBA (\c rgb or \c rgba)
987  *
988  * Separate average for R, G, B, and alpha channels.
989  *
990  * - Intensity (\c intensity)
991  *
992  * Average.
993  *
994  * - Label (\c label)
995  *
996  * Majority vote. If several labels have the same largest support then the
997  * smaller label wins.
998  *
999  * The template parameter defines the type of points that may be accumulated
1000  * with this class. This may be an arbitrary PCL point type, and centroid
1001  * computation will happen only for the fields that are present in it and are
1002  * supported.
1003  *
1004  * Current centroid may be retrieved at any time using get(). Note that the
1005  * function is templated on point type, so it is possible to fetch the
1006  * centroid into a point type that differs from the type of points that are
1007  * being accumulated. All the "extra" fields for which the centroid is not
1008  * being calculated will be left untouched.
1009  *
1010  * Example usage:
1011  *
1012  * \code
1013  * // Create and accumulate points
1014  * CentroidPoint<pcl::PointXYZ> centroid;
1015  * centroid.add (pcl::PointXYZ (1, 2, 3);
1016  * centroid.add (pcl::PointXYZ (5, 6, 7);
1017  * // Fetch centroid using `get()`
1018  * pcl::PointXYZ c1;
1019  * centroid.get (c1);
1020  * // The expected result is: c1.x == 3, c1.y == 4, c1.z == 5
1021  * // It is also okay to use `get()` with a different point type
1022  * pcl::PointXYZRGB c2;
1023  * centroid.get (c2);
1024  * // The expected result is: c2.x == 3, c2.y == 4, c2.z == 5,
1025  * // and c2.rgb is left untouched
1026  * \endcode
1027  *
1028  * \note Assumes that the points being inserted are valid.
1029  *
1030  * \note This class template can be successfully instantiated for *any*
1031  * PCL point type. Of course, each of the field averages is computed only if
1032  * the point type has the corresponding field.
1033  *
1034  * \ingroup common
1035  * \author Sergey Alexandrov */
1036  template <typename PointT>
1038  {
1039 
1040  public:
1041 
1043  : num_points_ (0)
1044  {
1045  }
1046 
1047  /** Add a new point to the centroid computation.
1048  *
1049  * In this function only the accumulators and point counter are updated,
1050  * actual centroid computation does not happen until get() is called. */
1051  void
1052  add (const PointT& point)
1053  {
1054  // Invoke add point on each accumulator
1055  boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
1056  ++num_points_;
1057  }
1058 
1059  /** Retrieve the current centroid.
1060  *
1061  * Computation (division of accumulated values by the number of points
1062  * and normalization where applicable) happens here. The result is not
1063  * cached, so any subsequent call to this function will trigger
1064  * re-computation.
1065  *
1066  * If the number of accumulated points is zero, then the point will be
1067  * left untouched. */
1068  template <typename PointOutT> void
1069  get (PointOutT& point) const
1070  {
1071  if (num_points_ != 0)
1072  {
1073  // Filter accumulators so that only those that are compatible with
1074  // both PointT and requested point type remain
1075  typename pcl::detail::Accumulators<PointT, PointOutT>::type ca (accumulators_);
1076  // Invoke get point on each accumulator in filtered list
1077  boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
1078  }
1079  }
1080 
1081  /** Get the total number of points that were added. */
1082  size_t
1083  getSize () const
1084  {
1085  return (num_points_);
1086  }
1087 
1088  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1089 
1090  private:
1091 
1092  size_t num_points_;
1093  typename pcl::detail::Accumulators<PointT>::type accumulators_;
1094 
1095  };
1096 
1097  /** Compute the centroid of a set of points and return it as a point.
1098  *
1099  * Implementation leverages \ref CentroidPoint class and therefore behaves
1100  * differently from \ref compute3DCentroid() and \ref computeNDCentroid().
1101  * See \ref CentroidPoint documentation for explanation.
1102  *
1103  * \param[in] cloud input point cloud
1104  * \param[out] centroid output centroid
1105  *
1106  * \return number of valid points used to determine the centroid (will be the
1107  * same as the size of the cloud if it is dense)
1108  *
1109  * \note If return value is \c 0, then the centroid is not changed, thus is
1110  * not valid.
1111  *
1112  * \ingroup common */
1113  template <typename PointInT, typename PointOutT> size_t
1115  PointOutT& centroid);
1116 
1117  /** Compute the centroid of a set of points and return it as a point.
1118  * \param[in] cloud
1119  * \param[in] indices point cloud indices that need to be used
1120  * \param[out] centroid
1121  * This is an overloaded function provided for convenience. See the
1122  * documentation for computeCentroid().
1123  *
1124  * \ingroup common */
1125  template <typename PointInT, typename PointOutT> size_t
1127  const std::vector<int>& indices,
1128  PointOutT& centroid);
1129 
1130 }
1131 /*@}*/
1132 #include <pcl/common/impl/centroid.hpp>
1133 
1134 #endif //#ifndef PCL_COMMON_CENTROID_H_
Iterator class for point clouds with or without given indices.
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:863
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:489
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
boost::fusion::result_of::as_vector< typename boost::mpl::filter_view< boost::mpl::vector< AccumulatorXYZ, AccumulatorNormal, AccumulatorCurvature, AccumulatorRGBA, AccumulatorIntensity, AccumulatorLabel >, boost::mpl::and_< IsCompatible< boost::mpl::_1, Point1T >, IsCompatible< boost::mpl::_1, Point2T > > > >::type type
Helper functor structure for n-D centroid estimation.
Definition: centroid.h:841
size_t getSize() const
Get the total number of points that were added.
Definition: centroid.h:1083
traits::POD< PointT >::type Pod
Definition: centroid.h:843
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.h:1052
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:631
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:809
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:251
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:1037