43 #include <pcl/common/centroid.h>
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 ())
695 cloud_out.
width =
static_cast<std::uint32_t
> (indices.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)
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]);
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...
Iterator class for point clouds with or without given indices.
std::size_t getSize() const
Get the total number of points that were added.
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.
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...
std::uint32_t width
The point cloud width (if organized as an image-structure).
Helper functor structure for n-D centroid estimation.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::uint32_t height
The point cloud height (if organized as an image-structure).
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.
void get(PointOutT &point) const
Retrieve the current centroid.
void resize(std::size_t n)
Resize the cloud.
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 is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
A generic class that computes the centroid of points fed to it.
pcl::PCLHeader header
The point cloud header.