44#include <pcl/conversions.h>
45#include <pcl/common/point_tests.h>
47#include <boost/fusion/algorithm/transformation/filter_if.hpp>
48#include <boost/fusion/algorithm/iteration/for_each.hpp>
49#include <boost/mpl/size.hpp>
55template <
typename Po
intT,
typename Scalar>
inline unsigned int
57 Eigen::Matrix<Scalar, 4, 1> ¢roid)
78 centroid /=
static_cast<Scalar
> (cp);
84template <
typename Po
intT,
typename Scalar>
inline unsigned int
86 Eigen::Matrix<Scalar, 4, 1> ¢roid)
97 for (
const auto& point: cloud)
99 centroid[0] += point.x;
100 centroid[1] += point.y;
101 centroid[2] += point.z;
103 centroid /=
static_cast<Scalar
> (cloud.
size ());
106 return (
static_cast<unsigned int> (cloud.
size ()));
110 for (
const auto& point: cloud)
116 centroid[0] += point.x;
117 centroid[1] += point.y;
118 centroid[2] += point.z;
121 centroid /=
static_cast<Scalar
> (cp);
128template <
typename Po
intT,
typename Scalar>
inline unsigned int
131 Eigen::Matrix<Scalar, 4, 1> ¢roid)
133 if (indices.empty ())
141 for (
const auto& index : indices)
143 centroid[0] += cloud[index].x;
144 centroid[1] += cloud[index].y;
145 centroid[2] += cloud[index].z;
147 centroid /=
static_cast<Scalar
> (indices.size ());
149 return (
static_cast<unsigned int> (indices.size ()));
153 for (
const auto& index : indices)
159 centroid[0] += cloud[index].x;
160 centroid[1] += cloud[index].y;
161 centroid[2] += cloud[index].z;
164 centroid /=
static_cast<Scalar
> (cp);
170template <
typename Po
intT,
typename Scalar>
inline unsigned int
173 Eigen::Matrix<Scalar, 4, 1> ¢roid)
179template <
typename Po
intT,
typename Scalar>
inline unsigned
181 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
196 for (
const auto& point: cloud)
198 Eigen::Matrix<Scalar, 4, 1>
pt;
199 pt[0] = point.x - centroid[0];
200 pt[1] = point.y - centroid[1];
201 pt[2] = point.z - centroid[2];
219 for (
const auto& point: cloud)
225 Eigen::Matrix<Scalar, 4, 1>
pt;
226 pt[0] = point.x - centroid[0];
227 pt[1] = point.y - centroid[1];
228 pt[2] = point.z - centroid[2];
250template <
typename Po
intT,
typename Scalar>
inline unsigned int
252 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
262template <
typename Po
intT,
typename Scalar>
inline unsigned int
265 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
268 if (indices.empty ())
280 for (
const auto& idx: indices)
282 Eigen::Matrix<Scalar, 4, 1>
pt;
283 pt[0] = cloud[idx].x - centroid[0];
284 pt[1] = cloud[idx].y - centroid[1];
285 pt[2] = cloud[idx].z - centroid[2];
303 for (
const auto &index : indices)
309 Eigen::Matrix<Scalar, 4, 1>
pt;
310 pt[0] = cloud[index].x - centroid[0];
311 pt[1] = cloud[index].y - centroid[1];
312 pt[2] = cloud[index].z - centroid[2];
333template <
typename Po
intT,
typename Scalar>
inline unsigned int
336 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
343template <
typename Po
intT,
typename Scalar>
inline unsigned int
346 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
357template <
typename Po
intT,
typename Scalar>
inline unsigned int
360 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
367template <
typename Po
intT,
typename Scalar>
inline unsigned int
372 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>
accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
379 for (
const auto& point: cloud)
381 accu [0] += point.x * point.x;
382 accu [1] += point.x * point.y;
383 accu [2] += point.x * point.z;
384 accu [3] += point.y * point.y;
385 accu [4] += point.y * point.z;
386 accu [5] += point.z * point.z;
392 for (
const auto& point: cloud)
397 accu [0] += point.x * point.x;
398 accu [1] += point.x * point.y;
399 accu [2] += point.x * point.z;
400 accu [3] += point.y * point.y;
401 accu [4] += point.y * point.z;
402 accu [5] += point.z * point.z;
421template <
typename Po
intT,
typename Scalar>
inline unsigned int
427 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>
accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
432 point_count =
static_cast<unsigned int> (indices.size ());
433 for (
const auto &index : indices)
436 accu [0] += cloud[index].x * cloud[index].x;
437 accu [1] += cloud[index].x * cloud[index].y;
438 accu [2] += cloud[index].x * cloud[index].z;
439 accu [3] += cloud[index].y * cloud[index].y;
440 accu [4] += cloud[index].y * cloud[index].z;
441 accu [5] += cloud[index].z * cloud[index].z;
447 for (
const auto &index : indices)
453 accu [0] += cloud[index].x * cloud[index].x;
454 accu [1] += cloud[index].x * cloud[index].y;
455 accu [2] += cloud[index].x * cloud[index].z;
456 accu [3] += cloud[index].y * cloud[index].y;
457 accu [4] += cloud[index].y * cloud[index].z;
458 accu [5] += cloud[index].z * cloud[index].z;
475template <
typename Po
intT,
typename Scalar>
inline unsigned int
484template <
typename Po
intT,
typename Scalar>
inline unsigned int
487 Eigen::Matrix<Scalar, 4, 1> ¢roid)
490 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>
accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
496 for (
const auto& point: cloud)
498 accu [0] += point.x * point.x;
499 accu [1] += point.x * point.y;
500 accu [2] += point.x * point.z;
501 accu [3] += point.y * point.y;
502 accu [4] += point.y * point.z;
503 accu [5] += point.z * point.z;
512 for (
const auto& point: cloud)
517 accu [0] += point.x * point.x;
518 accu [1] += point.x * point.y;
519 accu [2] += point.x * point.z;
520 accu [3] += point.y * point.y;
521 accu [4] += point.y * point.z;
522 accu [5] += point.z * point.z;
533 centroid[0] =
accu[6]; centroid[1] =
accu[7]; centroid[2] =
accu[8];
549template <
typename Po
intT,
typename Scalar>
inline unsigned int
553 Eigen::Matrix<Scalar, 4, 1> ¢roid)
556 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>
accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
561 for (
const auto &index : indices)
564 accu [0] += cloud[index].x * cloud[index].x;
565 accu [1] += cloud[index].x * cloud[index].y;
566 accu [2] += cloud[index].x * cloud[index].z;
567 accu [3] += cloud[index].y * cloud[index].y;
568 accu [4] += cloud[index].y * cloud[index].z;
569 accu [5] += cloud[index].z * cloud[index].z;
570 accu [6] += cloud[index].x;
571 accu [7] += cloud[index].y;
572 accu [8] += cloud[index].z;
578 for (
const auto &index : indices)
584 accu [0] += cloud[index].x * cloud[index].x;
585 accu [1] += cloud[index].x * cloud[index].y;
586 accu [2] += cloud[index].x * cloud[index].z;
587 accu [3] += cloud[index].y * cloud[index].y;
588 accu [4] += cloud[index].y * cloud[index].z;
589 accu [5] += cloud[index].z * cloud[index].z;
590 accu [6] += cloud[index].x;
591 accu [7] += cloud[index].y;
592 accu [8] += cloud[index].z;
600 centroid[0] =
accu[6]; centroid[1] =
accu[7]; centroid[2] =
accu[8];
616template <
typename Po
intT,
typename Scalar>
inline unsigned int
620 Eigen::Matrix<Scalar, 4, 1> ¢roid)
626template <
typename Po
intT,
typename Scalar>
void
628 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
659template <
typename Po
intT,
typename Scalar>
void
661 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
669 point.x -=
static_cast<float> (centroid[0]);
670 point.y -=
static_cast<float> (centroid[1]);
671 point.z -=
static_cast<float> (centroid[2]);
676template <
typename Po
intT,
typename Scalar>
void
679 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
697 for (std::size_t i = 0; i < indices.size (); ++i)
706template <
typename Po
intT,
typename Scalar>
void
709 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
716template <
typename Po
intT,
typename Scalar>
void
718 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
719 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &
cloud_out,
733 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4,
npts);
747template <
typename Po
intT,
typename Scalar>
void
749 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
750 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &
cloud_out)
754 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4,
npts);
756 for (std::size_t i = 0; i <
npts; ++i)
770template <
typename Po
intT,
typename Scalar>
void
773 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
774 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &
cloud_out)
778 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4,
npts);
780 for (std::size_t i = 0; i <
npts; ++i)
794template <
typename Po
intT,
typename Scalar>
void
797 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
798 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &
cloud_out)
804template <
typename Po
intT,
typename Scalar>
inline void
806 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
808 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
811 centroid.setZero (boost::mpl::size<FieldList>::value);
817 for (
const auto&
pt: cloud)
822 centroid /=
static_cast<Scalar
> (cloud.
size ());
826template <
typename Po
intT,
typename Scalar>
inline void
829 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
831 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
834 centroid.setZero (boost::mpl::size<FieldList>::value);
836 if (indices.empty ())
840 for (
const auto& index: indices)
845 centroid /=
static_cast<Scalar
> (indices.size ());
849template <
typename Po
intT,
typename Scalar>
inline void
852 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
857template <
typename Po
intT>
void
865template <
typename Po
intT>
866template <
typename Po
intOutT>
void
869 if (num_points_ != 0)
873 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
880template <
typename Po
intInT,
typename Po
intOutT> std::size_t
887 for (
const auto& point: cloud)
890 for (
const auto& point: cloud)
895 return (cp.getSize ());
899template <
typename Po
intInT,
typename Po
intOutT> std::size_t
907 for (
const auto &index : indices)
908 cp.add (cloud[index]);
910 for (
const auto &index : indices)
912 cp.add (cloud[index]);
915 return (cp.getSize ());
Define methods for centroid estimation and covariance matrix calculus.
void get(PointOutT &point) const
Retrieve the current centroid.
void add(const PointT &point)
Add a new point to the centroid computation.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
std::size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT ¢roid)
Compute the centroid of a set of points and return it as a point.
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.
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...
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 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 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.
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.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.