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)
59 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
65 while (cloud_iterator.
isValid ())
70 accumulator[0] += cloud_iterator->x;
71 accumulator[1] += cloud_iterator->y;
72 accumulator[2] += cloud_iterator->z;
79 centroid = accumulator;
80 centroid /=
static_cast<Scalar
> (cp);
87 template <
typename Po
intT,
typename Scalar>
inline unsigned int
89 Eigen::Matrix<Scalar, 4, 1> ¢roid)
100 for (
const auto& point: cloud)
102 centroid[0] += point.x;
103 centroid[1] += point.y;
104 centroid[2] += point.z;
106 centroid /=
static_cast<Scalar
> (cloud.size ());
109 return (
static_cast<unsigned int> (cloud.size ()));
113 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
114 for (
const auto& point: cloud)
120 accumulator[0] += point.x;
121 accumulator[1] += point.y;
122 accumulator[2] += point.z;
126 centroid = accumulator;
127 centroid /=
static_cast<Scalar
> (cp);
135 template <
typename Po
intT,
typename Scalar>
inline unsigned int
138 Eigen::Matrix<Scalar, 4, 1> ¢roid)
140 if (indices.empty ())
148 for (
const auto& index : indices)
150 centroid[0] += cloud[index].x;
151 centroid[1] += cloud[index].y;
152 centroid[2] += cloud[index].z;
154 centroid /=
static_cast<Scalar
> (indices.size ());
156 return (
static_cast<unsigned int> (indices.size ()));
159 Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
161 for (
const auto& index : indices)
167 accumulator[0] += cloud[index].x;
168 accumulator[1] += cloud[index].y;
169 accumulator[2] += cloud[index].z;
173 centroid = accumulator;
174 centroid /=
static_cast<Scalar
> (cp);
181 template <
typename Po
intT,
typename Scalar>
inline unsigned int
184 Eigen::Matrix<Scalar, 4, 1> ¢roid)
190 template <
typename Po
intT,
typename Scalar>
inline unsigned
192 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
193 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
198 unsigned point_count;
202 covariance_matrix.setZero ();
203 point_count =
static_cast<unsigned> (cloud.
size ());
205 for (
const auto& point: cloud)
207 Eigen::Matrix<Scalar, 4, 1> pt;
208 pt[0] = point.x - centroid[0];
209 pt[1] = point.y - centroid[1];
210 pt[2] = point.z - centroid[2];
212 covariance_matrix (1, 1) += pt.y () * pt.y ();
213 covariance_matrix (1, 2) += pt.y () * pt.z ();
215 covariance_matrix (2, 2) += pt.z () * pt.z ();
218 covariance_matrix (0, 0) += pt.x ();
219 covariance_matrix (0, 1) += pt.y ();
220 covariance_matrix (0, 2) += pt.z ();
226 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
227 temp_covariance_matrix.setZero();
230 for (
const auto& point: cloud)
236 Eigen::Matrix<Scalar, 4, 1> pt;
237 pt[0] = point.x - centroid[0];
238 pt[1] = point.y - centroid[1];
239 pt[2] = point.z - centroid[2];
241 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
242 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
244 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
247 temp_covariance_matrix (0, 0) += pt.x ();
248 temp_covariance_matrix (0, 1) += pt.y ();
249 temp_covariance_matrix (0, 2) += pt.z ();
252 if (point_count > 0) {
253 covariance_matrix = temp_covariance_matrix;
256 if (point_count == 0) {
259 covariance_matrix (1, 0) = covariance_matrix (0, 1);
260 covariance_matrix (2, 0) = covariance_matrix (0, 2);
261 covariance_matrix (2, 1) = covariance_matrix (1, 2);
263 return (point_count);
267 template <
typename Po
intT,
typename Scalar>
inline unsigned int
269 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
270 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
273 if (point_count != 0)
274 covariance_matrix /=
static_cast<Scalar
> (point_count);
275 return (point_count);
279 template <
typename Po
intT,
typename Scalar>
inline unsigned int
282 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
283 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
285 if (indices.empty ())
288 std::size_t point_count;
292 covariance_matrix.setZero ();
293 point_count = indices.size ();
295 for (
const auto& idx: indices)
297 Eigen::Matrix<Scalar, 4, 1> pt;
298 pt[0] = cloud[idx].x - centroid[0];
299 pt[1] = cloud[idx].y - centroid[1];
300 pt[2] = cloud[idx].z - centroid[2];
302 covariance_matrix (1, 1) += pt.y () * pt.y ();
303 covariance_matrix (1, 2) += pt.y () * pt.z ();
305 covariance_matrix (2, 2) += pt.z () * pt.z ();
308 covariance_matrix (0, 0) += pt.x ();
309 covariance_matrix (0, 1) += pt.y ();
310 covariance_matrix (0, 2) += pt.z ();
316 Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
317 temp_covariance_matrix.setZero ();
320 for (
const auto &index : indices)
326 Eigen::Matrix<Scalar, 4, 1> pt;
327 pt[0] = cloud[index].x - centroid[0];
328 pt[1] = cloud[index].y - centroid[1];
329 pt[2] = cloud[index].z - centroid[2];
331 temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
332 temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
334 temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
337 temp_covariance_matrix (0, 0) += pt.x ();
338 temp_covariance_matrix (0, 1) += pt.y ();
339 temp_covariance_matrix (0, 2) += pt.z ();
342 if (point_count > 0) {
343 covariance_matrix = temp_covariance_matrix;
346 if (point_count == 0) {
349 covariance_matrix (1, 0) = covariance_matrix (0, 1);
350 covariance_matrix (2, 0) = covariance_matrix (0, 2);
351 covariance_matrix (2, 1) = covariance_matrix (1, 2);
352 return (
static_cast<unsigned int> (point_count));
356 template <
typename Po
intT,
typename Scalar>
inline unsigned int
359 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
360 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
366 template <
typename Po
intT,
typename Scalar>
inline unsigned int
369 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
370 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
373 if (point_count != 0)
374 covariance_matrix /=
static_cast<Scalar
> (point_count);
376 return (point_count);
380 template <
typename Po
intT,
typename Scalar>
inline unsigned int
383 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
384 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
390 template <
typename Po
intT,
typename Scalar>
inline unsigned int
392 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
395 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
397 unsigned int point_count;
400 point_count =
static_cast<unsigned int> (cloud.
size ());
402 for (
const auto& point: cloud)
404 accu [0] += point.x * point.x;
405 accu [1] += point.x * point.y;
406 accu [2] += point.x * point.z;
407 accu [3] += point.y * point.y;
408 accu [4] += point.y * point.z;
409 accu [5] += point.z * point.z;
415 for (
const auto& point: cloud)
420 accu [0] += point.x * point.x;
421 accu [1] += point.x * point.y;
422 accu [2] += point.x * point.z;
423 accu [3] += point.y * point.y;
424 accu [4] += point.y * point.z;
425 accu [5] += point.z * point.z;
430 if (point_count != 0)
432 accu /=
static_cast<Scalar
> (point_count);
433 covariance_matrix.coeffRef (0) = accu [0];
434 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
435 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
436 covariance_matrix.coeffRef (4) = accu [3];
437 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
438 covariance_matrix.coeffRef (8) = accu [5];
440 return (point_count);
444 template <
typename Po
intT,
typename Scalar>
inline unsigned int
447 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
450 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
452 unsigned int point_count;
455 point_count =
static_cast<unsigned int> (indices.size ());
456 for (
const auto &index : indices)
459 accu [0] += cloud[index].x * cloud[index].x;
460 accu [1] += cloud[index].x * cloud[index].y;
461 accu [2] += cloud[index].x * cloud[index].z;
462 accu [3] += cloud[index].y * cloud[index].y;
463 accu [4] += cloud[index].y * cloud[index].z;
464 accu [5] += cloud[index].z * cloud[index].z;
470 for (
const auto &index : indices)
476 accu [0] += cloud[index].x * cloud[index].x;
477 accu [1] += cloud[index].x * cloud[index].y;
478 accu [2] += cloud[index].x * cloud[index].z;
479 accu [3] += cloud[index].y * cloud[index].y;
480 accu [4] += cloud[index].y * cloud[index].z;
481 accu [5] += cloud[index].z * cloud[index].z;
484 if (point_count != 0)
486 accu /=
static_cast<Scalar
> (point_count);
487 covariance_matrix.coeffRef (0) = accu [0];
488 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
489 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
490 covariance_matrix.coeffRef (4) = accu [3];
491 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
492 covariance_matrix.coeffRef (8) = accu [5];
494 return (point_count);
498 template <
typename Po
intT,
typename Scalar>
inline unsigned int
501 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
507 template <
typename Po
intT,
typename Scalar>
inline unsigned int
509 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
510 Eigen::Matrix<Scalar, 4, 1> ¢roid)
514 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
515 Eigen::Matrix<Scalar, 3, 1>
K(0.0, 0.0, 0.0);
516 for(
const auto& point: cloud)
518 K.x() = point.x;
K.y() = point.y;
K.z() = point.z;
break;
520 std::size_t point_count;
523 point_count = cloud.
size ();
525 for (
const auto& point: cloud)
527 Scalar x = point.x -
K.x(), y = point.y -
K.y(), z = point.z -
K.z();
542 for (
const auto& point: cloud)
547 Scalar x = point.x -
K.x(), y = point.y -
K.y(), z = point.z -
K.z();
560 if (point_count != 0)
562 accu /=
static_cast<Scalar
> (point_count);
563 centroid[0] = accu[6] +
K.x(); centroid[1] = accu[7] +
K.y(); centroid[2] = accu[8] +
K.z();
565 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
566 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
567 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
568 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
569 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
570 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
571 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
572 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
573 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
575 return (
static_cast<unsigned int> (point_count));
579 template <
typename Po
intT,
typename Scalar>
inline unsigned int
582 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
583 Eigen::Matrix<Scalar, 4, 1> ¢roid)
587 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
588 Eigen::Matrix<Scalar, 3, 1>
K(0.0, 0.0, 0.0);
589 for(
const auto& index : indices)
591 K.x() = cloud[index].x;
K.y() = cloud[index].y;
K.z() = cloud[index].z;
break;
593 std::size_t point_count;
596 point_count = indices.
size ();
597 for (
const auto &index : indices)
599 Scalar x = cloud[index].x -
K.x(), y = cloud[index].y -
K.y(), z = cloud[index].z -
K.z();
614 for (
const auto &index : indices)
620 Scalar x = cloud[index].x -
K.x(), y = cloud[index].y -
K.y(), z = cloud[index].z -
K.z();
633 if (point_count != 0)
635 accu /=
static_cast<Scalar
> (point_count);
636 centroid[0] = accu[6] +
K.x(); centroid[1] = accu[7] +
K.y(); centroid[2] = accu[8] +
K.z();
638 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
639 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
640 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
641 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
642 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
643 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
644 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
645 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
646 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
648 return (
static_cast<unsigned int> (point_count));
652 template <
typename Po
intT,
typename Scalar>
inline unsigned int
655 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
656 Eigen::Matrix<Scalar, 4, 1> ¢roid)
662 template <
typename Po
intT,
typename Scalar>
void
664 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
671 while (cloud_iterator.
isValid ())
676 cloud_iterator.
reset ();
682 while (cloud_iterator.
isValid ())
684 cloud_out[i].x = cloud_iterator->x - centroid[0];
685 cloud_out[i].y = cloud_iterator->y - centroid[1];
686 cloud_out[i].z = cloud_iterator->z - centroid[2];
695 template <
typename Po
intT,
typename Scalar>
void
697 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
700 cloud_out = cloud_in;
703 for (
auto& point: cloud_out)
705 point.x -=
static_cast<float> (centroid[0]);
706 point.y -=
static_cast<float> (centroid[1]);
707 point.z -=
static_cast<float> (centroid[2]);
712 template <
typename Po
intT,
typename Scalar>
void
715 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
720 if (indices.size () == cloud_in.
size ())
727 cloud_out.
width = indices.size ();
730 cloud_out.
resize (indices.size ());
733 for (std::size_t i = 0; i < indices.size (); ++i)
735 cloud_out[i].x =
static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
736 cloud_out[i].y =
static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
737 cloud_out[i].z =
static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
742 template <
typename Po
intT,
typename Scalar>
void
745 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
752 template <
typename Po
intT,
typename Scalar>
void
754 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
755 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
761 while (cloud_iterator.
isValid ())
766 cloud_iterator.
reset ();
769 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
772 while (cloud_iterator.
isValid ())
774 cloud_out (0, i) = cloud_iterator->x - centroid[0];
775 cloud_out (1, i) = cloud_iterator->y - centroid[1];
776 cloud_out (2, i) = cloud_iterator->z - centroid[2];
783 template <
typename Po
intT,
typename Scalar>
void
785 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
786 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
788 std::size_t npts = cloud_in.
size ();
790 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
792 for (std::size_t i = 0; i < npts; ++i)
794 cloud_out (0, i) = cloud_in[i].x - centroid[0];
795 cloud_out (1, i) = cloud_in[i].y - centroid[1];
796 cloud_out (2, i) = cloud_in[i].z - centroid[2];
806 template <
typename Po
intT,
typename Scalar>
void
809 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
810 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
812 std::size_t npts = indices.size ();
814 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
816 for (std::size_t i = 0; i < npts; ++i)
818 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
819 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
820 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
830 template <
typename Po
intT,
typename Scalar>
void
833 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
834 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
840 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);
853 for (
const auto& pt: cloud)
858 centroid /=
static_cast<Scalar
> (cloud.
size ());
862 template <
typename Po
intT,
typename Scalar>
inline void
865 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
867 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
870 centroid.setZero (boost::mpl::size<FieldList>::value);
872 if (indices.empty ())
876 for (
const auto& index: indices)
881 centroid /=
static_cast<Scalar
> (indices.size ());
885 template <
typename Po
intT,
typename Scalar>
inline void
888 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
893 template <
typename Po
intT>
void
901 template <
typename Po
intT>
902 template <
typename Po
intOutT>
void
905 if (num_points_ != 0)
909 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
916 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
923 for (
const auto& point: cloud)
926 for (
const auto& point: cloud)
931 return (cp.getSize ());
935 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
943 for (
const auto &index : indices)
944 cp.add (cloud[index]);
946 for (
const auto &index : indices)
948 cp.add (cloud[index]);
951 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.