Point Cloud Library (PCL)  1.7.1
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 point 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  * \ingroup common
62  */
63  template <typename PointT, typename Scalar> inline unsigned int
64  compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
65  Eigen::Matrix<Scalar, 4, 1> &centroid);
66 
67  template <typename PointT> inline unsigned int
69  Eigen::Vector4f &centroid)
70  {
71  return (compute3DCentroid <PointT, float> (cloud_iterator, centroid));
72  }
73 
74  template <typename PointT> inline unsigned int
76  Eigen::Vector4d &centroid)
77  {
78  return (compute3DCentroid <PointT, double> (cloud_iterator, centroid));
79  }
80 
81  /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
82  * \param[in] cloud the input point cloud
83  * \param[out] centroid the output centroid
84  * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
85  * \note if return value is 0, the centroid is not changed, thus not valid.
86  * \ingroup common
87  */
88  template <typename PointT, typename Scalar> inline unsigned int
90  Eigen::Matrix<Scalar, 4, 1> &centroid);
91 
92  template <typename PointT> inline unsigned int
94  Eigen::Vector4f &centroid)
95  {
96  return (compute3DCentroid <PointT, float> (cloud, centroid));
97  }
98 
99  template <typename PointT> inline unsigned int
101  Eigen::Vector4d &centroid)
102  {
103  return (compute3DCentroid <PointT, double> (cloud, centroid));
104  }
105 
106  /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and
107  * return it as a 3D vector.
108  * \param[in] cloud the input point cloud
109  * \param[in] indices the point cloud indices that need to be used
110  * \param[out] centroid the output centroid
111  * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
112  * \note if return value is 0, the centroid is not changed, thus not valid.
113  * \ingroup common
114  */
115  template <typename PointT, typename Scalar> inline unsigned int
117  const std::vector<int> &indices,
118  Eigen::Matrix<Scalar, 4, 1> &centroid);
119 
120  template <typename PointT> inline unsigned int
122  const std::vector<int> &indices,
123  Eigen::Vector4f &centroid)
124  {
125  return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
126  }
127 
128  template <typename PointT> inline unsigned int
130  const std::vector<int> &indices,
131  Eigen::Vector4d &centroid)
132  {
133  return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
134  }
135 
136  /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and
137  * return it as a 3D vector.
138  * \param[in] cloud the input point cloud
139  * \param[in] indices the point cloud indices that need to be used
140  * \param[out] centroid the output centroid
141  * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
142  * \note if return value is 0, the centroid is not changed, thus not valid.
143  * \ingroup common
144  */
145  template <typename PointT, typename Scalar> inline unsigned int
147  const pcl::PointIndices &indices,
148  Eigen::Matrix<Scalar, 4, 1> &centroid);
149 
150  template <typename PointT> inline unsigned int
152  const pcl::PointIndices &indices,
153  Eigen::Vector4f &centroid)
154  {
155  return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
156  }
157 
158  template <typename PointT> inline unsigned int
160  const pcl::PointIndices &indices,
161  Eigen::Vector4d &centroid)
162  {
163  return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
164  }
165 
166  /** \brief Compute the 3x3 covariance matrix of a given set of points.
167  * The result is returned as a Eigen::Matrix3f.
168  * Note: the covariance matrix is not normalized with the number of
169  * points. For a normalized covariance, please use
170  * computeNormalizedCovarianceMatrix.
171  * \param[in] cloud the input point cloud
172  * \param[in] centroid the centroid of the set of points in the cloud
173  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
174  * \return number of valid point used to determine the covariance matrix.
175  * In case of dense point clouds, this is the same as the size of input cloud.
176  * \note if return value is 0, the covariance matrix is not changed, thus not valid.
177  * \ingroup common
178  */
179  template <typename PointT, typename Scalar> inline unsigned int
181  const Eigen::Matrix<Scalar, 4, 1> &centroid,
182  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
183 
184  template <typename PointT> inline unsigned int
186  const Eigen::Vector4f &centroid,
187  Eigen::Matrix3f &covariance_matrix)
188  {
189  return (computeCovarianceMatrix<PointT, float> (cloud, centroid, covariance_matrix));
190  }
191 
192  template <typename PointT> inline unsigned int
194  const Eigen::Vector4d &centroid,
195  Eigen::Matrix3d &covariance_matrix)
196  {
197  return (computeCovarianceMatrix<PointT, double> (cloud, centroid, covariance_matrix));
198  }
199 
200  /** \brief Compute normalized the 3x3 covariance matrix of a given set of points.
201  * The result is returned as a Eigen::Matrix3f.
202  * Normalized means that every entry has been divided by the number of points in the point cloud.
203  * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix
204  * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
205  * the covariance matrix and is returned by the computeCovarianceMatrix function.
206  * \param[in] cloud the input point cloud
207  * \param[in] centroid the centroid of the set of points in the cloud
208  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
209  * \return number of valid point used to determine the covariance matrix.
210  * In case of dense point clouds, this is the same as the size of input cloud.
211  * \ingroup common
212  */
213  template <typename PointT, typename Scalar> inline unsigned int
215  const Eigen::Matrix<Scalar, 4, 1> &centroid,
216  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
217 
218  template <typename PointT> inline unsigned int
220  const Eigen::Vector4f &centroid,
221  Eigen::Matrix3f &covariance_matrix)
222  {
223  return (computeCovarianceMatrixNormalized<PointT, float> (cloud, centroid, covariance_matrix));
224  }
225 
226  template <typename PointT> inline unsigned int
228  const Eigen::Vector4d &centroid,
229  Eigen::Matrix3d &covariance_matrix)
230  {
231  return (computeCovarianceMatrixNormalized<PointT, double> (cloud, centroid, covariance_matrix));
232  }
233 
234  /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
235  * The result is returned as a Eigen::Matrix3f.
236  * Note: the covariance matrix is not normalized with the number of
237  * points. For a normalized covariance, please use
238  * computeNormalizedCovarianceMatrix.
239  * \param[in] cloud the input point cloud
240  * \param[in] indices the point cloud indices that need to be used
241  * \param[in] centroid the centroid of the set of points in the cloud
242  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
243  * \return number of valid point used to determine the covariance matrix.
244  * In case of dense point clouds, this is the same as the size of input cloud.
245  * \ingroup common
246  */
247  template <typename PointT, typename Scalar> inline unsigned int
249  const std::vector<int> &indices,
250  const Eigen::Matrix<Scalar, 4, 1> &centroid,
251  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
252 
253  template <typename PointT> inline unsigned int
255  const std::vector<int> &indices,
256  const Eigen::Vector4f &centroid,
257  Eigen::Matrix3f &covariance_matrix)
258  {
259  return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
260  }
261 
262  template <typename PointT> inline unsigned int
264  const std::vector<int> &indices,
265  const Eigen::Vector4d &centroid,
266  Eigen::Matrix3d &covariance_matrix)
267  {
268  return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
269  }
270 
271  /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
272  * The result is returned as a Eigen::Matrix3f.
273  * Note: the covariance matrix is not normalized with the number of
274  * points. For a normalized covariance, please use
275  * computeNormalizedCovarianceMatrix.
276  * \param[in] cloud the input point cloud
277  * \param[in] indices the point cloud indices that need to be used
278  * \param[in] centroid the centroid of the set of points in the cloud
279  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
280  * \return number of valid point used to determine the covariance matrix.
281  * In case of dense point clouds, this is the same as the size of input cloud.
282  * \ingroup common
283  */
284  template <typename PointT, typename Scalar> inline unsigned int
286  const pcl::PointIndices &indices,
287  const Eigen::Matrix<Scalar, 4, 1> &centroid,
288  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
289 
290  template <typename PointT> inline unsigned int
292  const pcl::PointIndices &indices,
293  const Eigen::Vector4f &centroid,
294  Eigen::Matrix3f &covariance_matrix)
295  {
296  return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
297  }
298 
299  template <typename PointT> inline unsigned int
301  const pcl::PointIndices &indices,
302  const Eigen::Vector4d &centroid,
303  Eigen::Matrix3d &covariance_matrix)
304  {
305  return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
306  }
307 
308  /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using
309  * their indices.
310  * The result is returned as a Eigen::Matrix3f.
311  * Normalized means that every entry has been divided by the number of entries in indices.
312  * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix
313  * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
314  * the covariance matrix and is returned by the computeCovarianceMatrix function.
315  * \param[in] cloud the input point cloud
316  * \param[in] indices the point cloud indices that need to be used
317  * \param[in] centroid the centroid of the set of points in the cloud
318  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
319  * \return number of valid point used to determine the covariance matrix.
320  * In case of dense point clouds, this is the same as the size of input cloud.
321  * \ingroup common
322  */
323  template <typename PointT, typename Scalar> inline unsigned int
325  const std::vector<int> &indices,
326  const Eigen::Matrix<Scalar, 4, 1> &centroid,
327  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
328 
329  template <typename PointT> inline unsigned int
331  const std::vector<int> &indices,
332  const Eigen::Vector4f &centroid,
333  Eigen::Matrix3f &covariance_matrix)
334  {
335  return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
336  }
337 
338  template <typename PointT> inline unsigned int
340  const std::vector<int> &indices,
341  const Eigen::Vector4d &centroid,
342  Eigen::Matrix3d &covariance_matrix)
343  {
344  return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
345  }
346 
347  /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using
348  * their indices. The result is returned as a Eigen::Matrix3f.
349  * Normalized means that every entry has been divided by the number of entries in indices.
350  * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix
351  * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
352  * the covariance matrix and is returned by the computeCovarianceMatrix function.
353  * \param[in] cloud the input point cloud
354  * \param[in] indices the point cloud indices that need to be used
355  * \param[in] centroid the centroid of the set of points in the cloud
356  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
357  * \return number of valid point used to determine the covariance matrix.
358  * In case of dense point clouds, this is the same as the size of input cloud.
359  * \ingroup common
360  */
361  template <typename PointT, typename Scalar> inline unsigned int
363  const pcl::PointIndices &indices,
364  const Eigen::Matrix<Scalar, 4, 1> &centroid,
365  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
366 
367  template <typename PointT> inline unsigned int
369  const pcl::PointIndices &indices,
370  const Eigen::Vector4f &centroid,
371  Eigen::Matrix3f &covariance_matrix)
372  {
373  return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
374  }
375 
376  template <typename PointT> inline unsigned int
378  const pcl::PointIndices &indices,
379  const Eigen::Vector4d &centroid,
380  Eigen::Matrix3d &covariance_matrix)
381  {
382  return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
383  }
384 
385  /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
386  * Normalized means that every entry has been divided by the number of entries in indices.
387  * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
388  * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
389  * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
390  * \param[in] cloud the input point cloud
391  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
392  * \param[out] centroid the centroid of the set of points in the cloud
393  * \return number of valid point used to determine the covariance matrix.
394  * In case of dense point clouds, this is the same as the size of input cloud.
395  * \ingroup common
396  */
397  template <typename PointT, typename Scalar> inline unsigned int
399  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
400  Eigen::Matrix<Scalar, 4, 1> &centroid);
401 
402  template <typename PointT> inline unsigned int
404  Eigen::Matrix3f &covariance_matrix,
405  Eigen::Vector4f &centroid)
406  {
407  return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, covariance_matrix, centroid));
408  }
409 
410  template <typename PointT> inline unsigned int
412  Eigen::Matrix3d &covariance_matrix,
413  Eigen::Vector4d &centroid)
414  {
415  return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, covariance_matrix, centroid));
416  }
417 
418  /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
419  * Normalized means that every entry has been divided by the number of entries in indices.
420  * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
421  * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
422  * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
423  * \param[in] cloud the input point cloud
424  * \param[in] indices subset of points given by their indices
425  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
426  * \param[out] centroid the centroid of the set of points in the cloud
427  * \return number of valid point used to determine the covariance matrix.
428  * In case of dense point clouds, this is the same as the size of input cloud.
429  * \ingroup common
430  */
431  template <typename PointT, typename Scalar> inline unsigned int
433  const std::vector<int> &indices,
434  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
435  Eigen::Matrix<Scalar, 4, 1> &centroid);
436 
437  template <typename PointT> inline unsigned int
439  const std::vector<int> &indices,
440  Eigen::Matrix3f &covariance_matrix,
441  Eigen::Vector4f &centroid)
442  {
443  return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
444  }
445 
446  template <typename PointT> inline unsigned int
448  const std::vector<int> &indices,
449  Eigen::Matrix3d &covariance_matrix,
450  Eigen::Vector4d &centroid)
451  {
452  return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
453  }
454 
455  /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
456  * Normalized means that every entry has been divided by the number of entries in indices.
457  * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
458  * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
459  * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
460  * \param[in] cloud the input point cloud
461  * \param[in] indices subset of points given by their indices
462  * \param[out] centroid the centroid of the set of points in the cloud
463  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
464  * \return number of valid point used to determine the covariance matrix.
465  * In case of dense point clouds, this is the same as the size of input cloud.
466  * \ingroup common
467  */
468  template <typename PointT, typename Scalar> inline unsigned int
470  const pcl::PointIndices &indices,
471  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
472  Eigen::Matrix<Scalar, 4, 1> &centroid);
473 
474  template <typename PointT> inline unsigned int
476  const pcl::PointIndices &indices,
477  Eigen::Matrix3f &covariance_matrix,
478  Eigen::Vector4f &centroid)
479  {
480  return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
481  }
482 
483  template <typename PointT> inline unsigned int
485  const pcl::PointIndices &indices,
486  Eigen::Matrix3d &covariance_matrix,
487  Eigen::Vector4d &centroid)
488  {
489  return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
490  }
491 
492  /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
493  * Normalized means that every entry has been divided by the number of entries in indices.
494  * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
495  * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
496  * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
497  * \param[in] cloud the input point cloud
498  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
499  * \return number of valid point used to determine the covariance matrix.
500  * In case of dense point clouds, this is the same as the size of input cloud.
501  * \ingroup common
502  */
503  template <typename PointT, typename Scalar> inline unsigned int
505  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
506 
507  template <typename PointT> inline unsigned int
509  Eigen::Matrix3f &covariance_matrix)
510  {
511  return (computeCovarianceMatrix<PointT, float> (cloud, covariance_matrix));
512  }
513 
514  template <typename PointT> inline unsigned int
516  Eigen::Matrix3d &covariance_matrix)
517  {
518  return (computeCovarianceMatrix<PointT, double> (cloud, covariance_matrix));
519  }
520 
521  /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
522  * Normalized means that every entry has been divided by the number of entries in indices.
523  * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
524  * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
525  * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
526  * \param[in] cloud the input point cloud
527  * \param[in] indices subset of points given by their indices
528  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
529  * \return number of valid point used to determine the covariance matrix.
530  * In case of dense point clouds, this is the same as the size of input cloud.
531  * \ingroup common
532  */
533  template <typename PointT, typename Scalar> inline unsigned int
535  const std::vector<int> &indices,
536  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
537 
538  template <typename PointT> inline unsigned int
540  const std::vector<int> &indices,
541  Eigen::Matrix3f &covariance_matrix)
542  {
543  return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
544  }
545 
546  template <typename PointT> inline unsigned int
548  const std::vector<int> &indices,
549  Eigen::Matrix3d &covariance_matrix)
550  {
551  return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
552  }
553 
554  /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
555  * Normalized means that every entry has been divided by the number of entries in indices.
556  * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
557  * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
558  * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
559  * \param[in] cloud the input point cloud
560  * \param[in] indices subset of points given by their indices
561  * \param[out] covariance_matrix the resultant 3x3 covariance matrix
562  * \return number of valid point used to determine the covariance matrix.
563  * In case of dense point clouds, this is the same as the size of input cloud.
564  * \ingroup common
565  */
566  template <typename PointT, typename Scalar> inline unsigned int
568  const pcl::PointIndices &indices,
569  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
570 
571  template <typename PointT> inline unsigned int
573  const pcl::PointIndices &indices,
574  Eigen::Matrix3f &covariance_matrix)
575  {
576  return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
577  }
578 
579  template <typename PointT> inline unsigned int
581  const pcl::PointIndices &indices,
582  Eigen::Matrix3d &covariance_matrix)
583  {
584  return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
585  }
586 
587  /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
588  * \param[in] cloud_iterator an iterator over the input point cloud
589  * \param[in] centroid the centroid of the point cloud
590  * \param[out] cloud_out the resultant output point cloud
591  * \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.
592  * \ingroup common
593  */
594  template <typename PointT, typename Scalar> void
595  demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
596  const Eigen::Matrix<Scalar, 4, 1> &centroid,
597  pcl::PointCloud<PointT> &cloud_out,
598  int npts = 0);
599 
600  template <typename PointT> void
602  const Eigen::Vector4f &centroid,
603  pcl::PointCloud<PointT> &cloud_out,
604  int npts = 0)
605  {
606  return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
607  }
608 
609  template <typename PointT> void
611  const Eigen::Vector4d &centroid,
612  pcl::PointCloud<PointT> &cloud_out,
613  int npts = 0)
614  {
615  return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
616  }
617 
618  /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
619  * \param[in] cloud_in the input point cloud
620  * \param[in] centroid the centroid of the point cloud
621  * \param[out] cloud_out the resultant output point cloud
622  * \ingroup common
623  */
624  template <typename PointT, typename Scalar> void
625  demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
626  const Eigen::Matrix<Scalar, 4, 1> &centroid,
627  pcl::PointCloud<PointT> &cloud_out);
628 
629  template <typename PointT> void
631  const Eigen::Vector4f &centroid,
632  pcl::PointCloud<PointT> &cloud_out)
633  {
634  return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out));
635  }
636 
637  template <typename PointT> void
639  const Eigen::Vector4d &centroid,
640  pcl::PointCloud<PointT> &cloud_out)
641  {
642  return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out));
643  }
644 
645  /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
646  * \param[in] cloud_in the input point cloud
647  * \param[in] indices the set of point indices to use from the input point cloud
648  * \param[out] centroid the centroid of the point cloud
649  * \param cloud_out the resultant output point cloud
650  * \ingroup common
651  */
652  template <typename PointT, typename Scalar> void
653  demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
654  const std::vector<int> &indices,
655  const Eigen::Matrix<Scalar, 4, 1> &centroid,
656  pcl::PointCloud<PointT> &cloud_out);
657 
658  template <typename PointT> void
660  const std::vector<int> &indices,
661  const Eigen::Vector4f &centroid,
662  pcl::PointCloud<PointT> &cloud_out)
663  {
664  return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
665  }
666 
667  template <typename PointT> void
669  const std::vector<int> &indices,
670  const Eigen::Vector4d &centroid,
671  pcl::PointCloud<PointT> &cloud_out)
672  {
673  return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
674  }
675 
676  /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
677  * \param[in] cloud_in the input point cloud
678  * \param[in] indices the set of point indices to use from the input point cloud
679  * \param[out] centroid the centroid of the point cloud
680  * \param cloud_out the resultant output point cloud
681  * \ingroup common
682  */
683  template <typename PointT, typename Scalar> void
684  demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
685  const pcl::PointIndices& indices,
686  const Eigen::Matrix<Scalar, 4, 1> &centroid,
687  pcl::PointCloud<PointT> &cloud_out);
688 
689  template <typename PointT> void
691  const pcl::PointIndices& indices,
692  const Eigen::Vector4f &centroid,
693  pcl::PointCloud<PointT> &cloud_out)
694  {
695  return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
696  }
697 
698  template <typename PointT> void
700  const pcl::PointIndices& indices,
701  const Eigen::Vector4d &centroid,
702  pcl::PointCloud<PointT> &cloud_out)
703  {
704  return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
705  }
706 
707  /** \brief Subtract a centroid from a point cloud and return the de-meaned
708  * representation as an Eigen matrix
709  * \param[in] cloud_iterator an iterator over the input point cloud
710  * \param[in] centroid the centroid of the point cloud
711  * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
712  * an Eigen matrix (4 rows, N pts columns)
713  * \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.
714  * \ingroup common
715  */
716  template <typename PointT, typename Scalar> void
717  demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
718  const Eigen::Matrix<Scalar, 4, 1> &centroid,
719  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
720  int npts = 0);
721 
722  template <typename PointT> void
724  const Eigen::Vector4f &centroid,
725  Eigen::MatrixXf &cloud_out,
726  int npts = 0)
727  {
728  return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
729  }
730 
731  template <typename PointT> void
733  const Eigen::Vector4d &centroid,
734  Eigen::MatrixXd &cloud_out,
735  int npts = 0)
736  {
737  return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
738  }
739 
740  /** \brief Subtract a centroid from a point cloud and return the de-meaned
741  * representation as an Eigen matrix
742  * \param[in] cloud_in the input point cloud
743  * \param[in] centroid the centroid of the point cloud
744  * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
745  * an Eigen matrix (4 rows, N pts columns)
746  * \ingroup common
747  */
748  template <typename PointT, typename Scalar> void
749  demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
750  const Eigen::Matrix<Scalar, 4, 1> &centroid,
751  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
752 
753  template <typename PointT> void
755  const Eigen::Vector4f &centroid,
756  Eigen::MatrixXf &cloud_out)
757  {
758  return (demeanPointCloud<PointT, float> (cloud_in, centroid, cloud_out));
759  }
760 
761  template <typename PointT> void
763  const Eigen::Vector4d &centroid,
764  Eigen::MatrixXd &cloud_out)
765  {
766  return (demeanPointCloud<PointT, double> (cloud_in, centroid, cloud_out));
767  }
768 
769  /** \brief Subtract a centroid from a point cloud and return the de-meaned
770  * representation as an Eigen matrix
771  * \param[in] cloud_in the input point cloud
772  * \param[in] indices the set of point indices to use from the input point cloud
773  * \param[in] centroid the centroid of the point cloud
774  * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
775  * an Eigen matrix (4 rows, N pts columns)
776  * \ingroup common
777  */
778  template <typename PointT, typename Scalar> void
779  demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
780  const std::vector<int> &indices,
781  const Eigen::Matrix<Scalar, 4, 1> &centroid,
782  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
783 
784  template <typename PointT> void
786  const std::vector<int> &indices,
787  const Eigen::Vector4f &centroid,
788  Eigen::MatrixXf &cloud_out)
789  {
790  return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
791  }
792 
793  template <typename PointT> void
795  const std::vector<int> &indices,
796  const Eigen::Vector4d &centroid,
797  Eigen::MatrixXd &cloud_out)
798  {
799  return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
800  }
801 
802  /** \brief Subtract a centroid from a point cloud and return the de-meaned
803  * representation as an Eigen matrix
804  * \param[in] cloud_in the input point cloud
805  * \param[in] indices the set of point indices to use from the input point cloud
806  * \param[in] centroid the centroid of the point cloud
807  * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
808  * an Eigen matrix (4 rows, N pts columns)
809  * \ingroup common
810  */
811  template <typename PointT, typename Scalar> void
812  demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
813  const pcl::PointIndices& indices,
814  const Eigen::Matrix<Scalar, 4, 1> &centroid,
815  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
816 
817  template <typename PointT> void
819  const pcl::PointIndices& indices,
820  const Eigen::Vector4f &centroid,
821  Eigen::MatrixXf &cloud_out)
822  {
823  return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
824  }
825 
826  template <typename PointT> void
828  const pcl::PointIndices& indices,
829  const Eigen::Vector4d &centroid,
830  Eigen::MatrixXd &cloud_out)
831  {
832  return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
833  }
834 
835  /** \brief Helper functor structure for n-D centroid estimation. */
836  template<typename PointT, typename Scalar>
838  {
839  typedef typename traits::POD<PointT>::type Pod;
840 
841  NdCentroidFunctor (const PointT &p, Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
842  : f_idx_ (0),
843  centroid_ (centroid),
844  p_ (reinterpret_cast<const Pod&>(p)) { }
845 
846  template<typename Key> inline void operator() ()
847  {
848  typedef typename pcl::traits::datatype<PointT, Key>::type T;
849  const uint8_t* raw_ptr = reinterpret_cast<const uint8_t*>(&p_) + pcl::traits::offset<PointT, Key>::value;
850  const T* data_ptr = reinterpret_cast<const T*>(raw_ptr);
851 
852  // Check if the value is invalid
853  if (!pcl_isfinite (*data_ptr))
854  {
855  f_idx_++;
856  return;
857  }
858 
859  centroid_[f_idx_++] += *data_ptr;
860  }
861 
862  private:
863  int f_idx_;
864  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid_;
865  const Pod &p_;
866  };
867 
868  /** \brief General, all purpose nD centroid estimation for a set of points using their
869  * indices.
870  * \param cloud the input point cloud
871  * \param centroid the output centroid
872  * \ingroup common
873  */
874  template <typename PointT, typename Scalar> inline void
876  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
877 
878  template <typename PointT> inline void
880  Eigen::VectorXf &centroid)
881  {
882  return (computeNDCentroid<PointT, float> (cloud, centroid));
883  }
884 
885  template <typename PointT> inline void
887  Eigen::VectorXd &centroid)
888  {
889  return (computeNDCentroid<PointT, double> (cloud, centroid));
890  }
891 
892  /** \brief General, all purpose nD centroid estimation for a set of points using their
893  * indices.
894  * \param cloud the input point cloud
895  * \param indices the point cloud indices that need to be used
896  * \param centroid the output centroid
897  * \ingroup common
898  */
899  template <typename PointT, typename Scalar> inline void
901  const std::vector<int> &indices,
902  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
903 
904  template <typename PointT> inline void
906  const std::vector<int> &indices,
907  Eigen::VectorXf &centroid)
908  {
909  return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
910  }
911 
912  template <typename PointT> inline void
914  const std::vector<int> &indices,
915  Eigen::VectorXd &centroid)
916  {
917  return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
918  }
919 
920  /** \brief General, all purpose nD centroid estimation for a set of points using their
921  * indices.
922  * \param cloud the input point cloud
923  * \param indices the point cloud indices that need to be used
924  * \param centroid the output centroid
925  * \ingroup common
926  */
927  template <typename PointT, typename Scalar> inline void
929  const pcl::PointIndices &indices,
930  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
931 
932  template <typename PointT> inline void
934  const pcl::PointIndices &indices,
935  Eigen::VectorXf &centroid)
936  {
937  return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
938  }
939 
940  template <typename PointT> inline void
942  const pcl::PointIndices &indices,
943  Eigen::VectorXd &centroid)
944  {
945  return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
946  }
947 
948 }
949 /*@}*/
950 #include <pcl/common/impl/centroid.hpp>
951 
952 #endif //#ifndef PCL_COMMON_CENTROID_H_