Point Cloud Library (PCL)  1.3.1
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 3317 2011-12-02 11:17:47Z mdixon $
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 /= (float) 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.0f;
00117     centroid /= (float) 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   pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix);
00289   covariance_matrix /= indices.indices.size ();
00290 }
00291 
00293 template <typename PointT> void
00294 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00295                        const Eigen::Vector4f &centroid,
00296                        pcl::PointCloud<PointT> &cloud_out)
00297 {
00298   cloud_out = cloud_in;
00299 
00300   // Subtract the centroid from cloud_in
00301   for (size_t i = 0; i < cloud_in.points.size (); ++i)
00302     cloud_out.points[i].getVector4fMap () -= centroid;
00303 }
00304 
00306 template <typename PointT> void
00307 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00308                        const std::vector<int> &indices,
00309                        const Eigen::Vector4f &centroid, 
00310                        pcl::PointCloud<PointT> &cloud_out)
00311 {
00312   cloud_out.header = cloud_in.header;
00313   cloud_out.is_dense = cloud_in.is_dense;
00314   if (indices.size () == cloud_in.points.size ())
00315   {
00316     cloud_out.width    = cloud_in.width;
00317     cloud_out.height   = cloud_in.height;
00318   }
00319   else
00320   {
00321     cloud_out.width    = indices.size ();
00322     cloud_out.height   = 1;
00323   }
00324   cloud_out.points.resize (indices.size ());
00325 
00326   // Subtract the centroid from cloud_in
00327   for (size_t i = 0; i < indices.size (); ++i)
00328     cloud_out.points[i].getVector4fMap () = cloud_in.points[indices[i]].getVector4fMap () - 
00329                                             centroid;
00330 }
00331 
00333 template <typename PointT> void
00334 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00335                        const Eigen::Vector4f &centroid,
00336                        Eigen::MatrixXf &cloud_out)
00337 {
00338   size_t npts = cloud_in.points.size ();
00339 
00340   cloud_out = Eigen::MatrixXf::Zero (4, npts);        // keep the data aligned
00341 
00342   for (size_t i = 0; i < npts; ++i)
00343     // One column at a time
00344     cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
00345   
00346   // Make sure we zero the 4th dimension out (1 row, N columns)
00347   cloud_out.block (3, 0, 1, npts).setZero ();
00348 }
00349 
00351 template <typename PointT> void
00352 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00353                        const std::vector<int> &indices,
00354                        const Eigen::Vector4f &centroid, 
00355                        Eigen::MatrixXf &cloud_out)
00356 {
00357   size_t npts = indices.size ();
00358 
00359   cloud_out = Eigen::MatrixXf::Zero (4, npts);        // keep the data aligned
00360 
00361   for (size_t i = 0; i < npts; ++i)
00362     // One column at a time
00363     cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid;
00364 
00365   // Make sure we zero the 4th dimension out (1 row, N columns)
00366   cloud_out.block (3, 0, 1, npts).setZero ();
00367 }
00368 
00370 template <typename PointT> void
00371 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
00372                        const pcl::PointIndices &indices,
00373                        const Eigen::Vector4f &centroid, 
00374                        Eigen::MatrixXf &cloud_out)
00375 {
00376   return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
00377 }
00378 
00380 template <typename PointT> inline void
00381 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf &centroid)
00382 {
00383   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00384 
00385   // Get the size of the fields
00386   centroid.setZero (boost::mpl::size<FieldList>::value);
00387 
00388   if (cloud.points.empty ())
00389     return;
00390   // Iterate over each point
00391   int size = cloud.points.size ();
00392   for (int i = 0; i < size; ++i)
00393   {
00394     // Iterate over each dimension
00395     pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[i], centroid));
00396   }
00397   centroid /= size;
00398 }
00399 
00401 template <typename PointT> inline void
00402 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00403                         Eigen::VectorXf &centroid)
00404 {
00405   typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00406 
00407   // Get the size of the fields
00408   centroid.setZero (boost::mpl::size<FieldList>::value);
00409 
00410   if (indices.empty ()) 
00411     return;
00412   // Iterate over each point
00413   int nr_points = indices.size ();
00414   for (int i = 0; i < nr_points; ++i)
00415   {
00416     // Iterate over each dimension
00417     pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[indices[i]], centroid));
00418   }
00419   centroid /= nr_points;
00420 }
00421 
00423 template <typename PointT> inline void
00424 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
00425                         const pcl::PointIndices &indices, Eigen::VectorXf &centroid)
00426 {
00427   return (pcl::computeNDCentroid<PointT> (cloud, indices.indices, centroid));
00428 }
00429 
00430 #endif  //#ifndef PCL_COMMON_IMPL_CENTROID_H_
00431 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines