centroid.hpp

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009-present, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: centroid.hpp 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #ifndef PCL_COMMON_IMPL_CENTROID_H_
00039 #define PCL_COMMON_IMPL_CENTROID_H_
00040 
00041 #include "pcl/ros/conversions.h"
00042 #include <boost/mpl/size.hpp>
00043 
00045 template <typename PointT> inline void
00046 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &centroid)
00047 {
00048   // Initialize to 0
00049   centroid.setZero ();
00050   if (cloud.points.empty ()) 
00051     return;
00052   // For each point in the cloud
00053   int cp = 0;
00054 
00055   // If the data is dense, we don't need to check for NaN
00056   if (cloud.is_dense)
00057   {
00058     for (size_t i = 0; i < cloud.points.size (); ++i)
00059       centroid += cloud.points[i].getVector4fMap ();
00060     centroid[3] = 0;
00061     centroid /= cloud.points.size ();
00062   }
00063   // NaN or Inf values could exist => check for them
00064   else
00065   {
00066     for (size_t i = 0; i < cloud.points.size (); ++i)
00067     {
00068       // Check if the point is invalid
00069       if (!pcl_isfinite (cloud.points[i].x) || 
00070           !pcl_isfinite (cloud.points[i].y) || 
00071           !pcl_isfinite (cloud.points[i].z))
00072         continue;
00073 
00074       centroid += cloud.points[i].getVector4fMap ();
00075       cp++;
00076     }
00077     centroid[3] = 0;
00078     centroid /= cp;
00079   }
00080 }
00081 
00083 template <typename PointT> inline void
00084 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00085                         Eigen::Vector4f &centroid)
00086 {
00087   // Initialize to 0
00088   centroid.setZero ();
00089   if (indices.empty ()) 
00090     return;
00091   // For each point in the cloud
00092   int cp = 0;
00093 
00094   // If the data is dense, we don't need to check for NaN
00095   if (cloud.is_dense)
00096   {
00097     for (size_t i = 0; i < indices.size (); ++i)
00098       centroid += cloud.points[indices[i]].getVector4fMap ();
00099     centroid[3] = 0;
00100     centroid /= indices.size ();
00101   }
00102   // NaN or Inf values could exist => check for them
00103   else
00104   {
00105     for (size_t i = 0; i < indices.size (); ++i)
00106     {
00107       // Check if the point is invalid
00108       if (!pcl_isfinite (cloud.points[indices[i]].x) || 
00109           !pcl_isfinite (cloud.points[indices[i]].y) || 
00110           !pcl_isfinite (cloud.points[indices[i]].z))
00111         continue;
00112 
00113       centroid += cloud.points[indices[i]].getVector4fMap ();
00114       cp++;
00115     }
00116     centroid[3] = 0;
00117     centroid /= cp;
00118   }
00119 }
00120 
00122 template <typename PointT> inline void
00123 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 
00124                         const pcl::PointIndices &indices, Eigen::Vector4f &centroid)
00125 {
00126   return (pcl::compute3DCentroid<PointT> (cloud, indices.indices, centroid));
00127 }
00128 
00130 template <typename PointT> inline void
00131 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00132                               const Eigen::Vector4f &centroid, 
00133                               Eigen::Matrix3f &covariance_matrix)
00134 {
00135   // Initialize to 0
00136   covariance_matrix.setZero ();
00137 
00138   if (cloud.points.empty ())
00139     return;
00140   // If the data is dense, we don't need to check for NaN
00141   if (cloud.is_dense)
00142   {
00143     // For each point in the cloud
00144     for (size_t i = 0; i < cloud.points.size (); ++i)
00145     {
00146       Eigen::Vector4f pt = cloud.points[i].getVector4fMap () - centroid;
00147 
00148       covariance_matrix (1, 1) += pt.y () * pt.y ();
00149       covariance_matrix (1, 2) += pt.y () * pt.z ();
00150 
00151       covariance_matrix (2, 2) += pt.z () * pt.z ();
00152 
00153       pt *= pt.x ();
00154       covariance_matrix (0, 0) += pt.x ();
00155       covariance_matrix (0, 1) += pt.y ();
00156       covariance_matrix (0, 2) += pt.z ();
00157     }
00158   }
00159   // NaN or Inf values could exist => check for them
00160   else
00161   {
00162     // For each point in the cloud
00163     for (size_t i = 0; i < cloud.points.size (); ++i)
00164     {
00165       // Check if the point is invalid
00166       if (!pcl_isfinite (cloud.points[i].x) || 
00167           !pcl_isfinite (cloud.points[i].y) || 
00168           !pcl_isfinite (cloud.points[i].z))
00169         continue;
00170 
00171       Eigen::Vector4f pt = cloud.points[i].getVector4fMap () - centroid;
00172 
00173       covariance_matrix (1, 1) += pt.y () * pt.y ();
00174       covariance_matrix (1, 2) += pt.y () * pt.z ();
00175 
00176       covariance_matrix (2, 2) += pt.z () * pt.z ();
00177 
00178       pt *= pt.x ();
00179       covariance_matrix (0, 0) += pt.x ();
00180       covariance_matrix (0, 1) += pt.y ();
00181       covariance_matrix (0, 2) += pt.z ();
00182     }
00183   }
00184   covariance_matrix (1, 0) = covariance_matrix (0, 1);
00185   covariance_matrix (2, 0) = covariance_matrix (0, 2);
00186   covariance_matrix (2, 1) = covariance_matrix (1, 2);
00187 }
00188 
00190 template <typename PointT> inline void
00191 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00192                                         const Eigen::Vector4f &centroid, 
00193                                         Eigen::Matrix3f &covariance_matrix)
00194 {
00195   pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
00196   covariance_matrix /= cloud.points.size ();
00197 }
00198 
00200 template <typename PointT> inline void
00201 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 
00202                               const std::vector<int> &indices,
00203                               const Eigen::Vector4f &centroid, 
00204                               Eigen::Matrix3f &covariance_matrix)
00205 {
00206   // Initialize to 0
00207   covariance_matrix.setZero ();
00208 
00209   if (indices.empty ())
00210     return;
00211   // If the data is dense, we don't need to check for NaN
00212   if (cloud.is_dense)
00213   {
00214     // For each point in the cloud
00215     for (size_t i = 0; i < indices.size (); ++i)
00216     {
00217       Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
00218 
00219       covariance_matrix (1, 1) += pt.y () * pt.y ();
00220       covariance_matrix (1, 2) += pt.y () * pt.z ();
00221 
00222       covariance_matrix (2, 2) += pt.z () * pt.z ();
00223 
00224       pt *= pt.x ();
00225       covariance_matrix (0, 0) += pt.x ();
00226       covariance_matrix (0, 1) += pt.y ();
00227       covariance_matrix (0, 2) += pt.z ();
00228     }
00229   }
00230   // NaN or Inf values could exist => check for them
00231   else
00232   {
00233     // For each point in the cloud
00234     for (size_t i = 0; i < indices.size (); ++i)
00235     {
00236       // Check if the point is invalid
00237       if (!pcl_isfinite (cloud.points[indices[i]].x) || 
00238           !pcl_isfinite (cloud.points[indices[i]].y) || 
00239           !pcl_isfinite (cloud.points[indices[i]].z))
00240         continue;
00241 
00242       Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
00243 
00244       covariance_matrix (1, 1) += pt.y () * pt.y ();
00245       covariance_matrix (1, 2) += pt.y () * pt.z ();
00246 
00247       covariance_matrix (2, 2) += pt.z () * pt.z ();
00248 
00249       pt *= pt.x ();
00250       covariance_matrix (0, 0) += pt.x ();
00251       covariance_matrix (0, 1) += pt.y ();
00252       covariance_matrix (0, 2) += pt.z ();
00253     }
00254   }
00255   covariance_matrix (1, 0) = covariance_matrix (0, 1);
00256   covariance_matrix (2, 0) = covariance_matrix (0, 2);
00257   covariance_matrix (2, 1) = covariance_matrix (1, 2);
00258 }
00259 
00261 template <typename PointT> inline void
00262 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 
00263                               const pcl::PointIndices &indices,
00264                               const Eigen::Vector4f &centroid, 
00265                               Eigen::Matrix3f &covariance_matrix)
00266 {
00267   return (pcl::computeCovarianceMatrix<PointT> (cloud, indices.indices, centroid));
00268 }
00269 
00271 template <typename PointT> inline void
00272 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 
00273                                         const std::vector<int> &indices,
00274                                         const Eigen::Vector4f &centroid, 
00275                                         Eigen::Matrix3f &covariance_matrix)
00276 {
00277   pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
00278   covariance_matrix /= indices.size ();
00279 }
00280 
00282 template <typename PointT> inline void
00283 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 
00284                                         const pcl::PointIndices &indices,
00285                                         const Eigen::Vector4f &centroid, 
00286                                         Eigen::Matrix3f &covariance_matrix)
00287 {
00288   return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
00289 }
00290 
00292 template <typename PointT> void
00293 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00294                        const Eigen::Vector4f &centroid,
00295                        pcl::PointCloud<PointT> &cloud_out)
00296 {
00297   cloud_out = cloud_in;
00298 
00299   // Subtract the centroid from cloud_in
00300   for (size_t i = 0; i < cloud_in.points.size (); ++i)
00301     cloud_out.points[i].getVector4fMap () -= centroid;
00302 }
00303 
00305 template <typename PointT> void
00306 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00307                        const std::vector<int> &indices,
00308                        const Eigen::Vector4f &centroid, 
00309                        pcl::PointCloud<PointT> &cloud_out)
00310 {
00311   cloud_out.header = cloud_in.header;
00312   cloud_out.is_dense = cloud_in.is_dense;
00313   if (indices.size () == cloud_in.points.size ())
00314   {
00315     cloud_out.width    = cloud_in.width;
00316     cloud_out.height   = cloud_in.height;
00317   }
00318   else
00319   {
00320     cloud_out.width    = indices.size ();
00321     cloud_out.height   = 1;
00322   }
00323   cloud_out.points.resize (indices.size ());
00324 
00325   // Subtract the centroid from cloud_in
00326   for (size_t i = 0; i < indices.size (); ++i)
00327     cloud_out.points[i].getVector4fMap () = cloud_in.points[indices[i]].getVector4fMap () - 
00328                                             centroid;
00329 }
00330 
00332 template <typename PointT> void
00333 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00334                        const Eigen::Vector4f &centroid,
00335                        Eigen::MatrixXf &cloud_out)
00336 {
00337   size_t npts = cloud_in.points.size ();
00338 
00339   cloud_out = Eigen::MatrixXf::Zero (4, npts);        // keep the data aligned
00340 
00341   for (size_t i = 0; i < npts; ++i)
00342     // One column at a time
00343     cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
00344   
00345   // Make sure we zero the 4th dimension out (1 row, N columns)
00346   cloud_out.block (3, 0, 1, npts).setZero ();
00347 }
00348 
00350 template <typename PointT> void
00351 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00352                        const std::vector<int> &indices,
00353                        const Eigen::Vector4f &centroid, 
00354                        Eigen::MatrixXf &cloud_out)
00355 {
00356   size_t npts = indices.size ();
00357 
00358   cloud_out = Eigen::MatrixXf::Zero (4, npts);        // keep the data aligned
00359 
00360   for (size_t i = 0; i < npts; ++i)
00361     // One column at a time
00362     cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid;
00363 
00364   // Make sure we zero the 4th dimension out (1 row, N columns)
00365   cloud_out.block (3, 0, 1, npts).setZero ();
00366 }
00367 
00369 template <typename PointT> inline void
00370 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf &centroid)
00371 {
00372   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00373 
00374   // Get the size of the fields
00375   centroid.setZero (boost::mpl::size<FieldList>::value);
00376 
00377   if (cloud.points.empty ())
00378     return;
00379   // Iterate over each point
00380   int size = cloud.points.size ();
00381   for (int i = 0; i < size; ++i)
00382   {
00383     // Iterate over each dimension
00384     pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[i], centroid));
00385   }
00386   centroid /= size;
00387 }
00388 
00390 template <typename PointT> inline void
00391 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00392                         Eigen::VectorXf &centroid)
00393 {
00394   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00395 
00396   // Get the size of the fields
00397   centroid.setZero (boost::mpl::size<FieldList>::value);
00398 
00399   if (indices.empty ()) 
00400     return;
00401   // Iterate over each point
00402   int nr_points = indices.size ();
00403   for (int i = 0; i < nr_points; ++i)
00404   {
00405     // Iterate over each dimension
00406     pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[indices[i]], centroid));
00407   }
00408   centroid /= nr_points;
00409 }
00410 
00412 template <typename PointT> inline void
00413 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00414                         const pcl::PointIndices &indices, Eigen::VectorXf &centroid)
00415 {
00416   return (pcl::computeNDCentroid<PointT> (cloud, indices.indices, centroid));
00417 }
00418 
00419 #endif  //#ifndef PCL_COMMON_IMPL_CENTROID_H_
00420