pcl: centroid.h Source File

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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  */
00035 
00036 #ifndef PCL_COMMON_CENTROID_H_
00037 #define PCL_COMMON_CENTROID_H_
00038 
00039 #include <pcl/point_cloud.h>
00040 #include <pcl/point_traits.h>
00041 #include <pcl/PointIndices.h>
00042 
00050 namespace pcl
00051 {
00060   template <typename PointT> inline unsigned int
00061   compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &centroid);
00062 
00072   template <typename PointT> inline unsigned int
00073   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00074                      const std::vector<int> &indices, Eigen::Vector4f &centroid);
00075 
00085   template <typename PointT> inline unsigned int
00086   compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00087                      const pcl::PointIndices &indices, Eigen::Vector4f &centroid);
00088 
00102   template <typename PointT> inline unsigned int
00103   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00104                            const Eigen::Vector4f &centroid,
00105                            Eigen::Matrix3f &covariance_matrix);
00106 
00120   template <typename PointT> inline unsigned int
00121   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00122                                      const Eigen::Vector4f &centroid,
00123                                      Eigen::Matrix3f &covariance_matrix);
00124 
00138   template <typename PointT> inline unsigned int
00139   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00140                            const std::vector<int> &indices,
00141                            const Eigen::Vector4f &centroid,
00142                            Eigen::Matrix3f &covariance_matrix);
00143 
00157   template <typename PointT> inline unsigned int
00158   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00159                            const pcl::PointIndices &indices,
00160                            const Eigen::Vector4f &centroid,
00161                            Eigen::Matrix3f &covariance_matrix);
00162 
00178   template <typename PointT> inline unsigned int
00179   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00180                                      const std::vector<int> &indices,
00181                                      const Eigen::Vector4f &centroid,
00182                                      Eigen::Matrix3f &covariance_matrix);
00183 
00198   template <typename PointT> inline unsigned int
00199   computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00200                                      const pcl::PointIndices &indices,
00201                                      const Eigen::Vector4f &centroid,
00202                                      Eigen::Matrix3f &covariance_matrix);
00203 
00216   template <typename PointT> inline unsigned int
00217   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00218                                   Eigen::Matrix3f &covariance_matrix,
00219                                   Eigen::Vector4f &centroid);
00220 
00234   template <typename PointT> inline unsigned int
00235   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00236                                   const std::vector<int> &indices,
00237                                   Eigen::Matrix3f &covariance_matrix,
00238                                   Eigen::Vector4f &centroid);
00239 
00253   template <typename PointT> inline unsigned int
00254   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00255                                   const pcl::PointIndices &indices,
00256                                   Eigen::Matrix3f &covariance_matrix,
00257                                   Eigen::Vector4f &centroid);
00258 
00270   template <typename PointT> inline unsigned int
00271   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00272                                   Eigen::Matrix3d &covariance_matrix,
00273                                   Eigen::Vector4d &centroid);
00274 
00287   template <typename PointT> inline unsigned int
00288   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00289                                   const std::vector<int> &indices,
00290                                   Eigen::Matrix3d &covariance_matrix,
00291                                   Eigen::Vector4d &centroid);
00292 
00305   template <typename PointT> inline unsigned int
00306   computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00307                                   const pcl::PointIndices &indices,
00308                                   Eigen::Matrix3d &covariance_matrix,
00309                                   Eigen::Vector4d &centroid);
00310 
00322   template <typename PointT> inline unsigned int
00323   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00324                            Eigen::Matrix3f &covariance_matrix);
00325 
00338   template <typename PointT> inline unsigned int
00339   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00340                           const std::vector<int> &indices,
00341                           Eigen::Matrix3f &covariance_matrix);
00342 
00355   template <typename PointT> inline unsigned int
00356   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00357                            const pcl::PointIndices &indices,
00358                            Eigen::Matrix3f &covariance_matrix);
00359 
00370   template <typename PointT> inline unsigned int
00371   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00372                            Eigen::Matrix3d &covariance_matrix);
00373 
00385   template <typename PointT> inline unsigned int
00386   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00387                            const std::vector<int> &indices,
00388                            Eigen::Matrix3d &covariance_matrix);
00389 
00401   template <typename PointT> inline unsigned int
00402   computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00403                            const pcl::PointIndices &indices,
00404                            Eigen::Matrix3d &covariance_matrix);
00405 
00412   template <typename PointT> void
00413   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00414                     const Eigen::Vector4f &centroid,
00415                     pcl::PointCloud<PointT> &cloud_out);
00416 
00424   template <typename PointT> void
00425   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00426                     const std::vector<int> &indices,
00427                     const Eigen::Vector4f &centroid,
00428                     pcl::PointCloud<PointT> &cloud_out);
00429 
00438   template <typename PointT> void
00439   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00440                     const Eigen::Vector4f &centroid,
00441                     Eigen::MatrixXf &cloud_out);
00442 
00452   template <typename PointT> void
00453   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00454                     const std::vector<int> &indices,
00455                     const Eigen::Vector4f &centroid,
00456                     Eigen::MatrixXf &cloud_out);
00457 
00467   template <typename PointT> void
00468   demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00469                     const pcl::PointIndices& indices,
00470                     const Eigen::Vector4f &centroid,
00471                     Eigen::MatrixXf &cloud_out);
00472 
00474   template<typename PointT>
00475   struct NdCentroidFunctor
00476   {
00477     typedef typename traits::POD<PointT>::type Pod;
00478 
00479     NdCentroidFunctor (const PointT &p, Eigen::VectorXf &centroid)
00480       : f_idx_ (0),
00481         centroid_ (centroid),
00482         p_ (reinterpret_cast<const Pod&>(p)) { }
00483 
00484     template<typename Key> inline void operator() ()
00485     {
00486       typedef typename pcl::traits::datatype<PointT, Key>::type T;
00487       const uint8_t* raw_ptr = reinterpret_cast<const uint8_t*>(&p_) + pcl::traits::offset<PointT, Key>::value;
00488       const T* data_ptr = reinterpret_cast<const T*>(raw_ptr);
00489 
00490       // Check if the value is invalid
00491       if (!pcl_isfinite (*data_ptr))
00492       {
00493         f_idx_++;
00494         return;
00495       }
00496 
00497       centroid_[f_idx_++] += *data_ptr;
00498     }
00499 
00500     private:
00501       int f_idx_;
00502       Eigen::VectorXf &centroid_;
00503       const Pod &p_;
00504   };
00505 
00512   template <typename PointT> inline void
00513   computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf &centroid);
00514 
00522   template <typename PointT> inline void
00523   computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00524                      const std::vector<int> &indices, Eigen::VectorXf &centroid);
00525 
00533   template <typename PointT> inline void
00534   computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00535                      const pcl::PointIndices &indices, Eigen::VectorXf &centroid);
00536 
00537 }
00539 #include <pcl/common/impl/centroid.hpp>
00540 
00541 #endif  //#ifndef PCL_COMMON_CENTROID_H_