41 #ifndef PCL_COMMON_IMPL_CENTROID_H_
42 #define PCL_COMMON_IMPL_CENTROID_H_
45 #include <pcl/conversions.h>
46 #include <boost/mpl/size.hpp>
49 template <
typename Po
intT,
typename Scalar>
inline unsigned int
51 Eigen::Matrix<Scalar, 4, 1> ¢roid)
60 while (cloud_iterator.
isValid ())
65 centroid[0] += cloud_iterator->x;
66 centroid[1] += cloud_iterator->y;
67 centroid[2] += cloud_iterator->z;
72 centroid /= static_cast<Scalar> (cp);
78 template <
typename Po
intT,
typename Scalar>
inline unsigned int
80 Eigen::Matrix<Scalar, 4, 1> ¢roid)
91 for (
const auto& point: cloud)
93 centroid[0] += point.x;
94 centroid[1] += point.y;
95 centroid[2] += point.z;
97 centroid /= static_cast<Scalar> (cloud.size ());
100 return (static_cast<unsigned int> (cloud.size ()));
104 for (
const auto& point: cloud)
110 centroid[0] += point.x;
111 centroid[1] += point.y;
112 centroid[2] += point.z;
115 centroid /= static_cast<Scalar> (cp);
122 template <
typename Po
intT,
typename Scalar>
inline unsigned int
124 const std::vector<int> &indices,
125 Eigen::Matrix<Scalar, 4, 1> ¢roid)
127 if (indices.empty ())
135 for (
const int& index : indices)
137 centroid[0] += cloud[index].x;
138 centroid[1] += cloud[index].y;
139 centroid[2] += cloud[index].z;
141 centroid /= static_cast<Scalar> (indices.size ());
143 return (static_cast<unsigned int> (indices.size ()));
147 for (
const int& index : indices)
153 centroid[0] += cloud[index].x;
154 centroid[1] += cloud[index].y;
155 centroid[2] += cloud[index].z;
158 centroid /= static_cast<Scalar> (cp);
164 template <
typename Po
intT,
typename Scalar>
inline unsigned int
167 Eigen::Matrix<Scalar, 4, 1> ¢roid)
173 template <
typename Po
intT,
typename Scalar>
inline unsigned
175 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
176 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
182 covariance_matrix.setZero ();
184 unsigned point_count;
188 point_count = static_cast<unsigned> (cloud.
size ());
190 for (
const auto& point: cloud)
192 Eigen::Matrix<Scalar, 4, 1> pt;
193 pt[0] = point.x - centroid[0];
194 pt[1] = point.y - centroid[1];
195 pt[2] = point.z - centroid[2];
197 covariance_matrix (1, 1) += pt.y () * pt.y ();
198 covariance_matrix (1, 2) += pt.y () * pt.z ();
200 covariance_matrix (2, 2) += pt.z () * pt.z ();
203 covariance_matrix (0, 0) += pt.x ();
204 covariance_matrix (0, 1) += pt.y ();
205 covariance_matrix (0, 2) += pt.z ();
213 for (
const auto& point: cloud)
216 if (!isFinite (point))
219 Eigen::Matrix<Scalar, 4, 1> pt;
220 pt[0] = point.x - centroid[0];
221 pt[1] = point.y - centroid[1];
222 pt[2] = point.z - centroid[2];
224 covariance_matrix (1, 1) += pt.y () * pt.y ();
225 covariance_matrix (1, 2) += pt.y () * pt.z ();
227 covariance_matrix (2, 2) += pt.z () * pt.z ();
230 covariance_matrix (0, 0) += pt.x ();
231 covariance_matrix (0, 1) += pt.y ();
232 covariance_matrix (0, 2) += pt.z ();
236 covariance_matrix (1, 0) = covariance_matrix (0, 1);
237 covariance_matrix (2, 0) = covariance_matrix (0, 2);
238 covariance_matrix (2, 1) = covariance_matrix (1, 2);
240 return (point_count);
244 template <
typename Po
intT,
typename Scalar>
inline unsigned int
246 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
247 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
250 if (point_count != 0)
251 covariance_matrix /= static_cast<Scalar> (point_count);
252 return (point_count);
256 template <
typename Po
intT,
typename Scalar>
inline unsigned int
258 const std::vector<int> &indices,
259 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
260 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
262 if (indices.empty ())
266 covariance_matrix.setZero ();
268 std::size_t point_count;
272 point_count = indices.size ();
274 for (
const auto& idx: indices)
276 Eigen::Matrix<Scalar, 4, 1> pt;
277 pt[0] = cloud[idx].x - centroid[0];
278 pt[1] = cloud[idx].y - centroid[1];
279 pt[2] = cloud[idx].z - centroid[2];
281 covariance_matrix (1, 1) += pt.y () * pt.y ();
282 covariance_matrix (1, 2) += pt.y () * pt.z ();
284 covariance_matrix (2, 2) += pt.z () * pt.z ();
287 covariance_matrix (0, 0) += pt.x ();
288 covariance_matrix (0, 1) += pt.y ();
289 covariance_matrix (0, 2) += pt.z ();
297 for (
const int &index : indices)
303 Eigen::Matrix<Scalar, 4, 1> pt;
304 pt[0] = cloud[index].x - centroid[0];
305 pt[1] = cloud[index].y - centroid[1];
306 pt[2] = cloud[index].z - centroid[2];
308 covariance_matrix (1, 1) += pt.y () * pt.y ();
309 covariance_matrix (1, 2) += pt.y () * pt.z ();
311 covariance_matrix (2, 2) += pt.z () * pt.z ();
314 covariance_matrix (0, 0) += pt.x ();
315 covariance_matrix (0, 1) += pt.y ();
316 covariance_matrix (0, 2) += pt.z ();
320 covariance_matrix (1, 0) = covariance_matrix (0, 1);
321 covariance_matrix (2, 0) = covariance_matrix (0, 2);
322 covariance_matrix (2, 1) = covariance_matrix (1, 2);
323 return (static_cast<unsigned int> (point_count));
327 template <
typename Po
intT,
typename Scalar>
inline unsigned int
330 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
331 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
337 template <
typename Po
intT,
typename Scalar>
inline unsigned int
339 const std::vector<int> &indices,
340 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
341 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
344 if (point_count != 0)
345 covariance_matrix /= static_cast<Scalar> (point_count);
347 return (point_count);
351 template <
typename Po
intT,
typename Scalar>
inline unsigned int
354 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
355 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
358 if (point_count != 0)
359 covariance_matrix /= static_cast<Scalar> (point_count);
365 template <
typename Po
intT,
typename Scalar>
inline unsigned int
367 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
370 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
372 unsigned int point_count;
375 point_count = static_cast<unsigned int> (cloud.
size ());
377 for (
const auto& point: cloud)
379 accu [0] += point.x * point.x;
380 accu [1] += point.x * point.y;
381 accu [2] += point.x * point.z;
382 accu [3] += point.y * point.y;
383 accu [4] += point.y * point.z;
384 accu [5] += point.z * point.z;
390 for (
const auto& point: cloud)
395 accu [0] += point.x * point.x;
396 accu [1] += point.x * point.y;
397 accu [2] += point.x * point.z;
398 accu [3] += point.y * point.y;
399 accu [4] += point.y * point.z;
400 accu [5] += point.z * point.z;
405 if (point_count != 0)
407 accu /= static_cast<Scalar> (point_count);
408 covariance_matrix.coeffRef (0) = accu [0];
409 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
410 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
411 covariance_matrix.coeffRef (4) = accu [3];
412 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
413 covariance_matrix.coeffRef (8) = accu [5];
415 return (point_count);
419 template <
typename Po
intT,
typename Scalar>
inline unsigned int
421 const std::vector<int> &indices,
422 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
425 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
427 unsigned int point_count;
430 point_count = static_cast<unsigned int> (indices.size ());
431 for (
const int &index : indices)
434 accu [0] += cloud[index].x * cloud[index].x;
435 accu [1] += cloud[index].x * cloud[index].y;
436 accu [2] += cloud[index].x * cloud[index].z;
437 accu [3] += cloud[index].y * cloud[index].y;
438 accu [4] += cloud[index].y * cloud[index].z;
439 accu [5] += cloud[index].z * cloud[index].z;
445 for (
const int &index : indices)
451 accu [0] += cloud[index].x * cloud[index].x;
452 accu [1] += cloud[index].x * cloud[index].y;
453 accu [2] += cloud[index].x * cloud[index].z;
454 accu [3] += cloud[index].y * cloud[index].y;
455 accu [4] += cloud[index].y * cloud[index].z;
456 accu [5] += cloud[index].z * cloud[index].z;
459 if (point_count != 0)
461 accu /= static_cast<Scalar> (point_count);
462 covariance_matrix.coeffRef (0) = accu [0];
463 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
464 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
465 covariance_matrix.coeffRef (4) = accu [3];
466 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
467 covariance_matrix.coeffRef (8) = accu [5];
469 return (point_count);
473 template <
typename Po
intT,
typename Scalar>
inline unsigned int
476 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
482 template <
typename Po
intT,
typename Scalar>
inline unsigned int
484 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
485 Eigen::Matrix<Scalar, 4, 1> ¢roid)
488 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
489 std::size_t point_count;
492 point_count = cloud.
size ();
494 for (
const auto& point: cloud)
496 accu [0] += point.x * point.x;
497 accu [1] += point.x * point.y;
498 accu [2] += point.x * point.z;
499 accu [3] += point.y * point.y;
500 accu [4] += point.y * point.z;
501 accu [5] += point.z * point.z;
510 for (
const auto& point: cloud)
515 accu [0] += point.x * point.x;
516 accu [1] += point.x * point.y;
517 accu [2] += point.x * point.z;
518 accu [3] += point.y * point.y;
519 accu [4] += point.y * point.z;
520 accu [5] += point.z * point.z;
527 accu /= static_cast<Scalar> (point_count);
528 if (point_count != 0)
531 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
533 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
534 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
535 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
536 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
537 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
538 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
539 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
540 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
541 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
543 return (static_cast<unsigned int> (point_count));
547 template <
typename Po
intT,
typename Scalar>
inline unsigned int
549 const std::vector<int> &indices,
550 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
551 Eigen::Matrix<Scalar, 4, 1> ¢roid)
554 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
555 std::size_t point_count;
558 point_count = indices.size ();
559 for (
const int &index : indices)
562 accu [0] += cloud[index].x * cloud[index].x;
563 accu [1] += cloud[index].x * cloud[index].y;
564 accu [2] += cloud[index].x * cloud[index].z;
565 accu [3] += cloud[index].y * cloud[index].y;
566 accu [4] += cloud[index].y * cloud[index].z;
567 accu [5] += cloud[index].z * cloud[index].z;
568 accu [6] += cloud[index].x;
569 accu [7] += cloud[index].y;
570 accu [8] += cloud[index].z;
576 for (
const int &index : indices)
582 accu [0] += cloud[index].x * cloud[index].x;
583 accu [1] += cloud[index].x * cloud[index].y;
584 accu [2] += cloud[index].x * cloud[index].z;
585 accu [3] += cloud[index].y * cloud[index].y;
586 accu [4] += cloud[index].y * cloud[index].z;
587 accu [5] += cloud[index].z * cloud[index].z;
588 accu [6] += cloud[index].x;
589 accu [7] += cloud[index].y;
590 accu [8] += cloud[index].z;
594 accu /= static_cast<Scalar> (point_count);
598 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
600 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
601 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
602 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
603 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
604 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
605 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
606 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
607 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
608 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
610 return (static_cast<unsigned int> (point_count));
614 template <
typename Po
intT,
typename Scalar>
inline unsigned int
617 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
618 Eigen::Matrix<Scalar, 4, 1> ¢roid)
624 template <
typename Po
intT,
typename Scalar>
void
626 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
633 while (cloud_iterator.
isValid ())
638 cloud_iterator.
reset ();
644 while (cloud_iterator.
isValid ())
646 cloud_out[i].x = cloud_iterator->x - centroid[0];
647 cloud_out[i].y = cloud_iterator->y - centroid[1];
648 cloud_out[i].z = cloud_iterator->z - centroid[2];
657 template <
typename Po
intT,
typename Scalar>
void
659 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
662 cloud_out = cloud_in;
665 for (
auto& point: cloud_out)
667 point.x -= static_cast<float> (centroid[0]);
668 point.y -= static_cast<float> (centroid[1]);
669 point.z -= static_cast<float> (centroid[2]);
674 template <
typename Po
intT,
typename Scalar>
void
676 const std::vector<int> &indices,
677 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
682 if (indices.size () == cloud_in.
points.size ())
689 cloud_out.
width = static_cast<std::uint32_t> (indices.size ());
692 cloud_out.
resize (indices.size ());
695 for (std::size_t i = 0; i < indices.size (); ++i)
697 cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
698 cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
699 cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
704 template <
typename Po
intT,
typename Scalar>
void
707 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
714 template <
typename Po
intT,
typename Scalar>
void
716 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
717 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
723 while (cloud_iterator.
isValid ())
728 cloud_iterator.
reset ();
731 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
734 while (cloud_iterator.
isValid ())
736 cloud_out (0, i) = cloud_iterator->x - centroid[0];
737 cloud_out (1, i) = cloud_iterator->y - centroid[1];
738 cloud_out (2, i) = cloud_iterator->z - centroid[2];
745 template <
typename Po
intT,
typename Scalar>
void
747 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
748 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
750 std::size_t npts = cloud_in.
size ();
752 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
754 for (std::size_t i = 0; i < npts; ++i)
756 cloud_out (0, i) = cloud_in[i].x - centroid[0];
757 cloud_out (1, i) = cloud_in[i].y - centroid[1];
758 cloud_out (2, i) = cloud_in[i].z - centroid[2];
768 template <
typename Po
intT,
typename Scalar>
void
770 const std::vector<int> &indices,
771 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
772 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
774 std::size_t npts = indices.size ();
776 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
778 for (std::size_t i = 0; i < npts; ++i)
780 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
781 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
782 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
792 template <
typename Po
intT,
typename Scalar>
void
795 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
796 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
802 template <
typename Po
intT,
typename Scalar>
inline void
804 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
809 centroid.setZero (boost::mpl::size<FieldList>::value);
815 for (
const auto& pt: cloud)
820 centroid /= static_cast<Scalar> (cloud.size ());
824 template <
typename Po
intT,
typename Scalar>
inline void
826 const std::vector<int> &indices,
827 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
832 centroid.setZero (boost::mpl::size<FieldList>::value);
834 if (indices.empty ())
838 for (
const auto& index: indices)
843 centroid /= static_cast<Scalar> (indices.size ());
847 template <
typename Po
intT,
typename Scalar>
inline void
850 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
855 template <
typename Po
intT>
void
863 template <
typename Po
intT>
864 template <
typename Po
intOutT>
void
867 if (num_points_ != 0)
871 auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
877 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
884 for (
const auto& point: cloud)
887 for (
const auto& point: cloud)
892 return (cp.getSize ());
895 template <
typename Po
intInT,
typename Po
intOutT> std::size_t
897 const std::vector<int>& indices,
903 for (
const int &index : indices)
904 cp.add (cloud[index]);
906 for (
const int &index : indices)
908 cp.add (cloud[index]);
911 return (cp.getSize ());
914 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_