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 ());