pcl: centroid.h Source File

Go to the documentation of this file.
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 
00054 namespace pcl
00055 {
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 }
00950 #include <pcl/common/impl/centroid.hpp>
00951 
00952 #endif  //#ifndef PCL_COMMON_CENTROID_H_