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