Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/common/include/pcl/common/centroid.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  */
00038 
00039 #ifndef PCL_COMMON_CENTROID_H_
00040 #define PCL_COMMON_CENTROID_H_
00041 
00042 #include <pcl/point_cloud.h>
00043 #include <pcl/point_traits.h>
00044 #include <pcl/PointIndices.h>
00045 #include <pcl/cloud_iterator.h>
00046 
00047 /**
00048   * \file pcl/common/centroid.h
00049   * Define methods for centroid estimation and covariance matrix calculus
00050   * \ingroup common
00051   */
00052 
00053 /*@{*/
00054 namespace pcl
00055 {
00056   /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
00057     * \param[in] cloud_iterator an iterator over the input point cloud
00058     * \param[out] centroid the output centroid
00059     * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
00060     * \note if return value is 0, the centroid is not changed, thus not valid.
00061     * \ingroup common
00062     */
00063   template <typename PointT, typename Scalar> inline unsigned int
00064   compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
00065                      Eigen::Matrix<Scalar, 4, 1> &centroid);
00066 
00067   template <typename PointT> inline unsigned int
00068   compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
00069                      Eigen::Vector4f &centroid)
00070   {
00071     return (compute3DCentroid <PointT, float> (cloud_iterator, centroid));
00072   }
00073 
00074   template <typename PointT> inline unsigned int
00075   compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
00076                      Eigen::Vector4d &centroid)
00077   {
00078     return (compute3DCentroid <PointT, double> (cloud_iterator, centroid));
00079   }
00080 
00081   /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
00082     * \param[in] cloud the input point cloud
00083     * \param[out] centroid the output centroid
00084     * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
00085     * \note if return value is 0, the centroid is not changed, thus not valid.
00086     * \ingroup common
00087     */
00088   template <typename PointT, typename Scalar> inline unsigned int
00089   compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 
00090                      Eigen::Matrix<Scalar, 4, 1> &centroid);
00091 
00092   template <typename PointT> inline unsigned int
00093   compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 
00094                      Eigen::Vector4f &centroid)
00095   {
00096     return (compute3DCentroid <PointT, float> (cloud, centroid));
00097   }
00098 
00099   template <typename PointT> inline unsigned int
00100   compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 
00101                      Eigen::Vector4d &centroid)
00102   {
00103     return (compute3DCentroid <PointT, double> (cloud, centroid));
00104   }
00105 
00106   /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and
00107     * return it as a 3D vector.
00108     * \param[in] cloud the input point cloud
00109     * \param[in] indices the point cloud indices that need to be used
00110     * \param[out] centroid the output centroid
00111     * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
00112     * \note if return value is 0, the centroid is not changed, thus not valid.
00113     * \ingroup common
00114     */
00115   template <typename PointT, typename Scalar> inline unsigned int
00116   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00117                      const std::vector<int> &indices, 
00118                      Eigen::Matrix<Scalar, 4, 1> &centroid);
00119 
00120   template <typename PointT> inline unsigned int
00121   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00122                      const std::vector<int> &indices, 
00123                      Eigen::Vector4f &centroid)
00124   {
00125     return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
00126   }
00127 
00128   template <typename PointT> inline unsigned int
00129   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00130                      const std::vector<int> &indices, 
00131                      Eigen::Vector4d &centroid)
00132   {
00133     return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
00134   }
00135 
00136   /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and
00137     * return it as a 3D vector.
00138     * \param[in] cloud the input point cloud
00139     * \param[in] indices the point cloud indices that need to be used
00140     * \param[out] centroid the output centroid
00141     * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud.
00142     * \note if return value is 0, the centroid is not changed, thus not valid.
00143     * \ingroup common
00144     */
00145   template <typename PointT, typename Scalar> inline unsigned int
00146   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00147                      const pcl::PointIndices &indices, 
00148                      Eigen::Matrix<Scalar, 4, 1> &centroid);
00149 
00150   template <typename PointT> inline unsigned int
00151   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00152                      const pcl::PointIndices &indices, 
00153                      Eigen::Vector4f &centroid)
00154   {
00155     return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
00156   }
00157 
00158   template <typename PointT> inline unsigned int
00159   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00160                      const pcl::PointIndices &indices, 
00161                      Eigen::Vector4d &centroid)
00162   {
00163     return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
00164   }
00165 
00166   /** \brief Compute the 3x3 covariance matrix of a given set of points.
00167     * The result is returned as a Eigen::Matrix3f.
00168     * Note: the covariance matrix is not normalized with the number of
00169     * points. For a normalized covariance, please use
00170     * computeNormalizedCovarianceMatrix.
00171     * \param[in] cloud the input point cloud
00172     * \param[in] centroid the centroid of the set of points in the cloud
00173     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00174     * \return number of valid point used to determine the covariance matrix.
00175     * In case of dense point clouds, this is the same as the size of input cloud.
00176     * \note if return value is 0, the covariance matrix is not changed, thus not valid.
00177     * \ingroup common
00178     */
00179   template <typename PointT, typename Scalar> inline unsigned int
00180   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00181                            const Eigen::Matrix<Scalar, 4, 1> &centroid,
00182                            Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00183 
00184   template <typename PointT> inline unsigned int
00185   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00186                            const Eigen::Vector4f &centroid,
00187                            Eigen::Matrix3f &covariance_matrix)
00188   {
00189     return (computeCovarianceMatrix<PointT, float> (cloud, centroid, covariance_matrix));
00190   }
00191 
00192   template <typename PointT> inline unsigned int
00193   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00194                            const Eigen::Vector4d &centroid,
00195                            Eigen::Matrix3d &covariance_matrix)
00196   {
00197     return (computeCovarianceMatrix<PointT, double> (cloud, centroid, covariance_matrix));
00198   }
00199 
00200   /** \brief Compute normalized the 3x3 covariance matrix of a given set of points.
00201     * The result is returned as a Eigen::Matrix3f.
00202     * Normalized means that every entry has been divided by the number of points in the point cloud.
00203     * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix
00204     * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
00205     * the covariance matrix and is returned by the computeCovarianceMatrix function.
00206     * \param[in] cloud the input point cloud
00207     * \param[in] centroid the centroid of the set of points in the cloud
00208     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00209     * \return number of valid point used to determine the covariance matrix.
00210     * In case of dense point clouds, this is the same as the size of input cloud.
00211     * \ingroup common
00212     */
00213   template <typename PointT, typename Scalar> inline unsigned int
00214   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00215                                      const Eigen::Matrix<Scalar, 4, 1> &centroid,
00216                                      Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00217 
00218   template <typename PointT> inline unsigned int
00219   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00220                                      const Eigen::Vector4f &centroid,
00221                                      Eigen::Matrix3f &covariance_matrix)
00222   {
00223     return (computeCovarianceMatrixNormalized<PointT, float> (cloud, centroid, covariance_matrix));
00224   }
00225 
00226   template <typename PointT> inline unsigned int
00227   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00228                                      const Eigen::Vector4d &centroid,
00229                                      Eigen::Matrix3d &covariance_matrix)
00230   {
00231     return (computeCovarianceMatrixNormalized<PointT, double> (cloud, centroid, covariance_matrix));
00232   }
00233 
00234   /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
00235     * The result is returned as a Eigen::Matrix3f.
00236     * Note: the covariance matrix is not normalized with the number of
00237     * points. For a normalized covariance, please use
00238     * computeNormalizedCovarianceMatrix.
00239     * \param[in] cloud the input point cloud
00240     * \param[in] indices the point cloud indices that need to be used
00241     * \param[in] centroid the centroid of the set of points in the cloud
00242     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00243     * \return number of valid point used to determine the covariance matrix.
00244     * In case of dense point clouds, this is the same as the size of input cloud.
00245     * \ingroup common
00246     */
00247   template <typename PointT, typename Scalar> inline unsigned int
00248   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00249                            const std::vector<int> &indices,
00250                            const Eigen::Matrix<Scalar, 4, 1> &centroid,
00251                            Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00252 
00253   template <typename PointT> inline unsigned int
00254   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00255                            const std::vector<int> &indices,
00256                            const Eigen::Vector4f &centroid,
00257                            Eigen::Matrix3f &covariance_matrix)
00258   {
00259     return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
00260   }
00261 
00262   template <typename PointT> inline unsigned int
00263   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00264                            const std::vector<int> &indices,
00265                            const Eigen::Vector4d &centroid,
00266                            Eigen::Matrix3d &covariance_matrix)
00267   {
00268     return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
00269   }
00270 
00271   /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices.
00272     * The result is returned as a Eigen::Matrix3f.
00273     * Note: the covariance matrix is not normalized with the number of
00274     * points. For a normalized covariance, please use
00275     * computeNormalizedCovarianceMatrix.
00276     * \param[in] cloud the input point cloud
00277     * \param[in] indices the point cloud indices that need to be used
00278     * \param[in] centroid the centroid of the set of points in the cloud
00279     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00280     * \return number of valid point used to determine the covariance matrix.
00281     * In case of dense point clouds, this is the same as the size of input cloud.
00282     * \ingroup common
00283     */
00284   template <typename PointT, typename Scalar> inline unsigned int
00285   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00286                            const pcl::PointIndices &indices,
00287                            const Eigen::Matrix<Scalar, 4, 1> &centroid,
00288                            Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00289 
00290   template <typename PointT> inline unsigned int
00291   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00292                            const pcl::PointIndices &indices,
00293                            const Eigen::Vector4f &centroid,
00294                            Eigen::Matrix3f &covariance_matrix)
00295   {
00296     return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
00297   }
00298 
00299   template <typename PointT> inline unsigned int
00300   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00301                            const pcl::PointIndices &indices,
00302                            const Eigen::Vector4d &centroid,
00303                            Eigen::Matrix3d &covariance_matrix)
00304   {
00305     return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
00306   }
00307 
00308   /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using
00309     * their indices.
00310     * The result is returned as a Eigen::Matrix3f.
00311     * Normalized means that every entry has been divided by the number of entries in indices.
00312     * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix
00313     * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
00314     * the covariance matrix and is returned by the computeCovarianceMatrix function.
00315     * \param[in] cloud the input point cloud
00316     * \param[in] indices the point cloud indices that need to be used
00317     * \param[in] centroid the centroid of the set of points in the cloud
00318     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00319     * \return number of valid point used to determine the covariance matrix.
00320     * In case of dense point clouds, this is the same as the size of input cloud.
00321     * \ingroup common
00322     */
00323   template <typename PointT, typename Scalar> inline unsigned int
00324   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00325                                      const std::vector<int> &indices,
00326                                      const Eigen::Matrix<Scalar, 4, 1> &centroid,
00327                                      Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00328 
00329   template <typename PointT> inline unsigned int
00330   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00331                                      const std::vector<int> &indices,
00332                                      const Eigen::Vector4f &centroid,
00333                                      Eigen::Matrix3f &covariance_matrix)
00334   {
00335     return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
00336   }
00337 
00338   template <typename PointT> inline unsigned int
00339   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00340                                      const std::vector<int> &indices,
00341                                      const Eigen::Vector4d &centroid,
00342                                      Eigen::Matrix3d &covariance_matrix)
00343   {
00344     return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
00345   }
00346 
00347   /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using
00348     * their indices. The result is returned as a Eigen::Matrix3f.
00349     * Normalized means that every entry has been divided by the number of entries in indices.
00350     * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix
00351     * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate
00352     * the covariance matrix and is returned by the computeCovarianceMatrix function.
00353     * \param[in] cloud the input point cloud
00354     * \param[in] indices the point cloud indices that need to be used
00355     * \param[in] centroid the centroid of the set of points in the cloud
00356     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00357     * \return number of valid point used to determine the covariance matrix.
00358     * In case of dense point clouds, this is the same as the size of input cloud.
00359     * \ingroup common
00360     */
00361   template <typename PointT, typename Scalar> inline unsigned int
00362   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00363                                      const pcl::PointIndices &indices,
00364                                      const Eigen::Matrix<Scalar, 4, 1> &centroid,
00365                                      Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00366 
00367   template <typename PointT> inline unsigned int
00368   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00369                                      const pcl::PointIndices &indices,
00370                                      const Eigen::Vector4f &centroid,
00371                                      Eigen::Matrix3f &covariance_matrix)
00372   {
00373     return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
00374   }
00375 
00376   template <typename PointT> inline unsigned int
00377   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00378                                      const pcl::PointIndices &indices,
00379                                      const Eigen::Vector4d &centroid,
00380                                      Eigen::Matrix3d &covariance_matrix)
00381   {
00382     return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
00383   }
00384 
00385   /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
00386     * Normalized means that every entry has been divided by the number of entries in indices.
00387     * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
00388     * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
00389     * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
00390     * \param[in] cloud the input point cloud
00391     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00392     * \param[out] centroid the centroid of the set of points in the cloud
00393     * \return number of valid point used to determine the covariance matrix.
00394     * In case of dense point clouds, this is the same as the size of input cloud.
00395     * \ingroup common
00396     */
00397   template <typename PointT, typename Scalar> inline unsigned int
00398   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00399                                   Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
00400                                   Eigen::Matrix<Scalar, 4, 1> &centroid);
00401 
00402   template <typename PointT> inline unsigned int
00403   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00404                                   Eigen::Matrix3f &covariance_matrix,
00405                                   Eigen::Vector4f &centroid)
00406   {
00407     return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, covariance_matrix, centroid));
00408   }
00409 
00410   template <typename PointT> inline unsigned int
00411   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00412                                   Eigen::Matrix3d &covariance_matrix,
00413                                   Eigen::Vector4d &centroid)
00414   {
00415     return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, covariance_matrix, centroid));
00416   }
00417 
00418   /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
00419     * Normalized means that every entry has been divided by the number of entries in indices.
00420     * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
00421     * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
00422     * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
00423     * \param[in] cloud the input point cloud
00424     * \param[in] indices subset of points given by their indices
00425     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00426     * \param[out] centroid the centroid of the set of points in the cloud
00427     * \return number of valid point used to determine the covariance matrix.
00428     * In case of dense point clouds, this is the same as the size of input cloud.
00429     * \ingroup common
00430     */
00431   template <typename PointT, typename Scalar> inline unsigned int
00432   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00433                                   const std::vector<int> &indices,
00434                                   Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
00435                                   Eigen::Matrix<Scalar, 4, 1> &centroid);
00436 
00437   template <typename PointT> inline unsigned int
00438   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00439                                   const std::vector<int> &indices,
00440                                   Eigen::Matrix3f &covariance_matrix,
00441                                   Eigen::Vector4f &centroid)
00442   {
00443     return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
00444   }
00445 
00446   template <typename PointT> inline unsigned int
00447   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00448                                   const std::vector<int> &indices,
00449                                   Eigen::Matrix3d &covariance_matrix,
00450                                   Eigen::Vector4d &centroid)
00451   {
00452     return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
00453   }
00454 
00455   /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop.
00456     * Normalized means that every entry has been divided by the number of entries in indices.
00457     * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
00458     * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
00459     * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
00460     * \param[in] cloud the input point cloud
00461     * \param[in] indices subset of points given by their indices
00462     * \param[out] centroid the centroid of the set of points in the cloud
00463     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00464     * \return number of valid point used to determine the covariance matrix.
00465     * In case of dense point clouds, this is the same as the size of input cloud.
00466     * \ingroup common
00467     */
00468   template <typename PointT, typename Scalar> inline unsigned int
00469   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00470                                   const pcl::PointIndices &indices,
00471                                   Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
00472                                   Eigen::Matrix<Scalar, 4, 1> &centroid);
00473 
00474   template <typename PointT> inline unsigned int
00475   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00476                                   const pcl::PointIndices &indices,
00477                                   Eigen::Matrix3f &covariance_matrix,
00478                                   Eigen::Vector4f &centroid)
00479   {
00480     return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
00481   }
00482 
00483   template <typename PointT> inline unsigned int
00484   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00485                                   const pcl::PointIndices &indices,
00486                                   Eigen::Matrix3d &covariance_matrix,
00487                                   Eigen::Vector4d &centroid)
00488   {
00489     return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
00490   }
00491 
00492   /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
00493     * Normalized means that every entry has been divided by the number of entries in indices.
00494     * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
00495     * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
00496     * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
00497     * \param[in] cloud the input point cloud
00498     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00499     * \return number of valid point used to determine the covariance matrix.
00500     * In case of dense point clouds, this is the same as the size of input cloud.
00501     * \ingroup common
00502     */
00503   template <typename PointT, typename Scalar> inline unsigned int
00504   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00505                            Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00506 
00507   template <typename PointT> inline unsigned int
00508   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00509                            Eigen::Matrix3f &covariance_matrix)
00510   {
00511     return (computeCovarianceMatrix<PointT, float> (cloud, covariance_matrix));
00512   }
00513 
00514   template <typename PointT> inline unsigned int
00515   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00516                            Eigen::Matrix3d &covariance_matrix)
00517   {
00518     return (computeCovarianceMatrix<PointT, double> (cloud, covariance_matrix));
00519   }
00520 
00521   /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
00522     * Normalized means that every entry has been divided by the number of entries in indices.
00523     * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
00524     * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
00525     * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
00526     * \param[in] cloud the input point cloud
00527     * \param[in] indices subset of points given by their indices
00528     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00529     * \return number of valid point used to determine the covariance matrix.
00530     * In case of dense point clouds, this is the same as the size of input cloud.
00531     * \ingroup common
00532     */
00533   template <typename PointT, typename Scalar> inline unsigned int
00534   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00535                            const std::vector<int> &indices,
00536                            Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00537 
00538   template <typename PointT> inline unsigned int
00539   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00540                            const std::vector<int> &indices,
00541                            Eigen::Matrix3f &covariance_matrix)
00542   {
00543     return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
00544   }
00545 
00546   template <typename PointT> inline unsigned int
00547   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00548                            const std::vector<int> &indices,
00549                            Eigen::Matrix3d &covariance_matrix)
00550   {
00551     return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
00552   }
00553 
00554   /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud.
00555     * Normalized means that every entry has been divided by the number of entries in indices.
00556     * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix
00557     * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function.
00558     * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency.
00559     * \param[in] cloud the input point cloud
00560     * \param[in] indices subset of points given by their indices
00561     * \param[out] covariance_matrix the resultant 3x3 covariance matrix
00562     * \return number of valid point used to determine the covariance matrix.
00563     * In case of dense point clouds, this is the same as the size of input cloud.
00564     * \ingroup common
00565     */
00566   template <typename PointT, typename Scalar> inline unsigned int
00567   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00568                            const pcl::PointIndices &indices,
00569                            Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
00570 
00571   template <typename PointT> inline unsigned int
00572   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00573                            const pcl::PointIndices &indices,
00574                            Eigen::Matrix3f &covariance_matrix)
00575   {
00576     return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
00577   }
00578 
00579   template <typename PointT> inline unsigned int
00580   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00581                            const pcl::PointIndices &indices,
00582                            Eigen::Matrix3d &covariance_matrix)
00583   {
00584     return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
00585   }
00586 
00587   /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
00588     * \param[in] cloud_iterator an iterator over the input point cloud
00589     * \param[in] centroid the centroid of the point cloud
00590     * \param[out] cloud_out the resultant output point cloud
00591     * \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.
00592     * \ingroup common
00593     */
00594   template <typename PointT, typename Scalar> void
00595   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00596                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00597                     pcl::PointCloud<PointT> &cloud_out,
00598                     int npts = 0);
00599 
00600   template <typename PointT> void
00601   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00602                     const Eigen::Vector4f &centroid,
00603                     pcl::PointCloud<PointT> &cloud_out,
00604                     int npts = 0)
00605   {
00606     return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
00607   }
00608 
00609   template <typename PointT> void
00610   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00611                     const Eigen::Vector4d &centroid,
00612                     pcl::PointCloud<PointT> &cloud_out,
00613                     int npts = 0)
00614   {
00615     return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
00616   }
00617 
00618   /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
00619     * \param[in] cloud_in the input point cloud
00620     * \param[in] centroid the centroid of the point cloud
00621     * \param[out] cloud_out the resultant output point cloud
00622     * \ingroup common
00623     */
00624   template <typename PointT, typename Scalar> void
00625   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00626                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00627                     pcl::PointCloud<PointT> &cloud_out);
00628 
00629   template <typename PointT> void
00630   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00631                     const Eigen::Vector4f &centroid,
00632                     pcl::PointCloud<PointT> &cloud_out)
00633   {
00634     return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out));
00635   }
00636 
00637   template <typename PointT> void
00638   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00639                     const Eigen::Vector4d &centroid,
00640                     pcl::PointCloud<PointT> &cloud_out)
00641   {
00642     return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out));
00643   }
00644 
00645   /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
00646     * \param[in] cloud_in the input point cloud
00647     * \param[in] indices the set of point indices to use from the input point cloud
00648     * \param[out] centroid the centroid of the point cloud
00649     * \param cloud_out the resultant output point cloud
00650     * \ingroup common
00651     */
00652   template <typename PointT, typename Scalar> void
00653   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00654                     const std::vector<int> &indices,
00655                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00656                     pcl::PointCloud<PointT> &cloud_out);
00657 
00658   template <typename PointT> void
00659   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00660                     const std::vector<int> &indices,
00661                     const Eigen::Vector4f &centroid,
00662                     pcl::PointCloud<PointT> &cloud_out)
00663   {
00664     return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
00665   }
00666 
00667   template <typename PointT> void
00668   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00669                     const std::vector<int> &indices,
00670                     const Eigen::Vector4d &centroid,
00671                     pcl::PointCloud<PointT> &cloud_out)
00672   {
00673     return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
00674   }
00675 
00676   /** \brief Subtract a centroid from a point cloud and return the de-meaned representation
00677     * \param[in] cloud_in the input point cloud
00678     * \param[in] indices the set of point indices to use from the input point cloud
00679     * \param[out] centroid the centroid of the point cloud
00680     * \param cloud_out the resultant output point cloud
00681     * \ingroup common
00682     */
00683   template <typename PointT, typename Scalar> void
00684   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00685                     const pcl::PointIndices& indices,
00686                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00687                     pcl::PointCloud<PointT> &cloud_out);
00688 
00689   template <typename PointT> void
00690   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00691                     const pcl::PointIndices& indices,
00692                     const Eigen::Vector4f &centroid,
00693                     pcl::PointCloud<PointT> &cloud_out)
00694   {
00695     return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
00696   }
00697 
00698   template <typename PointT> void
00699   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00700                     const pcl::PointIndices& indices,
00701                     const Eigen::Vector4d &centroid,
00702                     pcl::PointCloud<PointT> &cloud_out)
00703   {
00704     return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
00705   }
00706 
00707   /** \brief Subtract a centroid from a point cloud and return the de-meaned
00708     * representation as an Eigen matrix
00709     * \param[in] cloud_iterator an iterator over the input point cloud
00710     * \param[in] centroid the centroid of the point cloud
00711     * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
00712     * an Eigen matrix (4 rows, N pts columns)
00713     * \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.
00714     * \ingroup common
00715     */
00716   template <typename PointT, typename Scalar> void
00717   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00718                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00719                     Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
00720                     int npts = 0);
00721 
00722   template <typename PointT> void
00723   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00724                     const Eigen::Vector4f &centroid,
00725                     Eigen::MatrixXf &cloud_out,
00726                     int npts = 0)
00727   {
00728     return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
00729   }
00730 
00731   template <typename PointT> void
00732   demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
00733                     const Eigen::Vector4d &centroid,
00734                     Eigen::MatrixXd &cloud_out,
00735                     int npts = 0)
00736   {
00737     return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
00738   }
00739 
00740   /** \brief Subtract a centroid from a point cloud and return the de-meaned
00741     * representation as an Eigen matrix
00742     * \param[in] cloud_in the input point cloud
00743     * \param[in] centroid the centroid of the point cloud
00744     * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
00745     * an Eigen matrix (4 rows, N pts columns)
00746     * \ingroup common
00747     */
00748   template <typename PointT, typename Scalar> void
00749   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00750                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00751                     Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
00752 
00753   template <typename PointT> void
00754   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00755                     const Eigen::Vector4f &centroid,
00756                     Eigen::MatrixXf &cloud_out)
00757   {
00758     return (demeanPointCloud<PointT, float> (cloud_in, centroid, cloud_out));
00759   }
00760 
00761   template <typename PointT> void
00762   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00763                     const Eigen::Vector4d &centroid,
00764                     Eigen::MatrixXd &cloud_out)
00765   {
00766     return (demeanPointCloud<PointT, double> (cloud_in, centroid, cloud_out));
00767   }
00768 
00769   /** \brief Subtract a centroid from a point cloud and return the de-meaned
00770     * representation as an Eigen matrix
00771     * \param[in] cloud_in the input point cloud
00772     * \param[in] indices the set of point indices to use from the input point cloud
00773     * \param[in] centroid the centroid of the point cloud
00774     * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
00775     * an Eigen matrix (4 rows, N pts columns)
00776     * \ingroup common
00777     */
00778   template <typename PointT, typename Scalar> void
00779   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00780                     const std::vector<int> &indices,
00781                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00782                     Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
00783 
00784   template <typename PointT> void
00785   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00786                     const std::vector<int> &indices,
00787                     const Eigen::Vector4f &centroid,
00788                     Eigen::MatrixXf &cloud_out)
00789   {
00790     return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
00791   }
00792 
00793   template <typename PointT> void
00794   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00795                     const std::vector<int> &indices,
00796                     const Eigen::Vector4d &centroid,
00797                     Eigen::MatrixXd &cloud_out)
00798   {
00799     return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
00800   }
00801 
00802   /** \brief Subtract a centroid from a point cloud and return the de-meaned
00803     * representation as an Eigen matrix
00804     * \param[in] cloud_in the input point cloud
00805     * \param[in] indices the set of point indices to use from the input point cloud
00806     * \param[in] centroid the centroid of the point cloud
00807     * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as
00808     * an Eigen matrix (4 rows, N pts columns)
00809     * \ingroup common
00810     */
00811   template <typename PointT, typename Scalar> void
00812   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00813                     const pcl::PointIndices& indices,
00814                     const Eigen::Matrix<Scalar, 4, 1> &centroid,
00815                     Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
00816 
00817   template <typename PointT> void
00818   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00819                     const pcl::PointIndices& indices,
00820                     const Eigen::Vector4f &centroid,
00821                     Eigen::MatrixXf &cloud_out)
00822   {
00823     return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
00824   }
00825 
00826   template <typename PointT> void
00827   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00828                     const pcl::PointIndices& indices,
00829                     const Eigen::Vector4d &centroid,
00830                     Eigen::MatrixXd &cloud_out)
00831   {
00832     return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
00833   }
00834 
00835   /** \brief Helper functor structure for n-D centroid estimation. */
00836   template<typename PointT, typename Scalar>
00837   struct NdCentroidFunctor
00838   {
00839     typedef typename traits::POD<PointT>::type Pod;
00840 
00841     NdCentroidFunctor (const PointT &p, Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
00842       : f_idx_ (0),
00843         centroid_ (centroid),
00844         p_ (reinterpret_cast<const Pod&>(p)) { }
00845 
00846     template<typename Key> inline void operator() ()
00847     {
00848       typedef typename pcl::traits::datatype<PointT, Key>::type T;
00849       const uint8_t* raw_ptr = reinterpret_cast<const uint8_t*>(&p_) + pcl::traits::offset<PointT, Key>::value;
00850       const T* data_ptr = reinterpret_cast<const T*>(raw_ptr);
00851 
00852       // Check if the value is invalid
00853       if (!pcl_isfinite (*data_ptr))
00854       {
00855         f_idx_++;
00856         return;
00857       }
00858 
00859       centroid_[f_idx_++] += *data_ptr;
00860     }
00861 
00862     private:
00863       int f_idx_;
00864       Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid_;
00865       const Pod &p_;
00866   };
00867 
00868   /** \brief General, all purpose nD centroid estimation for a set of points using their
00869     * indices.
00870     * \param cloud the input point cloud
00871     * \param centroid the output centroid
00872     * \ingroup common
00873     */
00874   template <typename PointT, typename Scalar> inline void
00875   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00876                      Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
00877 
00878   template <typename PointT> inline void
00879   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00880                      Eigen::VectorXf &centroid)
00881   {
00882     return (computeNDCentroid<PointT, float> (cloud, centroid));
00883   }
00884 
00885   template <typename PointT> inline void
00886   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00887                      Eigen::VectorXd &centroid)
00888   {
00889     return (computeNDCentroid<PointT, double> (cloud, centroid));
00890   }
00891 
00892   /** \brief General, all purpose nD centroid estimation for a set of points using their
00893     * indices.
00894     * \param cloud the input point cloud
00895     * \param indices the point cloud indices that need to be used
00896     * \param centroid the output centroid
00897     * \ingroup common
00898     */
00899   template <typename PointT, typename Scalar> inline void
00900   computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00901                      const std::vector<int> &indices, 
00902                      Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
00903 
00904   template <typename PointT> inline void
00905   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00906                      const std::vector<int> &indices, 
00907                      Eigen::VectorXf &centroid)
00908   {
00909     return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
00910   }
00911 
00912   template <typename PointT> inline void
00913   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00914                      const std::vector<int> &indices, 
00915                      Eigen::VectorXd &centroid)
00916   {
00917     return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
00918   }
00919 
00920   /** \brief General, all purpose nD centroid estimation for a set of points using their
00921     * indices.
00922     * \param cloud the input point cloud
00923     * \param indices the point cloud indices that need to be used
00924     * \param centroid the output centroid
00925     * \ingroup common
00926     */
00927   template <typename PointT, typename Scalar> inline void
00928   computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00929                      const pcl::PointIndices &indices, 
00930                      Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid);
00931 
00932   template <typename PointT> inline void
00933   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00934                      const pcl::PointIndices &indices, 
00935                      Eigen::VectorXf &centroid)
00936   {
00937     return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
00938   }
00939 
00940   template <typename PointT> inline void
00941   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00942                      const pcl::PointIndices &indices, 
00943                      Eigen::VectorXd &centroid)
00944   {
00945     return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
00946   }
00947 
00948 }
00949 /*@}*/
00950 #include <pcl/common/impl/centroid.hpp>
00951 
00952 #endif  //#ifndef PCL_COMMON_CENTROID_H_