Point Cloud Library (PCL)  1.12.1-dev
centroid.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-present, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/common/centroid.h>
44 #include <pcl/conversions.h>
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 
47 #include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
48 #include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
49 #include <boost/mpl/size.hpp> // for boost::mpl::size
50 
51 
52 namespace pcl
53 {
54 
55 template <typename PointT, typename Scalar> inline unsigned int
57  Eigen::Matrix<Scalar, 4, 1> &centroid)
58 {
59  Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
60 
61  unsigned int cp = 0;
62 
63  // For each point in the cloud
64  // If the data is dense, we don't need to check for NaN
65  while (cloud_iterator.isValid ())
66  {
67  // Check if the point is invalid
68  if (pcl::isFinite (*cloud_iterator))
69  {
70  accumulator[0] += cloud_iterator->x;
71  accumulator[1] += cloud_iterator->y;
72  accumulator[2] += cloud_iterator->z;
73  ++cp;
74  }
75  ++cloud_iterator;
76  }
77 
78  if (cp > 0) {
79  centroid = accumulator;
80  centroid /= static_cast<Scalar> (cp);
81  centroid[3] = 1;
82  }
83  return (cp);
84 }
85 
86 
87 template <typename PointT, typename Scalar> inline unsigned int
89  Eigen::Matrix<Scalar, 4, 1> &centroid)
90 {
91  if (cloud.empty ())
92  return (0);
93 
94  // For each point in the cloud
95  // If the data is dense, we don't need to check for NaN
96  if (cloud.is_dense)
97  {
98  // Initialize to 0
99  centroid.setZero ();
100  for (const auto& point: cloud)
101  {
102  centroid[0] += point.x;
103  centroid[1] += point.y;
104  centroid[2] += point.z;
105  }
106  centroid /= static_cast<Scalar> (cloud.size ());
107  centroid[3] = 1;
108 
109  return (static_cast<unsigned int> (cloud.size ()));
110  }
111  // NaN or Inf values could exist => check for them
112  unsigned int cp = 0;
113  Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
114  for (const auto& point: cloud)
115  {
116  // Check if the point is invalid
117  if (!isFinite (point))
118  continue;
119 
120  accumulator[0] += point.x;
121  accumulator[1] += point.y;
122  accumulator[2] += point.z;
123  ++cp;
124  }
125  if (cp > 0) {
126  centroid = accumulator;
127  centroid /= static_cast<Scalar> (cp);
128  centroid[3] = 1;
129  }
130 
131  return (cp);
132 }
133 
134 
135 template <typename PointT, typename Scalar> inline unsigned int
137  const Indices &indices,
138  Eigen::Matrix<Scalar, 4, 1> &centroid)
139 {
140  if (indices.empty ())
141  return (0);
142 
143  // If the data is dense, we don't need to check for NaN
144  if (cloud.is_dense)
145  {
146  // Initialize to 0
147  centroid.setZero ();
148  for (const auto& index : indices)
149  {
150  centroid[0] += cloud[index].x;
151  centroid[1] += cloud[index].y;
152  centroid[2] += cloud[index].z;
153  }
154  centroid /= static_cast<Scalar> (indices.size ());
155  centroid[3] = 1;
156  return (static_cast<unsigned int> (indices.size ()));
157  }
158  // NaN or Inf values could exist => check for them
159  Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
160  unsigned int cp = 0;
161  for (const auto& index : indices)
162  {
163  // Check if the point is invalid
164  if (!isFinite (cloud [index]))
165  continue;
166 
167  accumulator[0] += cloud[index].x;
168  accumulator[1] += cloud[index].y;
169  accumulator[2] += cloud[index].z;
170  ++cp;
171  }
172  if (cp > 0) {
173  centroid = accumulator;
174  centroid /= static_cast<Scalar> (cp);
175  centroid[3] = 1;
176  }
177  return (cp);
178 }
179 
180 
181 template <typename PointT, typename Scalar> inline unsigned int
183  const pcl::PointIndices &indices,
184  Eigen::Matrix<Scalar, 4, 1> &centroid)
185 {
186  return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
187 }
188 
189 
190 template <typename PointT, typename Scalar> inline unsigned
192  const Eigen::Matrix<Scalar, 4, 1> &centroid,
193  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
194 {
195  if (cloud.empty ())
196  return (0);
197 
198  unsigned point_count;
199  // If the data is dense, we don't need to check for NaN
200  if (cloud.is_dense)
201  {
202  covariance_matrix.setZero ();
203  point_count = static_cast<unsigned> (cloud.size ());
204  // For each point in the cloud
205  for (const auto& point: cloud)
206  {
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];
211 
212  covariance_matrix (1, 1) += pt.y () * pt.y ();
213  covariance_matrix (1, 2) += pt.y () * pt.z ();
214 
215  covariance_matrix (2, 2) += pt.z () * pt.z ();
216 
217  pt *= pt.x ();
218  covariance_matrix (0, 0) += pt.x ();
219  covariance_matrix (0, 1) += pt.y ();
220  covariance_matrix (0, 2) += pt.z ();
221  }
222  }
223  // NaN or Inf values could exist => check for them
224  else
225  {
226  Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
227  temp_covariance_matrix.setZero();
228  point_count = 0;
229  // For each point in the cloud
230  for (const auto& point: cloud)
231  {
232  // Check if the point is invalid
233  if (!isFinite (point))
234  continue;
235 
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];
240 
241  temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
242  temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
243 
244  temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
245 
246  pt *= pt.x ();
247  temp_covariance_matrix (0, 0) += pt.x ();
248  temp_covariance_matrix (0, 1) += pt.y ();
249  temp_covariance_matrix (0, 2) += pt.z ();
250  ++point_count;
251  }
252  if (point_count > 0) {
253  covariance_matrix = temp_covariance_matrix;
254  }
255  }
256  if (point_count == 0) {
257  return 0;
258  }
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);
262 
263  return (point_count);
264 }
265 
266 
267 template <typename PointT, typename Scalar> inline unsigned int
269  const Eigen::Matrix<Scalar, 4, 1> &centroid,
270  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
271 {
272  unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
273  if (point_count != 0)
274  covariance_matrix /= static_cast<Scalar> (point_count);
275  return (point_count);
276 }
277 
278 
279 template <typename PointT, typename Scalar> inline unsigned int
281  const Indices &indices,
282  const Eigen::Matrix<Scalar, 4, 1> &centroid,
283  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
284 {
285  if (indices.empty ())
286  return (0);
287 
288  std::size_t point_count;
289  // If the data is dense, we don't need to check for NaN
290  if (cloud.is_dense)
291  {
292  covariance_matrix.setZero ();
293  point_count = indices.size ();
294  // For each point in the cloud
295  for (const auto& idx: indices)
296  {
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];
301 
302  covariance_matrix (1, 1) += pt.y () * pt.y ();
303  covariance_matrix (1, 2) += pt.y () * pt.z ();
304 
305  covariance_matrix (2, 2) += pt.z () * pt.z ();
306 
307  pt *= pt.x ();
308  covariance_matrix (0, 0) += pt.x ();
309  covariance_matrix (0, 1) += pt.y ();
310  covariance_matrix (0, 2) += pt.z ();
311  }
312  }
313  // NaN or Inf values could exist => check for them
314  else
315  {
316  Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
317  temp_covariance_matrix.setZero ();
318  point_count = 0;
319  // For each point in the cloud
320  for (const auto &index : indices)
321  {
322  // Check if the point is invalid
323  if (!isFinite (cloud[index]))
324  continue;
325 
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];
330 
331  temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
332  temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
333 
334  temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
335 
336  pt *= pt.x ();
337  temp_covariance_matrix (0, 0) += pt.x ();
338  temp_covariance_matrix (0, 1) += pt.y ();
339  temp_covariance_matrix (0, 2) += pt.z ();
340  ++point_count;
341  }
342  if (point_count > 0) {
343  covariance_matrix = temp_covariance_matrix;
344  }
345  }
346  if (point_count == 0) {
347  return 0;
348  }
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));
353 }
354 
355 
356 template <typename PointT, typename Scalar> inline unsigned int
358  const pcl::PointIndices &indices,
359  const Eigen::Matrix<Scalar, 4, 1> &centroid,
360  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
361 {
362  return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
363 }
364 
365 
366 template <typename PointT, typename Scalar> inline unsigned int
368  const Indices &indices,
369  const Eigen::Matrix<Scalar, 4, 1> &centroid,
370  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
371 {
372  unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
373  if (point_count != 0)
374  covariance_matrix /= static_cast<Scalar> (point_count);
375 
376  return (point_count);
377 }
378 
379 
380 template <typename PointT, typename Scalar> inline unsigned int
382  const pcl::PointIndices &indices,
383  const Eigen::Matrix<Scalar, 4, 1> &centroid,
384  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
385 {
386  return computeCovarianceMatrixNormalized(cloud, indices.indices, centroid, covariance_matrix);
387 }
388 
389 
390 template <typename PointT, typename Scalar> inline unsigned int
392  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
393 {
394  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
395  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
396 
397  unsigned int point_count;
398  if (cloud.is_dense)
399  {
400  point_count = static_cast<unsigned int> (cloud.size ());
401  // For each point in the cloud
402  for (const auto& point: cloud)
403  {
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;
410  }
411  }
412  else
413  {
414  point_count = 0;
415  for (const auto& point: cloud)
416  {
417  if (!isFinite (point))
418  continue;
419 
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;
426  ++point_count;
427  }
428  }
429 
430  if (point_count != 0)
431  {
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];
439  }
440  return (point_count);
441 }
442 
443 
444 template <typename PointT, typename Scalar> inline unsigned int
446  const Indices &indices,
447  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
448 {
449  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
450  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
451 
452  unsigned int point_count;
453  if (cloud.is_dense)
454  {
455  point_count = static_cast<unsigned int> (indices.size ());
456  for (const auto &index : indices)
457  {
458  //const PointT& point = cloud[*iIt];
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;
465  }
466  }
467  else
468  {
469  point_count = 0;
470  for (const auto &index : indices)
471  {
472  if (!isFinite (cloud[index]))
473  continue;
474 
475  ++point_count;
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;
482  }
483  }
484  if (point_count != 0)
485  {
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];
493  }
494  return (point_count);
495 }
496 
497 
498 template <typename PointT, typename Scalar> inline unsigned int
500  const pcl::PointIndices &indices,
501  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
502 {
503  return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
504 }
505 
506 
507 template <typename PointT, typename Scalar> inline unsigned int
509  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
510  Eigen::Matrix<Scalar, 4, 1> &centroid)
511 {
512  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
513  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
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)
517  if(isFinite(point)) {
518  K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
519  }
520  std::size_t point_count;
521  if (cloud.is_dense)
522  {
523  point_count = cloud.size ();
524  // For each point in the cloud
525  for (const auto& point: cloud)
526  {
527  Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
528  accu [0] += x * x;
529  accu [1] += x * y;
530  accu [2] += x * z;
531  accu [3] += y * y;
532  accu [4] += y * z;
533  accu [5] += z * z;
534  accu [6] += x;
535  accu [7] += y;
536  accu [8] += z;
537  }
538  }
539  else
540  {
541  point_count = 0;
542  for (const auto& point: cloud)
543  {
544  if (!isFinite (point))
545  continue;
546 
547  Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
548  accu [0] += x * x;
549  accu [1] += x * y;
550  accu [2] += x * z;
551  accu [3] += y * y;
552  accu [4] += y * z;
553  accu [5] += z * z;
554  accu [6] += x;
555  accu [7] += y;
556  accu [8] += z;
557  ++point_count;
558  }
559  }
560  if (point_count != 0)
561  {
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();
564  centroid[3] = 1;
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);
574  }
575  return (static_cast<unsigned int> (point_count));
576 }
577 
578 
579 template <typename PointT, typename Scalar> inline unsigned int
581  const Indices &indices,
582  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
583  Eigen::Matrix<Scalar, 4, 1> &centroid)
584 {
585  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
586  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
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)
590  if(isFinite(cloud[index])) {
591  K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
592  }
593  std::size_t point_count;
594  if (cloud.is_dense)
595  {
596  point_count = indices.size ();
597  for (const auto &index : indices)
598  {
599  Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
600  accu [0] += x * x;
601  accu [1] += x * y;
602  accu [2] += x * z;
603  accu [3] += y * y;
604  accu [4] += y * z;
605  accu [5] += z * z;
606  accu [6] += x;
607  accu [7] += y;
608  accu [8] += z;
609  }
610  }
611  else
612  {
613  point_count = 0;
614  for (const auto &index : indices)
615  {
616  if (!isFinite (cloud[index]))
617  continue;
618 
619  ++point_count;
620  Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
621  accu [0] += x * x;
622  accu [1] += x * y;
623  accu [2] += x * z;
624  accu [3] += y * y;
625  accu [4] += y * z;
626  accu [5] += z * z;
627  accu [6] += x;
628  accu [7] += y;
629  accu [8] += z;
630  }
631  }
632 
633  if (point_count != 0)
634  {
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();
637  centroid[3] = 1;
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);
647  }
648  return (static_cast<unsigned int> (point_count));
649 }
650 
651 
652 template <typename PointT, typename Scalar> inline unsigned int
654  const pcl::PointIndices &indices,
655  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
656  Eigen::Matrix<Scalar, 4, 1> &centroid)
657 {
658  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
659 }
660 
661 
662 template <typename PointT, typename Scalar> void
664  const Eigen::Matrix<Scalar, 4, 1> &centroid,
665  pcl::PointCloud<PointT> &cloud_out,
666  int npts)
667 {
668  // Calculate the number of points if not given
669  if (npts == 0)
670  {
671  while (cloud_iterator.isValid ())
672  {
673  ++npts;
674  ++cloud_iterator;
675  }
676  cloud_iterator.reset ();
677  }
678 
679  int i = 0;
680  cloud_out.resize (npts);
681  // Subtract the centroid from cloud_in
682  while (cloud_iterator.isValid ())
683  {
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];
687  ++i;
688  ++cloud_iterator;
689  }
690  cloud_out.width = cloud_out.size ();
691  cloud_out.height = 1;
692 }
693 
694 
695 template <typename PointT, typename Scalar> void
697  const Eigen::Matrix<Scalar, 4, 1> &centroid,
698  pcl::PointCloud<PointT> &cloud_out)
699 {
700  cloud_out = cloud_in;
701 
702  // Subtract the centroid from cloud_in
703  for (auto& point: cloud_out)
704  {
705  point.x -= static_cast<float> (centroid[0]);
706  point.y -= static_cast<float> (centroid[1]);
707  point.z -= static_cast<float> (centroid[2]);
708  }
709 }
710 
711 
712 template <typename PointT, typename Scalar> void
714  const Indices &indices,
715  const Eigen::Matrix<Scalar, 4, 1> &centroid,
716  pcl::PointCloud<PointT> &cloud_out)
717 {
718  cloud_out.header = cloud_in.header;
719  cloud_out.is_dense = cloud_in.is_dense;
720  if (indices.size () == cloud_in.size ())
721  {
722  cloud_out.width = cloud_in.width;
723  cloud_out.height = cloud_in.height;
724  }
725  else
726  {
727  cloud_out.width = indices.size ();
728  cloud_out.height = 1;
729  }
730  cloud_out.resize (indices.size ());
731 
732  // Subtract the centroid from cloud_in
733  for (std::size_t i = 0; i < indices.size (); ++i)
734  {
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]);
738  }
739 }
740 
741 
742 template <typename PointT, typename Scalar> void
744  const pcl::PointIndices& indices,
745  const Eigen::Matrix<Scalar, 4, 1> &centroid,
746  pcl::PointCloud<PointT> &cloud_out)
747 {
748  return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
749 }
750 
751 
752 template <typename PointT, typename Scalar> void
754  const Eigen::Matrix<Scalar, 4, 1> &centroid,
755  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
756  int npts)
757 {
758  // Calculate the number of points if not given
759  if (npts == 0)
760  {
761  while (cloud_iterator.isValid ())
762  {
763  ++npts;
764  ++cloud_iterator;
765  }
766  cloud_iterator.reset ();
767  }
768 
769  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
770 
771  int i = 0;
772  while (cloud_iterator.isValid ())
773  {
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];
777  ++i;
778  ++cloud_iterator;
779  }
780 }
781 
782 
783 template <typename PointT, typename Scalar> void
785  const Eigen::Matrix<Scalar, 4, 1> &centroid,
786  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
787 {
788  std::size_t npts = cloud_in.size ();
789 
790  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
791 
792  for (std::size_t i = 0; i < npts; ++i)
793  {
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];
797  // One column at a time
798  //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
799  }
800 
801  // Make sure we zero the 4th dimension out (1 row, N columns)
802  //cloud_out.block (3, 0, 1, npts).setZero ();
803 }
804 
805 
806 template <typename PointT, typename Scalar> void
808  const Indices &indices,
809  const Eigen::Matrix<Scalar, 4, 1> &centroid,
810  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
811 {
812  std::size_t npts = indices.size ();
813 
814  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
815 
816  for (std::size_t i = 0; i < npts; ++i)
817  {
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];
821  // One column at a time
822  //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
823  }
824 
825  // Make sure we zero the 4th dimension out (1 row, N columns)
826  //cloud_out.block (3, 0, 1, npts).setZero ();
827 }
828 
829 
830 template <typename PointT, typename Scalar> void
832  const pcl::PointIndices &indices,
833  const Eigen::Matrix<Scalar, 4, 1> &centroid,
834  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
835 {
836  return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
837 }
838 
839 
840 template <typename PointT, typename Scalar> inline void
842  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
843 {
844  using FieldList = typename pcl::traits::fieldList<PointT>::type;
845 
846  // Get the size of the fields
847  centroid.setZero (boost::mpl::size<FieldList>::value);
848 
849  if (cloud.empty ())
850  return;
851 
852  // Iterate over each point
853  for (const auto& pt: cloud)
854  {
855  // Iterate over each dimension
856  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
857  }
858  centroid /= static_cast<Scalar> (cloud.size ());
859 }
860 
861 
862 template <typename PointT, typename Scalar> inline void
864  const Indices &indices,
865  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
866 {
867  using FieldList = typename pcl::traits::fieldList<PointT>::type;
868 
869  // Get the size of the fields
870  centroid.setZero (boost::mpl::size<FieldList>::value);
871 
872  if (indices.empty ())
873  return;
874 
875  // Iterate over each point
876  for (const auto& index: indices)
877  {
878  // Iterate over each dimension
879  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
880  }
881  centroid /= static_cast<Scalar> (indices.size ());
882 }
883 
884 
885 template <typename PointT, typename Scalar> inline void
887  const pcl::PointIndices &indices,
888  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
889 {
890  return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
891 }
892 
893 template <typename PointT> void
895 {
896  // Invoke add point on each accumulator
897  boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
898  ++num_points_;
899 }
900 
901 template <typename PointT>
902 template <typename PointOutT> void
903 CentroidPoint<PointT>::get (PointOutT& point) const
904 {
905  if (num_points_ != 0)
906  {
907  // Filter accumulators so that only those that are compatible with
908  // both PointT and requested point type remain
909  auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
910  // Invoke get point on each accumulator in filtered list
911  boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
912  }
913 }
914 
915 
916 template <typename PointInT, typename PointOutT> std::size_t
918  PointOutT& centroid)
919 {
921 
922  if (cloud.is_dense)
923  for (const auto& point: cloud)
924  cp.add (point);
925  else
926  for (const auto& point: cloud)
927  if (pcl::isFinite (point))
928  cp.add (point);
929 
930  cp.get (centroid);
931  return (cp.getSize ());
932 }
933 
934 
935 template <typename PointInT, typename PointOutT> std::size_t
937  const Indices& indices,
938  PointOutT& centroid)
939 {
941 
942  if (cloud.is_dense)
943  for (const auto &index : indices)
944  cp.add (cloud[index]);
945  else
946  for (const auto &index : indices)
947  if (pcl::isFinite (cloud[index]))
948  cp.add (cloud[index]);
949 
950  cp.get (centroid);
951  return (cp.getSize ());
952 }
953 
954 } // namespace pcl
955 
Define methods for centroid estimation and covariance matrix calculus.
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1023
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:903
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:894
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
std::size_t size() const
Definition: point_cloud.h:443
std::size_t computeCentroid(const pcl::PointCloud< PointInT > &cloud, PointOutT &centroid)
Compute the centroid of a set of points and return it as a point.
Definition: centroid.hpp:917
void computeNDCentroid(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > &centroid)
General, all purpose nD centroid estimation for a set of points using their indices.
Definition: centroid.hpp:841
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition: centroid.hpp:508
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition: centroid.hpp:663
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
Definition: centroid.hpp:268
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
Definition: centroid.hpp:191
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:56
@ K
Definition: norms.h:54
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...
Definition: point_tests.h:55
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Helper functor structure for n-D centroid estimation.
Definition: centroid.h:843
A point structure representing Euclidean xyz coordinates, and the RGB color.