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>
55 template <
typename Po
intT,
typename Scalar>
inline unsigned int
57 Eigen::Matrix<Scalar, 4, 1> ¢roid)
66 while (cloud_iterator.
isValid ())
71 centroid[0] += cloud_iterator->x;
72 centroid[1] += cloud_iterator->y;
73 centroid[2] += cloud_iterator->z;
78 centroid /=
static_cast<Scalar
> (cp);
84 template <
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);
128 template <
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);
170 template <
typename Po
intT,
typename Scalar>
inline unsigned int
173 Eigen::Matrix<Scalar, 4, 1> ¢roid)
179 template <
typename Po
intT,
typename Scalar>
inline unsigned
181 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
182 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
188 covariance_matrix.setZero ();
190 unsigned point_count;
194 point_count =
static_cast<unsigned> (cloud.
size ());
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];
203 covariance_matrix (1, 1) += pt.y () * pt.y ();
204 covariance_matrix (1, 2) += pt.y () * pt.z ();
206 covariance_matrix (2, 2) += pt.z () * pt.z ();
209 covariance_matrix (0, 0) += pt.x ();
210 covariance_matrix (0, 1) += pt.y ();
211 covariance_matrix (0, 2) += pt.z ();
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];
230 covariance_matrix (1, 1) += pt.y () * pt.y ();
231 covariance_matrix (1, 2) += pt.y () * pt.z ();
233 covariance_matrix (2, 2) += pt.z () * pt.z ();
236 covariance_matrix (0, 0) += pt.x ();
237 covariance_matrix (0, 1) += pt.y ();
238 covariance_matrix (0, 2) += pt.z ();
242 covariance_matrix (1, 0) = covariance_matrix (0, 1);
243 covariance_matrix (2, 0) = covariance_matrix (0, 2);
244 covariance_matrix (2, 1) = covariance_matrix (1, 2);
246 return (point_count);
250 template <
typename Po
intT,
typename Scalar>
inline unsigned int
252 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
253 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
256 if (point_count != 0)
257 covariance_matrix /=
static_cast<Scalar
> (point_count);
258 return (point_count);
262 template <
typename Po
intT,
typename Scalar>
inline unsigned int
265 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
266 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
268 if (indices.empty ())
272 covariance_matrix.setZero ();
274 std::size_t point_count;
278 point_count = indices.size ();
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];
287 covariance_matrix (1, 1) += pt.y () * pt.y ();
288 covariance_matrix (1, 2) += pt.y () * pt.z ();
290 covariance_matrix (2, 2) += pt.z () * pt.z ();
293 covariance_matrix (0, 0) += pt.x ();
294 covariance_matrix (0, 1) += pt.y ();
295 covariance_matrix (0, 2) += pt.z ();
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];
314 covariance_matrix (1, 1) += pt.y () * pt.y ();
315 covariance_matrix (1, 2) += pt.y () * pt.z ();
317 covariance_matrix (2, 2) += pt.z () * pt.z ();
320 covariance_matrix (0, 0) += pt.x ();
321 covariance_matrix (0, 1) += pt.y ();
322 covariance_matrix (0, 2) += pt.z ();
326 covariance_matrix (1, 0) = covariance_matrix (0, 1);
327 covariance_matrix (2, 0) = covariance_matrix (0, 2);
328 covariance_matrix (2, 1) = covariance_matrix (1, 2);
329 return (
static_cast<unsigned int> (point_count));
333 template <
typename Po
intT,
typename Scalar>
inline unsigned int
336 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
337 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
343 template <
typename Po
intT,
typename Scalar>
inline unsigned int
346 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
347 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
350 if (point_count != 0)
351 covariance_matrix /=
static_cast<Scalar
> (point_count);
353 return (point_count);
357 template <
typename Po
intT,
typename Scalar>
inline unsigned int
360 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
361 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
367 template <
typename Po
intT,
typename Scalar>
inline unsigned int
369 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
372 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
374 unsigned int point_count;
377 point_count =
static_cast<unsigned int> (cloud.
size ());
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;
407 if (point_count != 0)
409 accu /=
static_cast<Scalar
> (point_count);
410 covariance_matrix.coeffRef (0) = accu [0];
411 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
412 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
413 covariance_matrix.coeffRef (4) = accu [3];
414 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
415 covariance_matrix.coeffRef (8) = accu [5];
417 return (point_count);
421 template <
typename Po
intT,
typename Scalar>
inline unsigned int
424 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
427 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
429 unsigned int point_count;
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;
461 if (point_count != 0)
463 accu /=
static_cast<Scalar
> (point_count);
464 covariance_matrix.coeffRef (0) = accu [0];
465 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
466 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
467 covariance_matrix.coeffRef (4) = accu [3];
468 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
469 covariance_matrix.coeffRef (8) = accu [5];
471 return (point_count);
475 template <
typename Po
intT,
typename Scalar>
inline unsigned int
478 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
484 template <
typename Po
intT,
typename Scalar>
inline unsigned int
486 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
487 Eigen::Matrix<Scalar, 4, 1> ¢roid)
490 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
491 std::size_t point_count;
494 point_count = cloud.
size ();
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;
529 accu /=
static_cast<Scalar
> (point_count);
530 if (point_count != 0)
533 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
535 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
536 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
537 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
538 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
539 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
540 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
541 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
542 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
543 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
545 return (
static_cast<unsigned int> (point_count));
549 template <
typename Po
intT,
typename Scalar>
inline unsigned int
552 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
553 Eigen::Matrix<Scalar, 4, 1> ¢roid)
556 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
557 std::size_t point_count;
560 point_count = indices.size ();
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;
596 accu /=
static_cast<Scalar
> (point_count);
600 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
602 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
603 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
604 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
605 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
606 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
607 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
608 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
609 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
610 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
612 return (
static_cast<unsigned int> (point_count));
616 template <
typename Po
intT,
typename Scalar>
inline unsigned int
619 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
620 Eigen::Matrix<Scalar, 4, 1> ¢roid)
626 template <
typename Po
intT,
typename Scalar>
void
628 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
635 while (cloud_iterator.
isValid ())
640 cloud_iterator.
reset ();
646 while (cloud_iterator.
isValid ())
648 cloud_out[i].x = cloud_iterator->x - centroid[0];
649 cloud_out[i].y = cloud_iterator->y - centroid[1];
650 cloud_out[i].z = cloud_iterator->z - centroid[2];
659 template <
typename Po
intT,
typename Scalar>
void
661 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
664 cloud_out = cloud_in;
667 for (
auto& point: cloud_out)
669 point.x -=
static_cast<float> (centroid[0]);
670 point.y -=
static_cast<float> (centroid[1]);
671 point.z -=
static_cast<float> (centroid[2]);
676 template <
typename Po
intT,
typename Scalar>
void
679 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
684 if (indices.size () == cloud_in.
size ())
691 cloud_out.
width = indices.size ();
694 cloud_out.
resize (indices.size ());
697 for (std::size_t i = 0; i < indices.size (); ++i)
699 cloud_out[i].x =
static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
700 cloud_out[i].y =
static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
701 cloud_out[i].z =
static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
706 template <
typename Po
intT,
typename Scalar>
void
709 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
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,
725 while (cloud_iterator.
isValid ())
730 cloud_iterator.
reset ();
733 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
736 while (cloud_iterator.
isValid ())
738 cloud_out (0, i) = cloud_iterator->x - centroid[0];
739 cloud_out (1, i) = cloud_iterator->y - centroid[1];
740 cloud_out (2, i) = cloud_iterator->z - centroid[2];
747 template <
typename Po
intT,
typename Scalar>
void
749 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
750 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
752 std::size_t npts = cloud_in.
size ();
754 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
756 for (std::size_t i = 0; i < npts; ++i)
758 cloud_out (0, i) = cloud_in[i].x - centroid[0];
759 cloud_out (1, i) = cloud_in[i].y - centroid[1];
760 cloud_out (2, i) = cloud_in[i].z - centroid[2];
770 template <
typename Po
intT,
typename Scalar>
void
773 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
774 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
776 std::size_t npts = indices.size ();
778 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
780 for (std::size_t i = 0; i < npts; ++i)
782 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
783 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
784 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
794 template <
typename Po
intT,
typename Scalar>
void
797 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
798 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
804 template <
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 ());
826 template <
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 ());
849 template <
typename Po
intT,
typename Scalar>
inline void
852 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
857 template <
typename Po
intT>
void
865 template <
typename Po
intT>
866 template <
typename Po
intOutT>
void
869 if (num_points_ != 0)
873 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
880 template <
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 ());
899 template <
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.
A generic class that computes the centroid of points fed to it.
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
Helper functor structure for n-D centroid estimation.
A point structure representing Euclidean xyz coordinates, and the RGB color.