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