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>
63 template <
typename Po
intT,
typename Scalar>
inline unsigned int
65 Eigen::Matrix<Scalar, 4, 1> ¢roid);
67 template <
typename Po
intT>
inline unsigned int
69 Eigen::Vector4f ¢roid)
71 return (compute3DCentroid <PointT, float> (cloud_iterator, centroid));
74 template <
typename Po
intT>
inline unsigned int
76 Eigen::Vector4d ¢roid)
78 return (compute3DCentroid <PointT, double> (cloud_iterator, centroid));
88 template <
typename Po
intT,
typename Scalar>
inline unsigned int
90 Eigen::Matrix<Scalar, 4, 1> ¢roid);
92 template <
typename Po
intT>
inline unsigned int
94 Eigen::Vector4f ¢roid)
96 return (compute3DCentroid <PointT, float> (cloud, centroid));
99 template <
typename Po
intT>
inline unsigned int
101 Eigen::Vector4d ¢roid)
103 return (compute3DCentroid <PointT, double> (cloud, centroid));
115 template <
typename Po
intT,
typename Scalar>
inline unsigned int
117 const std::vector<int> &indices,
118 Eigen::Matrix<Scalar, 4, 1> ¢roid);
120 template <
typename Po
intT>
inline unsigned int
122 const std::vector<int> &indices,
123 Eigen::Vector4f ¢roid)
125 return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
128 template <
typename Po
intT>
inline unsigned int
130 const std::vector<int> &indices,
131 Eigen::Vector4d ¢roid)
133 return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
145 template <
typename Po
intT,
typename Scalar>
inline unsigned int
148 Eigen::Matrix<Scalar, 4, 1> ¢roid);
150 template <
typename Po
intT>
inline unsigned int
153 Eigen::Vector4f ¢roid)
155 return (compute3DCentroid <PointT, float> (cloud, indices, centroid));
158 template <
typename Po
intT>
inline unsigned int
161 Eigen::Vector4d ¢roid)
163 return (compute3DCentroid <PointT, double> (cloud, indices, centroid));
179 template <
typename Po
intT,
typename Scalar>
inline unsigned int
181 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
182 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
184 template <
typename Po
intT>
inline unsigned int
186 const Eigen::Vector4f ¢roid,
187 Eigen::Matrix3f &covariance_matrix)
189 return (computeCovarianceMatrix<PointT, float> (cloud, centroid, covariance_matrix));
192 template <
typename Po
intT>
inline unsigned int
194 const Eigen::Vector4d ¢roid,
195 Eigen::Matrix3d &covariance_matrix)
197 return (computeCovarianceMatrix<PointT, double> (cloud, centroid, covariance_matrix));
213 template <
typename Po
intT,
typename Scalar>
inline unsigned int
215 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
216 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
218 template <
typename Po
intT>
inline unsigned int
220 const Eigen::Vector4f ¢roid,
221 Eigen::Matrix3f &covariance_matrix)
223 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, centroid, covariance_matrix));
226 template <
typename Po
intT>
inline unsigned int
228 const Eigen::Vector4d ¢roid,
229 Eigen::Matrix3d &covariance_matrix)
231 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, centroid, covariance_matrix));
247 template <
typename Po
intT,
typename Scalar>
inline unsigned int
249 const std::vector<int> &indices,
250 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
251 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
253 template <
typename Po
intT>
inline unsigned int
255 const std::vector<int> &indices,
256 const Eigen::Vector4f ¢roid,
257 Eigen::Matrix3f &covariance_matrix)
259 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
262 template <
typename Po
intT>
inline unsigned int
264 const std::vector<int> &indices,
265 const Eigen::Vector4d ¢roid,
266 Eigen::Matrix3d &covariance_matrix)
268 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
284 template <
typename Po
intT,
typename Scalar>
inline unsigned int
287 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
288 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
290 template <
typename Po
intT>
inline unsigned int
293 const Eigen::Vector4f ¢roid,
294 Eigen::Matrix3f &covariance_matrix)
296 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix));
299 template <
typename Po
intT>
inline unsigned int
302 const Eigen::Vector4d ¢roid,
303 Eigen::Matrix3d &covariance_matrix)
305 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix));
323 template <
typename Po
intT,
typename Scalar>
inline unsigned int
325 const std::vector<int> &indices,
326 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
327 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
329 template <
typename Po
intT>
inline unsigned int
331 const std::vector<int> &indices,
332 const Eigen::Vector4f ¢roid,
333 Eigen::Matrix3f &covariance_matrix)
335 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
338 template <
typename Po
intT>
inline unsigned int
340 const std::vector<int> &indices,
341 const Eigen::Vector4d ¢roid,
342 Eigen::Matrix3d &covariance_matrix)
344 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
361 template <
typename Po
intT,
typename Scalar>
inline unsigned int
364 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
365 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
367 template <
typename Po
intT>
inline unsigned int
370 const Eigen::Vector4f ¢roid,
371 Eigen::Matrix3f &covariance_matrix)
373 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix));
376 template <
typename Po
intT>
inline unsigned int
379 const Eigen::Vector4d ¢roid,
380 Eigen::Matrix3d &covariance_matrix)
382 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix));
397 template <
typename Po
intT,
typename Scalar>
inline unsigned int
399 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
400 Eigen::Matrix<Scalar, 4, 1> ¢roid);
402 template <
typename Po
intT>
inline unsigned int
404 Eigen::Matrix3f &covariance_matrix,
405 Eigen::Vector4f ¢roid)
407 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, covariance_matrix, centroid));
410 template <
typename Po
intT>
inline unsigned int
412 Eigen::Matrix3d &covariance_matrix,
413 Eigen::Vector4d ¢roid)
415 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, covariance_matrix, centroid));
431 template <
typename Po
intT,
typename Scalar>
inline unsigned int
433 const std::vector<int> &indices,
434 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
435 Eigen::Matrix<Scalar, 4, 1> ¢roid);
437 template <
typename Po
intT>
inline unsigned int
439 const std::vector<int> &indices,
440 Eigen::Matrix3f &covariance_matrix,
441 Eigen::Vector4f ¢roid)
443 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
446 template <
typename Po
intT>
inline unsigned int
448 const std::vector<int> &indices,
449 Eigen::Matrix3d &covariance_matrix,
450 Eigen::Vector4d ¢roid)
452 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
468 template <
typename Po
intT,
typename Scalar>
inline unsigned int
471 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
472 Eigen::Matrix<Scalar, 4, 1> ¢roid);
474 template <
typename Po
intT>
inline unsigned int
477 Eigen::Matrix3f &covariance_matrix,
478 Eigen::Vector4f ¢roid)
480 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid));
483 template <
typename Po
intT>
inline unsigned int
486 Eigen::Matrix3d &covariance_matrix,
487 Eigen::Vector4d ¢roid)
489 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid));
503 template <
typename Po
intT,
typename Scalar>
inline unsigned int
505 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
507 template <
typename Po
intT>
inline unsigned int
509 Eigen::Matrix3f &covariance_matrix)
511 return (computeCovarianceMatrix<PointT, float> (cloud, covariance_matrix));
514 template <
typename Po
intT>
inline unsigned int
516 Eigen::Matrix3d &covariance_matrix)
518 return (computeCovarianceMatrix<PointT, double> (cloud, covariance_matrix));
533 template <
typename Po
intT,
typename Scalar>
inline unsigned int
535 const std::vector<int> &indices,
536 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
538 template <
typename Po
intT>
inline unsigned int
540 const std::vector<int> &indices,
541 Eigen::Matrix3f &covariance_matrix)
543 return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
546 template <
typename Po
intT>
inline unsigned int
548 const std::vector<int> &indices,
549 Eigen::Matrix3d &covariance_matrix)
551 return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
566 template <
typename Po
intT,
typename Scalar>
inline unsigned int
569 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix);
571 template <
typename Po
intT>
inline unsigned int
574 Eigen::Matrix3f &covariance_matrix)
576 return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix));
579 template <
typename Po
intT>
inline unsigned int
582 Eigen::Matrix3d &covariance_matrix)
584 return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix));
594 template <
typename Po
intT,
typename Scalar>
void
596 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
600 template <
typename Po
intT>
void
602 const Eigen::Vector4f ¢roid,
606 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
609 template <
typename Po
intT>
void
611 const Eigen::Vector4d ¢roid,
615 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
624 template <
typename Po
intT,
typename Scalar>
void
626 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
629 template <
typename Po
intT>
void
631 const Eigen::Vector4f ¢roid,
634 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out));
637 template <
typename Po
intT>
void
639 const Eigen::Vector4d ¢roid,
642 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out));
652 template <
typename Po
intT,
typename Scalar>
void
654 const std::vector<int> &indices,
655 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
658 template <
typename Po
intT>
void
660 const std::vector<int> &indices,
661 const Eigen::Vector4f ¢roid,
664 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
667 template <
typename Po
intT>
void
669 const std::vector<int> &indices,
670 const Eigen::Vector4d ¢roid,
673 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
683 template <
typename Po
intT,
typename Scalar>
void
686 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
689 template <
typename Po
intT>
void
692 const Eigen::Vector4f ¢roid,
695 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
698 template <
typename Po
intT>
void
701 const Eigen::Vector4d ¢roid,
704 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
716 template <
typename Po
intT,
typename Scalar>
void
718 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
719 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
722 template <
typename Po
intT>
void
724 const Eigen::Vector4f ¢roid,
725 Eigen::MatrixXf &cloud_out,
728 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts));
731 template <
typename Po
intT>
void
733 const Eigen::Vector4d ¢roid,
734 Eigen::MatrixXd &cloud_out,
737 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts));
748 template <
typename Po
intT,
typename Scalar>
void
750 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
751 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
753 template <
typename Po
intT>
void
755 const Eigen::Vector4f ¢roid,
756 Eigen::MatrixXf &cloud_out)
758 return (demeanPointCloud<PointT, float> (cloud_in, centroid, cloud_out));
761 template <
typename Po
intT>
void
763 const Eigen::Vector4d ¢roid,
764 Eigen::MatrixXd &cloud_out)
766 return (demeanPointCloud<PointT, double> (cloud_in, centroid, cloud_out));
778 template <
typename Po
intT,
typename Scalar>
void
780 const std::vector<int> &indices,
781 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
782 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
784 template <
typename Po
intT>
void
786 const std::vector<int> &indices,
787 const Eigen::Vector4f ¢roid,
788 Eigen::MatrixXf &cloud_out)
790 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
793 template <
typename Po
intT>
void
795 const std::vector<int> &indices,
796 const Eigen::Vector4d ¢roid,
797 Eigen::MatrixXd &cloud_out)
799 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
811 template <
typename Po
intT,
typename Scalar>
void
814 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
815 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out);
817 template <
typename Po
intT>
void
820 const Eigen::Vector4f ¢roid,
821 Eigen::MatrixXf &cloud_out)
823 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out));
826 template <
typename Po
intT>
void
829 const Eigen::Vector4d ¢roid,
830 Eigen::MatrixXd &cloud_out)
832 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out));
836 template<
typename Po
intT,
typename Scalar>
843 centroid_ (centroid),
844 p_ (reinterpret_cast<const
Pod&>(p)) { }
850 const T* data_ptr =
reinterpret_cast<const T*
>(raw_ptr);
853 if (!pcl_isfinite (*data_ptr))
859 centroid_[f_idx_++] += *data_ptr;
864 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid_;
874 template <
typename Po
intT,
typename Scalar>
inline void
876 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid);
878 template <
typename Po
intT>
inline void
880 Eigen::VectorXf ¢roid)
882 return (computeNDCentroid<PointT, float> (cloud, centroid));
885 template <
typename Po
intT>
inline void
887 Eigen::VectorXd ¢roid)
889 return (computeNDCentroid<PointT, double> (cloud, centroid));
899 template <
typename Po
intT,
typename Scalar>
inline void
901 const std::vector<int> &indices,
902 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid);
904 template <
typename Po
intT>
inline void
906 const std::vector<int> &indices,
907 Eigen::VectorXf ¢roid)
909 return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
912 template <
typename Po
intT>
inline void
914 const std::vector<int> &indices,
915 Eigen::VectorXd ¢roid)
917 return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
927 template <
typename Po
intT,
typename Scalar>
inline void
930 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid);
932 template <
typename Po
intT>
inline void
935 Eigen::VectorXf ¢roid)
937 return (computeNDCentroid<PointT, float> (cloud, indices, centroid));
940 template <
typename Po
intT>
inline void
943 Eigen::VectorXd ¢roid)
945 return (computeNDCentroid<PointT, double> (cloud, indices, centroid));
950 #include <pcl/common/impl/centroid.hpp>
952 #endif //#ifndef PCL_COMMON_CENTROID_H_
Iterator class for point clouds with or without given indices.
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...
Helper functor structure for n-D centroid estimation.
traits::POD< PointT >::type Pod
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)
A point structure representing Euclidean xyz coordinates, and the RGB color.
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.