00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 ¢roid);
00062
00072 template <typename PointT> inline unsigned int
00073 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00074 const std::vector<int> &indices, Eigen::Vector4f ¢roid);
00075
00085 template <typename PointT> inline unsigned int
00086 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00087 const pcl::PointIndices &indices, Eigen::Vector4f ¢roid);
00088
00102 template <typename PointT> inline unsigned int
00103 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00104 const Eigen::Vector4f ¢roid,
00105 Eigen::Matrix3f &covariance_matrix);
00106
00120 template <typename PointT> inline unsigned int
00121 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00122 const Eigen::Vector4f ¢roid,
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 ¢roid,
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 ¢roid,
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 ¢roid,
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 ¢roid,
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 ¢roid);
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 ¢roid);
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 ¢roid);
00258
00270 template <typename PointT> inline unsigned int
00271 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00272 Eigen::Matrix3d &covariance_matrix,
00273 Eigen::Vector4d ¢roid);
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 ¢roid);
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 ¢roid);
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 ¢roid,
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 ¢roid,
00428 pcl::PointCloud<PointT> &cloud_out);
00429
00438 template <typename PointT> void
00439 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00440 const Eigen::Vector4f ¢roid,
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 ¢roid,
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 ¢roid,
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 ¢roid)
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
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 ¢roid_;
00503 const Pod &p_;
00504 };
00505
00512 template <typename PointT> inline void
00513 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf ¢roid);
00514
00522 template <typename PointT> inline void
00523 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00524 const std::vector<int> &indices, Eigen::VectorXf ¢roid);
00525
00533 template <typename PointT> inline void
00534 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00535 const pcl::PointIndices &indices, Eigen::VectorXf ¢roid);
00536
00537 }
00539 #include <pcl/common/impl/centroid.hpp>
00540
00541 #endif //#ifndef PCL_COMMON_CENTROID_H_