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
130 const std::vector<int> &indices,
131 Eigen::Matrix<Scalar, 4, 1> ¢roid)
133 if (indices.empty ())
141 for (
const int& 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 int& 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
264 const std::vector<int> &indices,
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 int &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
345 const std::vector<int> &indices,
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)
364 if (point_count != 0)
365 covariance_matrix /=
static_cast<Scalar
> (point_count);
371 template <
typename Po
intT,
typename Scalar>
inline unsigned int
373 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
376 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
378 unsigned int point_count;
381 point_count =
static_cast<unsigned int> (cloud.
size ());
383 for (
const auto& point: cloud)
385 accu [0] += point.x * point.x;
386 accu [1] += point.x * point.y;
387 accu [2] += point.x * point.z;
388 accu [3] += point.y * point.y;
389 accu [4] += point.y * point.z;
390 accu [5] += point.z * point.z;
396 for (
const auto& point: cloud)
401 accu [0] += point.x * point.x;
402 accu [1] += point.x * point.y;
403 accu [2] += point.x * point.z;
404 accu [3] += point.y * point.y;
405 accu [4] += point.y * point.z;
406 accu [5] += point.z * point.z;
411 if (point_count != 0)
413 accu /=
static_cast<Scalar
> (point_count);
414 covariance_matrix.coeffRef (0) = accu [0];
415 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
416 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
417 covariance_matrix.coeffRef (4) = accu [3];
418 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
419 covariance_matrix.coeffRef (8) = accu [5];
421 return (point_count);
425 template <
typename Po
intT,
typename Scalar>
inline unsigned int
427 const std::vector<int> &indices,
428 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
431 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
433 unsigned int point_count;
436 point_count =
static_cast<unsigned int> (indices.size ());
437 for (
const int &index : indices)
440 accu [0] += cloud[index].x * cloud[index].x;
441 accu [1] += cloud[index].x * cloud[index].y;
442 accu [2] += cloud[index].x * cloud[index].z;
443 accu [3] += cloud[index].y * cloud[index].y;
444 accu [4] += cloud[index].y * cloud[index].z;
445 accu [5] += cloud[index].z * cloud[index].z;
451 for (
const int &index : indices)
457 accu [0] += cloud[index].x * cloud[index].x;
458 accu [1] += cloud[index].x * cloud[index].y;
459 accu [2] += cloud[index].x * cloud[index].z;
460 accu [3] += cloud[index].y * cloud[index].y;
461 accu [4] += cloud[index].y * cloud[index].z;
462 accu [5] += cloud[index].z * cloud[index].z;
465 if (point_count != 0)
467 accu /=
static_cast<Scalar
> (point_count);
468 covariance_matrix.coeffRef (0) = accu [0];
469 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
470 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
471 covariance_matrix.coeffRef (4) = accu [3];
472 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
473 covariance_matrix.coeffRef (8) = accu [5];
475 return (point_count);
479 template <
typename Po
intT,
typename Scalar>
inline unsigned int
482 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
488 template <
typename Po
intT,
typename Scalar>
inline unsigned int
490 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
491 Eigen::Matrix<Scalar, 4, 1> ¢roid)
494 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
495 std::size_t point_count;
498 point_count = cloud.
size ();
500 for (
const auto& point: cloud)
502 accu [0] += point.x * point.x;
503 accu [1] += point.x * point.y;
504 accu [2] += point.x * point.z;
505 accu [3] += point.y * point.y;
506 accu [4] += point.y * point.z;
507 accu [5] += point.z * point.z;
516 for (
const auto& point: cloud)
521 accu [0] += point.x * point.x;
522 accu [1] += point.x * point.y;
523 accu [2] += point.x * point.z;
524 accu [3] += point.y * point.y;
525 accu [4] += point.y * point.z;
526 accu [5] += point.z * point.z;
533 accu /=
static_cast<Scalar
> (point_count);
534 if (point_count != 0)
537 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
539 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
540 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
541 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
542 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
543 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
544 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
545 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
546 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
547 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
549 return (
static_cast<unsigned int> (point_count));
553 template <
typename Po
intT,
typename Scalar>
inline unsigned int
555 const std::vector<int> &indices,
556 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
557 Eigen::Matrix<Scalar, 4, 1> ¢roid)
560 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
561 std::size_t point_count;
564 point_count = indices.size ();
565 for (
const int &index : indices)
568 accu [0] += cloud[index].x * cloud[index].x;
569 accu [1] += cloud[index].x * cloud[index].y;
570 accu [2] += cloud[index].x * cloud[index].z;
571 accu [3] += cloud[index].y * cloud[index].y;
572 accu [4] += cloud[index].y * cloud[index].z;
573 accu [5] += cloud[index].z * cloud[index].z;
574 accu [6] += cloud[index].x;
575 accu [7] += cloud[index].y;
576 accu [8] += cloud[index].z;
582 for (
const int &index : indices)
588 accu [0] += cloud[index].x * cloud[index].x;
589 accu [1] += cloud[index].x * cloud[index].y;
590 accu [2] += cloud[index].x * cloud[index].z;
591 accu [3] += cloud[index].y * cloud[index].y;
592 accu [4] += cloud[index].y * cloud[index].z;
593 accu [5] += cloud[index].z * cloud[index].z;
594 accu [6] += cloud[index].x;
595 accu [7] += cloud[index].y;
596 accu [8] += cloud[index].z;
600 accu /=
static_cast<Scalar
> (point_count);
604 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
606 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
607 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
608 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
609 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
610 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
611 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
612 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
613 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
614 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
616 return (
static_cast<unsigned int> (point_count));
620 template <
typename Po
intT,
typename Scalar>
inline unsigned int
623 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
624 Eigen::Matrix<Scalar, 4, 1> ¢roid)
630 template <
typename Po
intT,
typename Scalar>
void
632 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
639 while (cloud_iterator.
isValid ())
644 cloud_iterator.
reset ();
650 while (cloud_iterator.
isValid ())
652 cloud_out[i].x = cloud_iterator->x - centroid[0];
653 cloud_out[i].y = cloud_iterator->y - centroid[1];
654 cloud_out[i].z = cloud_iterator->z - centroid[2];
663 template <
typename Po
intT,
typename Scalar>
void
665 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
668 cloud_out = cloud_in;
671 for (
auto& point: cloud_out)
673 point.x -=
static_cast<float> (centroid[0]);
674 point.y -=
static_cast<float> (centroid[1]);
675 point.z -=
static_cast<float> (centroid[2]);
680 template <
typename Po
intT,
typename Scalar>
void
682 const std::vector<int> &indices,
683 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
688 if (indices.size () == cloud_in.
points.size ())
698 cloud_out.
resize (indices.size ());
701 for (std::size_t i = 0; i < indices.size (); ++i)
703 cloud_out[i].x =
static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
704 cloud_out[i].y =
static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
705 cloud_out[i].z =
static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
710 template <
typename Po
intT,
typename Scalar>
void
713 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
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,
729 while (cloud_iterator.
isValid ())
734 cloud_iterator.
reset ();
737 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
740 while (cloud_iterator.
isValid ())
742 cloud_out (0, i) = cloud_iterator->x - centroid[0];
743 cloud_out (1, i) = cloud_iterator->y - centroid[1];
744 cloud_out (2, i) = cloud_iterator->z - centroid[2];
751 template <
typename Po
intT,
typename Scalar>
void
753 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
754 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
756 std::size_t npts = cloud_in.
size ();
758 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
760 for (std::size_t i = 0; i < npts; ++i)
762 cloud_out (0, i) = cloud_in[i].x - centroid[0];
763 cloud_out (1, i) = cloud_in[i].y - centroid[1];
764 cloud_out (2, i) = cloud_in[i].z - centroid[2];
774 template <
typename Po
intT,
typename Scalar>
void
776 const std::vector<int> &indices,
777 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
778 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
780 std::size_t npts = indices.size ();
782 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
784 for (std::size_t i = 0; i < npts; ++i)
786 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
787 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
788 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
798 template <
typename Po
intT,
typename Scalar>
void
801 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
802 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
808 template <
typename Po
intT,
typename Scalar>
inline void
810 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
815 centroid.setZero (boost::mpl::size<FieldList>::value);
821 for (
const auto& pt: cloud)
826 centroid /=
static_cast<Scalar
> (cloud.
size ());
830 template <
typename Po
intT,
typename Scalar>
inline void
832 const std::vector<int> &indices,
833 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
838 centroid.setZero (boost::mpl::size<FieldList>::value);
840 if (indices.empty ())
844 for (
const auto& index: indices)
849 centroid /=
static_cast<Scalar
> (indices.size ());
853 template <
typename Po
intT,
typename Scalar>
inline void
856 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
861 template <
typename Po
intT>
void
869 template <
typename Po
intT>
870 template <
typename Po
intOutT>
void
873 if (num_points_ != 0)
877 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
884 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
891 for (
const auto& point: cloud)
894 for (
const auto& point: cloud)
899 return (cp.getSize ());
903 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
905 const std::vector<int>& indices,
911 for (
const int &index : indices)
912 cp.add (cloud[index]);
914 for (
const int &index : indices)
916 cp.add (cloud[index]);
919 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).
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::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void resize(std::size_t n)
Resize the cloud.
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...
Helper functor structure for n-D centroid estimation.
A point structure representing Euclidean xyz coordinates, and the RGB color.