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 (
size_t i = 0; i < cloud.
size (); ++i)
93 centroid[0] += cloud[i].x;
94 centroid[1] += cloud[i].y;
95 centroid[2] += cloud[i].z;
97 centroid /= static_cast<Scalar> (cloud.
size ());
100 return (static_cast<unsigned int> (cloud.
size ()));
106 for (
size_t i = 0; i < cloud.
size (); ++i)
112 centroid[0] += cloud[i].x;
113 centroid[1] += cloud[i].y;
114 centroid[2] += cloud[i].z;
117 centroid /= static_cast<Scalar> (cp);
125 template <
typename Po
intT,
typename Scalar>
inline unsigned int 127 const std::vector<int> &indices,
128 Eigen::Matrix<Scalar, 4, 1> ¢roid)
130 if (indices.empty ())
138 for (
size_t i = 0; i < indices.size (); ++i)
140 centroid[0] += cloud[indices[i]].x;
141 centroid[1] += cloud[indices[i]].y;
142 centroid[2] += cloud[indices[i]].z;
144 centroid /= static_cast<Scalar> (indices.size ());
146 return (static_cast<unsigned int> (indices.size ()));
152 for (
size_t i = 0; i < indices.size (); ++i)
158 centroid[0] += cloud[indices[i]].x;
159 centroid[1] += cloud[indices[i]].y;
160 centroid[2] += cloud[indices[i]].z;
163 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 (
size_t i = 0; i < point_count; ++i)
198 Eigen::Matrix<Scalar, 4, 1> pt;
199 pt[0] = cloud[i].x - centroid[0];
200 pt[1] = cloud[i].y - centroid[1];
201 pt[2] = cloud[i].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 (
size_t i = 0; i < cloud.
size (); ++i)
222 if (!isFinite (cloud [i]))
225 Eigen::Matrix<Scalar, 4, 1> pt;
226 pt[0] = cloud[i].x - centroid[0];
227 pt[1] = cloud[i].y - centroid[1];
228 pt[2] = cloud[i].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 264 const std::vector<int> &indices,
265 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
266 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
268 if (indices.empty ())
272 covariance_matrix.setZero ();
278 point_count = indices.size ();
280 for (
size_t i = 0; i < point_count; ++i)
282 Eigen::Matrix<Scalar, 4, 1> pt;
283 pt[0] = cloud[indices[i]].x - centroid[0];
284 pt[1] = cloud[indices[i]].y - centroid[1];
285 pt[2] = cloud[indices[i]].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 (
size_t i = 0; i < indices.size (); ++i)
309 Eigen::Matrix<Scalar, 4, 1> pt;
310 pt[0] = cloud[indices[i]].x - centroid[0];
311 pt[1] = cloud[indices[i]].y - centroid[1];
312 pt[2] = cloud[indices[i]].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 345 const std::vector<int> &indices,
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)
364 if (point_count != 0)
365 covariance_matrix /= static_cast<Scalar> (point_count);
371 template <
typename Po
intT,
typename Scalar>
inline unsigned int 373 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
376 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
378 unsigned int point_count;
381 point_count = static_cast<unsigned int> (cloud.
size ());
383 for (
size_t i = 0; i < point_count; ++i)
385 accu [0] += cloud[i].x * cloud[i].x;
386 accu [1] += cloud[i].x * cloud[i].y;
387 accu [2] += cloud[i].x * cloud[i].z;
388 accu [3] += cloud[i].y * cloud[i].y;
389 accu [4] += cloud[i].y * cloud[i].z;
390 accu [5] += cloud[i].z * cloud[i].z;
396 for (
size_t i = 0; i < cloud.
size (); ++i)
401 accu [0] += cloud[i].x * cloud[i].x;
402 accu [1] += cloud[i].x * cloud[i].y;
403 accu [2] += cloud[i].x * cloud[i].z;
404 accu [3] += cloud[i].y * cloud[i].y;
405 accu [4] += cloud[i].y * cloud[i].z;
406 accu [5] += cloud[i].z * cloud[i].z;
411 if (point_count != 0)
413 accu /= static_cast<Scalar> (point_count);
414 covariance_matrix.coeffRef (0) = accu [0];
415 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
416 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
417 covariance_matrix.coeffRef (4) = accu [3];
418 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
419 covariance_matrix.coeffRef (8) = accu [5];
421 return (point_count);
425 template <
typename Po
intT,
typename Scalar>
inline unsigned int 427 const std::vector<int> &indices,
428 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
431 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
433 unsigned int point_count;
436 point_count = static_cast<unsigned int> (indices.size ());
437 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
440 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
441 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
442 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
443 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
444 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
445 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
451 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
457 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
458 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
459 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
460 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
461 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
462 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
465 if (point_count != 0)
467 accu /= static_cast<Scalar> (point_count);
468 covariance_matrix.coeffRef (0) = accu [0];
469 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
470 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
471 covariance_matrix.coeffRef (4) = accu [3];
472 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
473 covariance_matrix.coeffRef (8) = accu [5];
475 return (point_count);
479 template <
typename Po
intT,
typename Scalar>
inline unsigned int 482 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
488 template <
typename Po
intT,
typename Scalar>
inline unsigned int 490 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
491 Eigen::Matrix<Scalar, 4, 1> ¢roid)
494 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
498 point_count = cloud.
size ();
500 for (
size_t i = 0; i < point_count; ++i)
502 accu [0] += cloud[i].x * cloud[i].x;
503 accu [1] += cloud[i].x * cloud[i].y;
504 accu [2] += cloud[i].x * cloud[i].z;
505 accu [3] += cloud[i].y * cloud[i].y;
506 accu [4] += cloud[i].y * cloud[i].z;
507 accu [5] += cloud[i].z * cloud[i].z;
508 accu [6] += cloud[i].x;
509 accu [7] += cloud[i].y;
510 accu [8] += cloud[i].z;
516 for (
size_t i = 0; i < cloud.
size (); ++i)
521 accu [0] += cloud[i].x * cloud[i].x;
522 accu [1] += cloud[i].x * cloud[i].y;
523 accu [2] += cloud[i].x * cloud[i].z;
524 accu [3] += cloud[i].y * cloud[i].y;
525 accu [4] += cloud[i].y * cloud[i].z;
526 accu [5] += cloud[i].z * cloud[i].z;
527 accu [6] += cloud[i].x;
528 accu [7] += cloud[i].y;
529 accu [8] += cloud[i].z;
533 accu /= static_cast<Scalar> (point_count);
534 if (point_count != 0)
537 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
539 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
540 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
541 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
542 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
543 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
544 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
545 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
546 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
547 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
549 return (static_cast<unsigned int> (point_count));
553 template <
typename Po
intT,
typename Scalar>
inline unsigned int 555 const std::vector<int> &indices,
556 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
557 Eigen::Matrix<Scalar, 4, 1> ¢roid)
560 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
564 point_count = indices.size ();
565 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
568 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
569 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
570 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
571 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
572 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
573 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
574 accu [6] += cloud[*iIt].x;
575 accu [7] += cloud[*iIt].y;
576 accu [8] += cloud[*iIt].z;
582 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
588 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
589 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
590 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
591 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
592 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
593 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
594 accu [6] += cloud[*iIt].x;
595 accu [7] += cloud[*iIt].y;
596 accu [8] += cloud[*iIt].z;
600 accu /= static_cast<Scalar> (point_count);
604 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
606 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
607 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
608 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
609 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
610 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
611 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
612 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
613 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
614 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
616 return (static_cast<unsigned int> (point_count));
620 template <
typename Po
intT,
typename Scalar>
inline unsigned int 623 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
624 Eigen::Matrix<Scalar, 4, 1> ¢roid)
630 template <
typename Po
intT,
typename Scalar>
void 632 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
639 while (cloud_iterator.
isValid ())
644 cloud_iterator.
reset ();
650 while (cloud_iterator.
isValid ())
652 cloud_out[i].x = cloud_iterator->x - centroid[0];
653 cloud_out[i].y = cloud_iterator->y - centroid[1];
654 cloud_out[i].z = cloud_iterator->z - centroid[2];
663 template <
typename Po
intT,
typename Scalar>
void 665 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
668 cloud_out = cloud_in;
671 for (
size_t i = 0; i < cloud_in.
points.size (); ++i)
673 cloud_out[i].x -= static_cast<float> (centroid[0]);
674 cloud_out[i].y -= static_cast<float> (centroid[1]);
675 cloud_out[i].z -= static_cast<float> (centroid[2]);
680 template <
typename Po
intT,
typename Scalar>
void 682 const std::vector<int> &indices,
683 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
688 if (indices.size () == cloud_in.
points.size ())
695 cloud_out.
width = static_cast<uint32_t> (indices.size ());
698 cloud_out.
resize (indices.size ());
701 for (
size_t i = 0; i < indices.size (); ++i)
703 cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
704 cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
705 cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
710 template <
typename Po
intT,
typename Scalar>
void 713 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
720 template <
typename Po
intT,
typename Scalar>
void 722 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
723 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
729 while (cloud_iterator.
isValid ())
734 cloud_iterator.
reset ();
737 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
740 while (cloud_iterator.
isValid ())
742 cloud_out (0, i) = cloud_iterator->x - centroid[0];
743 cloud_out (1, i) = cloud_iterator->y - centroid[1];
744 cloud_out (2, i) = cloud_iterator->z - centroid[2];
751 template <
typename Po
intT,
typename Scalar>
void 753 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
754 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
756 size_t npts = cloud_in.
size ();
758 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
760 for (
size_t i = 0; i < npts; ++i)
762 cloud_out (0, i) = cloud_in[i].x - centroid[0];
763 cloud_out (1, i) = cloud_in[i].y - centroid[1];
764 cloud_out (2, i) = cloud_in[i].z - centroid[2];
774 template <
typename Po
intT,
typename Scalar>
void 776 const std::vector<int> &indices,
777 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
778 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
780 size_t npts = indices.size ();
782 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts);
784 for (
size_t i = 0; i < npts; ++i)
786 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
787 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
788 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
798 template <
typename Po
intT,
typename Scalar>
void 801 const Eigen::Matrix<Scalar, 4, 1> ¢roid,
802 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
808 template <
typename Po
intT,
typename Scalar>
inline void 810 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
815 centroid.setZero (boost::mpl::size<FieldList>::value);
820 int size = static_cast<int> (cloud.
size ());
821 for (
int i = 0; i < size; ++i)
826 centroid /= static_cast<Scalar> (size);
830 template <
typename Po
intT,
typename Scalar>
inline void 832 const std::vector<int> &indices,
833 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
838 centroid.setZero (boost::mpl::size<FieldList>::value);
840 if (indices.empty ())
843 int nr_points = static_cast<int> (indices.size ());
844 for (
int i = 0; i < nr_points; ++i)
849 centroid /= static_cast<Scalar> (nr_points);
853 template <
typename Po
intT,
typename Scalar>
inline void 856 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid)
862 template <
typename Po
intInT,
typename Po
intOutT>
size_t 869 for (
size_t i = 0; i < cloud.
size (); ++i)
872 for (
size_t i = 0; i < cloud.
size (); ++i)
877 return (cp.getSize ());
881 template <
typename Po
intInT,
typename Po
intOutT>
size_t 883 const std::vector<int>& indices,
889 for (
size_t i = 0; i < indices.size (); ++i)
890 cp.add (cloud[indices[i]]);
892 for (
size_t i = 0; i < indices.size (); ++i)
894 cp.add (cloud[indices[i]]);
897 return (cp.getSize ());
900 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_ 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...
Iterator class for point clouds with or without given indices.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT ¢roid)
Compute the centroid of a set of points and return it as a point.
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...
std::vector< int > indices
Helper functor structure for n-D centroid estimation.
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (if organized as an image-structure).
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 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.
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
pcl::PCLHeader header
The point cloud header.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void resize(size_t n)
Resize the cloud.
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 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.
A generic class that computes the centroid of points fed to it.
Define methods for centroid estimation and covariance matrix calculus.