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