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)
491 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
492 Eigen::Matrix<Scalar, 3, 1>
K(0.0, 0.0, 0.0);
493 for(
const auto& point: cloud)
495 K.x() = point.x;
K.y() = point.y;
K.z() = point.z;
break;
497 std::size_t point_count;
500 point_count = cloud.
size ();
502 for (
const auto& point: cloud)
504 Scalar x = point.x -
K.x(), y = point.y -
K.y(), z = point.z -
K.z();
519 for (
const auto& point: cloud)
524 Scalar x = point.x -
K.x(), y = point.y -
K.y(), z = point.z -
K.z();
537 if (point_count != 0)
539 accu /=
static_cast<Scalar
> (point_count);
540 centroid[0] = accu[6] +
K.x(); centroid[1] = accu[7] +
K.y(); centroid[2] = accu[8] +
K.z();
542 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
543 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
544 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
545 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
546 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
547 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
548 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
549 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
550 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
552 return (
static_cast<unsigned int> (point_count));
556 template <
typename Po
intT,
typename Scalar>
inline unsigned int
559 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
560 Eigen::Matrix<Scalar, 4, 1> ¢roid)
564 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
565 Eigen::Matrix<Scalar, 3, 1>
K(0.0, 0.0, 0.0);
566 for(
const auto& index : indices)
568 K.x() = cloud[index].x;
K.y() = cloud[index].y;
K.z() = cloud[index].z;
break;
570 std::size_t point_count;
573 point_count = indices.
size ();
574 for (
const auto &index : indices)
576 Scalar x = cloud[index].x -
K.x(), y = cloud[index].y -
K.y(), z = cloud[index].z -
K.z();
591 for (
const auto &index : indices)
597 Scalar x = cloud[index].x -
K.x(), y = cloud[index].y -
K.y(), z = cloud[index].z -
K.z();
610 if (point_count != 0)
612 accu /=
static_cast<Scalar
> (point_count);
613 centroid[0] = accu[6] +
K.x(); centroid[1] = accu[7] +
K.y(); centroid[2] = accu[8] +
K.z();
615 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
616 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
617 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
618 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
619 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
620 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
621 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
622 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
623 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
625 return (
static_cast<unsigned int> (point_count));
629 template <
typename Po
intT,
typename Scalar>
inline unsigned int
632 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
633 Eigen::Matrix<Scalar, 4, 1> ¢roid)
639 template <
typename Po
intT,
typename Scalar>
void
641 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
648 while (cloud_iterator.
isValid ())
653 cloud_iterator.
reset ();
659 while (cloud_iterator.
isValid ())
661 cloud_out[i].x = cloud_iterator->x - centroid[0];
662 cloud_out[i].y = cloud_iterator->y - centroid[1];
663 cloud_out[i].z = cloud_iterator->z - centroid[2];
672 template <
typename Po
intT,
typename Scalar>
void
674 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
677 cloud_out = cloud_in;
680 for (
auto& point: cloud_out)
682 point.x -=
static_cast<float> (centroid[0]);
683 point.y -=
static_cast<float> (centroid[1]);
684 point.z -=
static_cast<float> (centroid[2]);
689 template <
typename Po
intT,
typename Scalar>
void
692 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
697 if (indices.size () == cloud_in.
size ())
704 cloud_out.
width = indices.size ();
707 cloud_out.
resize (indices.size ());
710 for (std::size_t i = 0; i < indices.size (); ++i)
712 cloud_out[i].x =
static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
713 cloud_out[i].y =
static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
714 cloud_out[i].z =
static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
719 template <
typename Po
intT,
typename Scalar>
void
722 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
729 template <
typename Po
intT,
typename Scalar>
void
731 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
732 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
738 while (cloud_iterator.
isValid ())
743 cloud_iterator.
reset ();
746 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
749 while (cloud_iterator.
isValid ())
751 cloud_out (0, i) = cloud_iterator->x - centroid[0];
752 cloud_out (1, i) = cloud_iterator->y - centroid[1];
753 cloud_out (2, i) = cloud_iterator->z - centroid[2];
760 template <
typename Po
intT,
typename Scalar>
void
762 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
763 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
765 std::size_t npts = cloud_in.
size ();
767 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
769 for (std::size_t i = 0; i < npts; ++i)
771 cloud_out (0, i) = cloud_in[i].x - centroid[0];
772 cloud_out (1, i) = cloud_in[i].y - centroid[1];
773 cloud_out (2, i) = cloud_in[i].z - centroid[2];
783 template <
typename Po
intT,
typename Scalar>
void
786 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
787 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
789 std::size_t npts = indices.size ();
791 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
793 for (std::size_t i = 0; i < npts; ++i)
795 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
796 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
797 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
807 template <
typename Po
intT,
typename Scalar>
void
810 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
811 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
817 template <
typename Po
intT,
typename Scalar>
inline void
819 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
821 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
824 centroid.setZero (boost::mpl::size<FieldList>::value);
830 for (
const auto& pt: cloud)
835 centroid /=
static_cast<Scalar
> (cloud.
size ());
839 template <
typename Po
intT,
typename Scalar>
inline void
842 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
844 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
847 centroid.setZero (boost::mpl::size<FieldList>::value);
849 if (indices.empty ())
853 for (
const auto& index: indices)
858 centroid /=
static_cast<Scalar
> (indices.size ());
862 template <
typename Po
intT,
typename Scalar>
inline void
865 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
870 template <
typename Po
intT>
void
878 template <
typename Po
intT>
879 template <
typename Po
intOutT>
void
882 if (num_points_ != 0)
886 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
893 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
900 for (
const auto& point: cloud)
903 for (
const auto& point: cloud)
908 return (cp.getSize ());
912 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
920 for (
const auto &index : indices)
921 cp.add (cloud[index]);
923 for (
const auto &index : indices)
925 cp.add (cloud[index]);
928 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.