39 #ifndef PCL_COMMON_CENTROID_H_ 40 #define PCL_COMMON_CENTROID_H_ 42 #include <pcl/point_cloud.h> 43 #include <pcl/point_traits.h> 44 #include <pcl/PointIndices.h> 45 #include <pcl/cloud_iterator.h> 64 template <
typename Po
intT,
typename Scalar>
inline unsigned int 66 Eigen::Matrix<Scalar, 4, 1> ¢roid);
68 template <
typename Po
intT>
inline unsigned int 70 Eigen::Vector4f ¢roid)
72 return (compute3DCentroid <PointT, float> (cloud_iterator, centroid));
75 template <
typename Po
intT>
inline unsigned int 77 Eigen::Vector4d ¢roid)
79 return (compute3DCentroid <PointT, double> (cloud_iterator, centroid));
90 template <
typename Po
intT,
typename Scalar>
inline unsigned int 92 Eigen::Matrix<Scalar, 4, 1> ¢roid);
94 template <
typename Po
intT>
inline unsigned int 96 Eigen::Vector4f ¢roid)
98 return (compute3DCentroid <PointT, float> (cloud, centroid));
101 template <
typename Po
intT>
inline unsigned int 103 Eigen::Vector4d ¢roid)
105 return (compute3DCentroid <PointT, double> (cloud, centroid));
118 template <
typename Po
intT,
typename Scalar>
inline unsigned int 120 const std::vector<int> &indices,
121 Eigen::Matrix<Scalar, 4, 1> ¢roid);
123 template <
typename Po
intT>
inline unsigned int 125 const std::vector<int> &indices,
126 Eigen::Vector4f ¢roid)
128 return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
131 template <
typename Po
intT>
inline unsigned int 133 const std::vector<int> &indices,
134 Eigen::Vector4d ¢roid)
136 return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
149 template <
typename Po
intT,
typename Scalar>
inline unsigned int 152 Eigen::Matrix<Scalar, 4, 1> ¢roid);
154 template <
typename Po
intT>
inline unsigned int 157 Eigen::Vector4f ¢roid)
159 return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
162 template <
typename Po
intT>
inline unsigned int 165 Eigen::Vector4d ¢roid)
167 return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
183 template <
typename Po
intT,
typename Scalar>
inline unsigned int 185 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
186 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
188 template <
typename Po
intT>
inline unsigned int 190 const Eigen::Vector4f ¢roid,
191 Eigen::Matrix3f &covariance_matrix)
193 return (computeCovarianceMatrix<PointT, float> (cloud, centroid, covariance_matrix));
196 template <
typename Po
intT>
inline unsigned int 198 const Eigen::Vector4d ¢roid,
199 Eigen::Matrix3d &covariance_matrix)
201 return (computeCovarianceMatrix<PointT, double> (cloud, centroid, covariance_matrix));
217 template <
typename Po
intT,
typename Scalar>
inline unsigned int 219 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
220 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
222 template <
typename Po
intT>
inline unsigned int 224 const Eigen::Vector4f ¢roid,
225 Eigen::Matrix3f &covariance_matrix)
227 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, centroid, covariance_matrix));
230 template <
typename Po
intT>
inline unsigned int 232 const Eigen::Vector4d ¢roid,
233 Eigen::Matrix3d &covariance_matrix)
235 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, centroid, covariance_matrix));
251 template <
typename Po
intT,
typename Scalar>
inline unsigned int 253 const std::vector<int> &indices,
254 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
255 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
257 template <
typename Po
intT>
inline unsigned int 259 const std::vector<int> &indices,
260 const Eigen::Vector4f ¢roid,
261 Eigen::Matrix3f &covariance_matrix)
263 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
266 template <
typename Po
intT>
inline unsigned int 268 const std::vector<int> &indices,
269 const Eigen::Vector4d ¢roid,
270 Eigen::Matrix3d &covariance_matrix)
272 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
288 template <
typename Po
intT,
typename Scalar>
inline unsigned int 291 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
292 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
294 template <
typename Po
intT>
inline unsigned int 297 const Eigen::Vector4f ¢roid,
298 Eigen::Matrix3f &covariance_matrix)
300 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
303 template <
typename Po
intT>
inline unsigned int 306 const Eigen::Vector4d ¢roid,
307 Eigen::Matrix3d &covariance_matrix)
309 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
327 template <
typename Po
intT,
typename Scalar>
inline unsigned int 329 const std::vector<int> &indices,
330 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
331 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
333 template <
typename Po
intT>
inline unsigned int 335 const std::vector<int> &indices,
336 const Eigen::Vector4f ¢roid,
337 Eigen::Matrix3f &covariance_matrix)
339 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
342 template <
typename Po
intT>
inline unsigned int 344 const std::vector<int> &indices,
345 const Eigen::Vector4d ¢roid,
346 Eigen::Matrix3d &covariance_matrix)
348 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
365 template <
typename Po
intT,
typename Scalar>
inline unsigned int 368 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
369 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
371 template <
typename Po
intT>
inline unsigned int 374 const Eigen::Vector4f ¢roid,
375 Eigen::Matrix3f &covariance_matrix)
377 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
380 template <
typename Po
intT>
inline unsigned int 383 const Eigen::Vector4d ¢roid,
384 Eigen::Matrix3d &covariance_matrix)
386 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
401 template <
typename Po
intT,
typename Scalar>
inline unsigned int 403 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
404 Eigen::Matrix<Scalar, 4, 1> ¢roid);
406 template <
typename Po
intT>
inline unsigned int 408 Eigen::Matrix3f &covariance_matrix,
409 Eigen::Vector4f ¢roid)
411 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, covariance_matrix, centroid));
414 template <
typename Po
intT>
inline unsigned int 416 Eigen::Matrix3d &covariance_matrix,
417 Eigen::Vector4d ¢roid)
419 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, covariance_matrix, centroid));
435 template <
typename Po
intT,
typename Scalar>
inline unsigned int 437 const std::vector<int> &indices,
438 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
439 Eigen::Matrix<Scalar, 4, 1> ¢roid);
441 template <
typename Po
intT>
inline unsigned int 443 const std::vector<int> &indices,
444 Eigen::Matrix3f &covariance_matrix,
445 Eigen::Vector4f ¢roid)
447 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
450 template <
typename Po
intT>
inline unsigned int 452 const std::vector<int> &indices,
453 Eigen::Matrix3d &covariance_matrix,
454 Eigen::Vector4d ¢roid)
456 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
472 template <
typename Po
intT,
typename Scalar>
inline unsigned int 475 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
476 Eigen::Matrix<Scalar, 4, 1> ¢roid);
478 template <
typename Po
intT>
inline unsigned int 481 Eigen::Matrix3f &covariance_matrix,
482 Eigen::Vector4f ¢roid)
484 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
487 template <
typename Po
intT>
inline unsigned int 490 Eigen::Matrix3d &covariance_matrix,
491 Eigen::Vector4d ¢roid)
493 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
507 template <
typename Po
intT,
typename Scalar>
inline unsigned int 509 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
511 template <
typename Po
intT>
inline unsigned int 513 Eigen::Matrix3f &covariance_matrix)
515 return (computeCovarianceMatrix<PointT, float> (cloud, covariance_matrix));
518 template <
typename Po
intT>
inline unsigned int 520 Eigen::Matrix3d &covariance_matrix)
522 return (computeCovarianceMatrix<PointT, double> (cloud, covariance_matrix));
537 template <
typename Po
intT,
typename Scalar>
inline unsigned int 539 const std::vector<int> &indices,
540 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
542 template <
typename Po
intT>
inline unsigned int 544 const std::vector<int> &indices,
545 Eigen::Matrix3f &covariance_matrix)
547 return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
550 template <
typename Po
intT>
inline unsigned int 552 const std::vector<int> &indices,
553 Eigen::Matrix3d &covariance_matrix)
555 return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
570 template <
typename Po
intT,
typename Scalar>
inline unsigned int 573 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
575 template <
typename Po
intT>
inline unsigned int 578 Eigen::Matrix3f &covariance_matrix)
580 return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
583 template <
typename Po
intT>
inline unsigned int 586 Eigen::Matrix3d &covariance_matrix)
588 return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
598 template <
typename Po
intT,
typename Scalar>
void 600 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
604 template <
typename Po
intT>
void 606 const Eigen::Vector4f ¢roid,
610 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
613 template <
typename Po
intT>
void 615 const Eigen::Vector4d ¢roid,
619 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
628 template <
typename Po
intT,
typename Scalar>
void 630 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
633 template <
typename Po
intT>
void 635 const Eigen::Vector4f ¢roid,
638 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out));
641 template <
typename Po
intT>
void 643 const Eigen::Vector4d ¢roid,
646 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out));
656 template <
typename Po
intT,
typename Scalar>
void 658 const std::vector<int> &indices,
659 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
662 template <
typename Po
intT>
void 664 const std::vector<int> &indices,
665 const Eigen::Vector4f ¢roid,
668 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
671 template <
typename Po
intT>
void 673 const std::vector<int> &indices,
674 const Eigen::Vector4d ¢roid,
677 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
687 template <
typename Po
intT,
typename Scalar>
void 690 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
693 template <
typename Po
intT>
void 696 const Eigen::Vector4f ¢roid,
699 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
702 template <
typename Po
intT>
void 705 const Eigen::Vector4d ¢roid,
708 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
720 template <
typename Po
intT,
typename Scalar>
void 722 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
723 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
726 template <
typename Po
intT>
void 728 const Eigen::Vector4f ¢roid,
729 Eigen::MatrixXf &cloud_out,
732 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
735 template <
typename Po
intT>
void 737 const Eigen::Vector4d ¢roid,
738 Eigen::MatrixXd &cloud_out,
741 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
752 template <
typename Po
intT,
typename Scalar>
void 754 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
755 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
757 template <
typename Po
intT>
void 759 const Eigen::Vector4f ¢roid,
760 Eigen::MatrixXf &cloud_out)
762 return (demeanPointCloud<PointT, float> (cloud_in, centroid, cloud_out));
765 template <
typename Po
intT>
void 767 const Eigen::Vector4d ¢roid,
768 Eigen::MatrixXd &cloud_out)
770 return (demeanPointCloud<PointT, double> (cloud_in, centroid, cloud_out));
782 template <
typename Po
intT,
typename Scalar>
void 784 const std::vector<int> &indices,
785 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
786 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
788 template <
typename Po
intT>
void 790 const std::vector<int> &indices,
791 const Eigen::Vector4f ¢roid,
792 Eigen::MatrixXf &cloud_out)
794 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
797 template <
typename Po
intT>
void 799 const std::vector<int> &indices,
800 const Eigen::Vector4d ¢roid,
801 Eigen::MatrixXd &cloud_out)
803 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
815 template <
typename Po
intT,
typename Scalar>
void 818 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
819 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
821 template <
typename Po
intT>
void 824 const Eigen::Vector4f ¢roid,
825 Eigen::MatrixXf &cloud_out)
827 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
830 template <
typename Po
intT>
void 833 const Eigen::Vector4d ¢roid,
834 Eigen::MatrixXd &cloud_out)
836 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
840 template<
typename Po
intT,
typename Scalar>
847 centroid_ (centroid),
848 p_ (reinterpret_cast<const
Pod&>(p)) { }
854 const T* data_ptr = reinterpret_cast<const T*>(raw_ptr);
857 if (!pcl_isfinite (*data_ptr))
863 centroid_[f_idx_++] += *data_ptr;
868 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid_;
878 template <
typename Po
intT,
typename Scalar>
inline void 880 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid);
882 template <
typename Po
intT>
inline void 884 Eigen::VectorXf ¢roid)
886 return (computeNDCentroid<PointT, float> (cloud, centroid));
889 template <
typename Po
intT>
inline void 891 Eigen::VectorXd ¢roid)
893 return (computeNDCentroid<PointT, double> (cloud, centroid));
903 template <
typename Po
intT,
typename Scalar>
inline void 905 const std::vector<int> &indices,
906 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid);
908 template <
typename Po
intT>
inline void 910 const std::vector<int> &indices,
911 Eigen::VectorXf ¢roid)
913 return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
916 template <
typename Po
intT>
inline void 918 const std::vector<int> &indices,
919 Eigen::VectorXd ¢roid)
921 return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
931 template <
typename Po
intT,
typename Scalar>
inline void 934 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid);
936 template <
typename Po
intT>
inline void 939 Eigen::VectorXf ¢roid)
941 return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
944 template <
typename Po
intT>
inline void 947 Eigen::VectorXd ¢roid)
949 return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
954 #include <pcl/common/impl/accumulators.hpp> 1036 template <
typename Po
intT>
1068 template <
typename Po
intOutT>
void 1071 if (num_points_ != 0)
1085 return (num_points_);
1088 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1113 template <
typename Po
intInT,
typename Po
intOutT>
size_t 1115 PointOutT& centroid);
1125 template <
typename Po
intInT,
typename Po
intOutT>
size_t 1127 const std::vector<int>& indices,
1128 PointOutT& centroid);
1132 #include <pcl/common/impl/centroid.hpp> 1134 #endif //#ifndef PCL_COMMON_CENTROID_H_
Iterator class for point clouds with or without given indices.
size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT ¢roid)
Compute the centroid of a set of points and return it as a point.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
size_t getSize() const
Get the total number of points that were added.
boost::fusion::result_of::as_vector< typename boost::mpl::filter_view< boost::mpl::vector< AccumulatorXYZ, AccumulatorNormal, AccumulatorCurvature, AccumulatorRGBA, AccumulatorIntensity, AccumulatorLabel >, boost::mpl::and_< IsCompatible< boost::mpl::_1, Point1T >, IsCompatible< boost::mpl::_1, Point2T > > > >::type type
Helper functor structure for n-D centroid estimation.
traits::POD< PointT >::type Pod
void add(const PointT &point)
Add a new point to the centroid computation.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
void computeNDCentroid(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > ¢roid)
General, all purpose nD centroid estimation for a set of points using their indices.
NdCentroidFunctor(const PointT &p, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > ¢roid)
PointCloud represents the base class in PCL for storing collections of 3D points.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void get(PointOutT &point) const
Retrieve the current centroid.
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
A generic class that computes the centroid of points fed to it.