Point Cloud Library (PCL)  1.13.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 #include <Eigen/Eigenvalues> // for EigenSolver
47 
48 #include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
49 #include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
50 #include <boost/mpl/size.hpp> // for boost::mpl::size
51 
52 
53 namespace pcl
54 {
55 
56 template <typename PointT, typename Scalar> inline unsigned int
57 compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
58  Eigen::Matrix<Scalar, 4, 1> &centroid)
59 {
60  Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
61 
62  unsigned int cp = 0;
63 
64  // For each point in the cloud
65  // If the data is dense, we don't need to check for NaN
66  while (cloud_iterator.isValid ())
67  {
68  // Check if the point is invalid
69  if (pcl::isFinite (*cloud_iterator))
70  {
71  accumulator[0] += cloud_iterator->x;
72  accumulator[1] += cloud_iterator->y;
73  accumulator[2] += cloud_iterator->z;
74  ++cp;
75  }
76  ++cloud_iterator;
77  }
78 
79  if (cp > 0) {
80  centroid = accumulator;
81  centroid /= static_cast<Scalar> (cp);
82  centroid[3] = 1;
83  }
84  return (cp);
85 }
86 
87 
88 template <typename PointT, typename Scalar> inline unsigned int
89 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
90  Eigen::Matrix<Scalar, 4, 1> &centroid)
91 {
92  if (cloud.empty ())
93  return (0);
94 
95  // For each point in the cloud
96  // If the data is dense, we don't need to check for NaN
97  if (cloud.is_dense)
98  {
99  // Initialize to 0
100  centroid.setZero ();
101  for (const auto& point: cloud)
102  {
103  centroid[0] += point.x;
104  centroid[1] += point.y;
105  centroid[2] += point.z;
106  }
107  centroid /= static_cast<Scalar> (cloud.size ());
108  centroid[3] = 1;
109 
110  return (static_cast<unsigned int> (cloud.size ()));
111  }
112  // NaN or Inf values could exist => check for them
113  unsigned int cp = 0;
114  Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
115  for (const auto& point: cloud)
116  {
117  // Check if the point is invalid
118  if (!isFinite (point))
119  continue;
120 
121  accumulator[0] += point.x;
122  accumulator[1] += point.y;
123  accumulator[2] += point.z;
124  ++cp;
125  }
126  if (cp > 0) {
127  centroid = accumulator;
128  centroid /= static_cast<Scalar> (cp);
129  centroid[3] = 1;
130  }
131 
132  return (cp);
133 }
134 
135 
136 template <typename PointT, typename Scalar> inline unsigned int
137 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
138  const Indices &indices,
139  Eigen::Matrix<Scalar, 4, 1> &centroid)
140 {
141  if (indices.empty ())
142  return (0);
143 
144  // If the data is dense, we don't need to check for NaN
145  if (cloud.is_dense)
146  {
147  // Initialize to 0
148  centroid.setZero ();
149  for (const auto& index : indices)
150  {
151  centroid[0] += cloud[index].x;
152  centroid[1] += cloud[index].y;
153  centroid[2] += cloud[index].z;
154  }
155  centroid /= static_cast<Scalar> (indices.size ());
156  centroid[3] = 1;
157  return (static_cast<unsigned int> (indices.size ()));
158  }
159  // NaN or Inf values could exist => check for them
160  Eigen::Matrix<Scalar, 4, 1> accumulator {0, 0, 0, 0};
161  unsigned int cp = 0;
162  for (const auto& index : indices)
163  {
164  // Check if the point is invalid
165  if (!isFinite (cloud [index]))
166  continue;
167 
168  accumulator[0] += cloud[index].x;
169  accumulator[1] += cloud[index].y;
170  accumulator[2] += cloud[index].z;
171  ++cp;
172  }
173  if (cp > 0) {
174  centroid = accumulator;
175  centroid /= static_cast<Scalar> (cp);
176  centroid[3] = 1;
177  }
178  return (cp);
179 }
180 
181 
182 template <typename PointT, typename Scalar> inline unsigned int
183 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
184  const pcl::PointIndices &indices,
185  Eigen::Matrix<Scalar, 4, 1> &centroid)
186 {
187  return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
188 }
189 
190 
191 template <typename PointT, typename Scalar> inline unsigned
192 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
193  const Eigen::Matrix<Scalar, 4, 1> &centroid,
194  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
195 {
196  if (cloud.empty ())
197  return (0);
198 
199  unsigned point_count;
200  // If the data is dense, we don't need to check for NaN
201  if (cloud.is_dense)
202  {
203  covariance_matrix.setZero ();
204  point_count = static_cast<unsigned> (cloud.size ());
205  // For each point in the cloud
206  for (const auto& point: cloud)
207  {
208  Eigen::Matrix<Scalar, 4, 1> pt;
209  pt[0] = point.x - centroid[0];
210  pt[1] = point.y - centroid[1];
211  pt[2] = point.z - centroid[2];
212 
213  covariance_matrix (1, 1) += pt.y () * pt.y ();
214  covariance_matrix (1, 2) += pt.y () * pt.z ();
215 
216  covariance_matrix (2, 2) += pt.z () * pt.z ();
217 
218  pt *= pt.x ();
219  covariance_matrix (0, 0) += pt.x ();
220  covariance_matrix (0, 1) += pt.y ();
221  covariance_matrix (0, 2) += pt.z ();
222  }
223  }
224  // NaN or Inf values could exist => check for them
225  else
226  {
227  Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
228  temp_covariance_matrix.setZero();
229  point_count = 0;
230  // For each point in the cloud
231  for (const auto& point: cloud)
232  {
233  // Check if the point is invalid
234  if (!isFinite (point))
235  continue;
236 
237  Eigen::Matrix<Scalar, 4, 1> pt;
238  pt[0] = point.x - centroid[0];
239  pt[1] = point.y - centroid[1];
240  pt[2] = point.z - centroid[2];
241 
242  temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
243  temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
244 
245  temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
246 
247  pt *= pt.x ();
248  temp_covariance_matrix (0, 0) += pt.x ();
249  temp_covariance_matrix (0, 1) += pt.y ();
250  temp_covariance_matrix (0, 2) += pt.z ();
251  ++point_count;
252  }
253  if (point_count > 0) {
254  covariance_matrix = temp_covariance_matrix;
255  }
256  }
257  if (point_count == 0) {
258  return 0;
259  }
260  covariance_matrix (1, 0) = covariance_matrix (0, 1);
261  covariance_matrix (2, 0) = covariance_matrix (0, 2);
262  covariance_matrix (2, 1) = covariance_matrix (1, 2);
263 
264  return (point_count);
265 }
266 
267 
268 template <typename PointT, typename Scalar> inline unsigned int
269 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
270  const Eigen::Matrix<Scalar, 4, 1> &centroid,
271  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
272 {
273  unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
274  if (point_count != 0)
275  covariance_matrix /= static_cast<Scalar> (point_count);
276  return (point_count);
277 }
278 
279 
280 template <typename PointT, typename Scalar> inline unsigned int
281 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
282  const Indices &indices,
283  const Eigen::Matrix<Scalar, 4, 1> &centroid,
284  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
285 {
286  if (indices.empty ())
287  return (0);
288 
289  std::size_t point_count;
290  // If the data is dense, we don't need to check for NaN
291  if (cloud.is_dense)
292  {
293  covariance_matrix.setZero ();
294  point_count = indices.size ();
295  // For each point in the cloud
296  for (const auto& idx: indices)
297  {
298  Eigen::Matrix<Scalar, 4, 1> pt;
299  pt[0] = cloud[idx].x - centroid[0];
300  pt[1] = cloud[idx].y - centroid[1];
301  pt[2] = cloud[idx].z - centroid[2];
302 
303  covariance_matrix (1, 1) += pt.y () * pt.y ();
304  covariance_matrix (1, 2) += pt.y () * pt.z ();
305 
306  covariance_matrix (2, 2) += pt.z () * pt.z ();
307 
308  pt *= pt.x ();
309  covariance_matrix (0, 0) += pt.x ();
310  covariance_matrix (0, 1) += pt.y ();
311  covariance_matrix (0, 2) += pt.z ();
312  }
313  }
314  // NaN or Inf values could exist => check for them
315  else
316  {
317  Eigen::Matrix<Scalar, 3, 3> temp_covariance_matrix;
318  temp_covariance_matrix.setZero ();
319  point_count = 0;
320  // For each point in the cloud
321  for (const auto &index : indices)
322  {
323  // Check if the point is invalid
324  if (!isFinite (cloud[index]))
325  continue;
326 
327  Eigen::Matrix<Scalar, 4, 1> pt;
328  pt[0] = cloud[index].x - centroid[0];
329  pt[1] = cloud[index].y - centroid[1];
330  pt[2] = cloud[index].z - centroid[2];
331 
332  temp_covariance_matrix (1, 1) += pt.y () * pt.y ();
333  temp_covariance_matrix (1, 2) += pt.y () * pt.z ();
334 
335  temp_covariance_matrix (2, 2) += pt.z () * pt.z ();
336 
337  pt *= pt.x ();
338  temp_covariance_matrix (0, 0) += pt.x ();
339  temp_covariance_matrix (0, 1) += pt.y ();
340  temp_covariance_matrix (0, 2) += pt.z ();
341  ++point_count;
342  }
343  if (point_count > 0) {
344  covariance_matrix = temp_covariance_matrix;
345  }
346  }
347  if (point_count == 0) {
348  return 0;
349  }
350  covariance_matrix (1, 0) = covariance_matrix (0, 1);
351  covariance_matrix (2, 0) = covariance_matrix (0, 2);
352  covariance_matrix (2, 1) = covariance_matrix (1, 2);
353  return (static_cast<unsigned int> (point_count));
354 }
355 
356 
357 template <typename PointT, typename Scalar> inline unsigned int
358 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
359  const pcl::PointIndices &indices,
360  const Eigen::Matrix<Scalar, 4, 1> &centroid,
361  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
362 {
363  return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
364 }
365 
366 
367 template <typename PointT, typename Scalar> inline unsigned int
368 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
369  const Indices &indices,
370  const Eigen::Matrix<Scalar, 4, 1> &centroid,
371  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
372 {
373  unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
374  if (point_count != 0)
375  covariance_matrix /= static_cast<Scalar> (point_count);
376 
377  return (point_count);
378 }
379 
380 
381 template <typename PointT, typename Scalar> inline unsigned int
382 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
383  const pcl::PointIndices &indices,
384  const Eigen::Matrix<Scalar, 4, 1> &centroid,
385  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
386 {
387  return computeCovarianceMatrixNormalized(cloud, indices.indices, centroid, covariance_matrix);
388 }
389 
390 
391 template <typename PointT, typename Scalar> inline unsigned int
392 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
393  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
394 {
395  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
396  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
397 
398  unsigned int point_count;
399  if (cloud.is_dense)
400  {
401  point_count = static_cast<unsigned int> (cloud.size ());
402  // For each point in the cloud
403  for (const auto& point: cloud)
404  {
405  accu [0] += point.x * point.x;
406  accu [1] += point.x * point.y;
407  accu [2] += point.x * point.z;
408  accu [3] += point.y * point.y;
409  accu [4] += point.y * point.z;
410  accu [5] += point.z * point.z;
411  }
412  }
413  else
414  {
415  point_count = 0;
416  for (const auto& point: cloud)
417  {
418  if (!isFinite (point))
419  continue;
420 
421  accu [0] += point.x * point.x;
422  accu [1] += point.x * point.y;
423  accu [2] += point.x * point.z;
424  accu [3] += point.y * point.y;
425  accu [4] += point.y * point.z;
426  accu [5] += point.z * point.z;
427  ++point_count;
428  }
429  }
430 
431  if (point_count != 0)
432  {
433  accu /= static_cast<Scalar> (point_count);
434  covariance_matrix.coeffRef (0) = accu [0];
435  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
436  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
437  covariance_matrix.coeffRef (4) = accu [3];
438  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
439  covariance_matrix.coeffRef (8) = accu [5];
440  }
441  return (point_count);
442 }
443 
444 
445 template <typename PointT, typename Scalar> inline unsigned int
446 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
447  const Indices &indices,
448  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
449 {
450  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
451  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
452 
453  unsigned int point_count;
454  if (cloud.is_dense)
455  {
456  point_count = static_cast<unsigned int> (indices.size ());
457  for (const auto &index : indices)
458  {
459  //const PointT& point = cloud[*iIt];
460  accu [0] += cloud[index].x * cloud[index].x;
461  accu [1] += cloud[index].x * cloud[index].y;
462  accu [2] += cloud[index].x * cloud[index].z;
463  accu [3] += cloud[index].y * cloud[index].y;
464  accu [4] += cloud[index].y * cloud[index].z;
465  accu [5] += cloud[index].z * cloud[index].z;
466  }
467  }
468  else
469  {
470  point_count = 0;
471  for (const auto &index : indices)
472  {
473  if (!isFinite (cloud[index]))
474  continue;
475 
476  ++point_count;
477  accu [0] += cloud[index].x * cloud[index].x;
478  accu [1] += cloud[index].x * cloud[index].y;
479  accu [2] += cloud[index].x * cloud[index].z;
480  accu [3] += cloud[index].y * cloud[index].y;
481  accu [4] += cloud[index].y * cloud[index].z;
482  accu [5] += cloud[index].z * cloud[index].z;
483  }
484  }
485  if (point_count != 0)
486  {
487  accu /= static_cast<Scalar> (point_count);
488  covariance_matrix.coeffRef (0) = accu [0];
489  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
490  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
491  covariance_matrix.coeffRef (4) = accu [3];
492  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
493  covariance_matrix.coeffRef (8) = accu [5];
494  }
495  return (point_count);
496 }
497 
498 
499 template <typename PointT, typename Scalar> inline unsigned int
500 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
501  const pcl::PointIndices &indices,
502  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
503 {
504  return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
505 }
506 
507 
508 template <typename PointT, typename Scalar> inline unsigned int
509 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
510  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
511  Eigen::Matrix<Scalar, 4, 1> &centroid)
512 {
513  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
514  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
515  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
516  Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
517  for(const auto& point: cloud)
518  if(isFinite(point)) {
519  K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
520  }
521  std::size_t point_count;
522  if (cloud.is_dense)
523  {
524  point_count = cloud.size ();
525  // For each point in the cloud
526  for (const auto& point: cloud)
527  {
528  Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
529  accu [0] += x * x;
530  accu [1] += x * y;
531  accu [2] += x * z;
532  accu [3] += y * y;
533  accu [4] += y * z;
534  accu [5] += z * z;
535  accu [6] += x;
536  accu [7] += y;
537  accu [8] += z;
538  }
539  }
540  else
541  {
542  point_count = 0;
543  for (const auto& point: cloud)
544  {
545  if (!isFinite (point))
546  continue;
547 
548  Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
549  accu [0] += x * x;
550  accu [1] += x * y;
551  accu [2] += x * z;
552  accu [3] += y * y;
553  accu [4] += y * z;
554  accu [5] += z * z;
555  accu [6] += x;
556  accu [7] += y;
557  accu [8] += z;
558  ++point_count;
559  }
560  }
561  if (point_count != 0)
562  {
563  accu /= static_cast<Scalar> (point_count);
564  centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();//effective mean E[P=(x,y,z)]
565  centroid[3] = 1;
566  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2
567  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky]
568  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//(0,2)xz
569  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//(1,1)yy
570  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//(1,2)yz
571  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//(2,2)zz
572  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx
573  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx
574  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy
575  }
576  return (static_cast<unsigned int> (point_count));
577 }
578 
579 
580 template <typename PointT, typename Scalar> inline unsigned int
581 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
582  const Indices &indices,
583  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
584  Eigen::Matrix<Scalar, 4, 1> &centroid)
585 {
586  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
587  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
588  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
589  Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
590  for(const auto& index : indices)
591  if(isFinite(cloud[index])) {
592  K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
593  }
594  std::size_t point_count;
595  if (cloud.is_dense)
596  {
597  point_count = indices.size ();
598  for (const auto &index : indices)
599  {
600  Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
601  accu [0] += x * x;
602  accu [1] += x * y;
603  accu [2] += x * z;
604  accu [3] += y * y;
605  accu [4] += y * z;
606  accu [5] += z * z;
607  accu [6] += x;
608  accu [7] += y;
609  accu [8] += z;
610  }
611  }
612  else
613  {
614  point_count = 0;
615  for (const auto &index : indices)
616  {
617  if (!isFinite (cloud[index]))
618  continue;
619 
620  ++point_count;
621  Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
622  accu [0] += x * x;
623  accu [1] += x * y;
624  accu [2] += x * z;
625  accu [3] += y * y;
626  accu [4] += y * z;
627  accu [5] += z * z;
628  accu [6] += x;
629  accu [7] += y;
630  accu [8] += z;
631  }
632  }
633 
634  if (point_count != 0)
635  {
636  accu /= static_cast<Scalar> (point_count);
637  centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();//effective mean E[P=(x,y,z)]
638  centroid[3] = 1;
639  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//(0,0)xx : E[(x-E[x])^2]=E[x^2]-E[x]^2=E[(x-Kx)^2]-E[x-Kx]^2
640  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//(0,1)xy : E[(x-E[x])(y-E[y])]=E[xy]-E[x]E[y]=E[(x-Kx)(y-Ky)]-E[x-Kx]E[y-Ky]
641  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//(0,2)xz
642  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//(1,1)yy
643  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//(1,2)yz
644  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//(2,2)zz
645  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); //(1,0)yx
646  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); //(2,0)zx
647  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); //(2,1)zy
648  }
649  return (static_cast<unsigned int> (point_count));
650 }
651 
652 
653 template <typename PointT, typename Scalar> inline unsigned int
654 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
655  const pcl::PointIndices &indices,
656  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
657  Eigen::Matrix<Scalar, 4, 1> &centroid)
658 {
659  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
660 }
661 
662 template <typename PointT, typename Scalar> inline unsigned int
663 computeCentroidAndOBB (const pcl::PointCloud<PointT> &cloud,
664  Eigen::Matrix<Scalar, 3, 1> &centroid,
665  Eigen::Matrix<Scalar, 3, 1> &obb_center,
666  Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
667  Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
668 {
669  Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
670  Eigen::Matrix<Scalar, 4, 1> centroid4;
671  const auto point_count = computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid4);
672  if (!point_count)
673  return (0);
674  centroid(0) = centroid4(0);
675  centroid(1) = centroid4(1);
676  centroid(2) = centroid4(2);
677 
678  const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
679  const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
680  const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);//the eigenvectors do not need to be normalized (they are already)
681  const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
682  // Enforce right hand rule:
683  const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
684 
685  obb_rotational_matrix <<
686  major_axis(0), middle_axis(0), minor_axis(0),
687  major_axis(1), middle_axis(1), minor_axis(1),
688  major_axis(2), middle_axis(2), minor_axis(2);
689  //obb_rotational_matrix.col(0)==major_axis
690  //obb_rotational_matrix.col(1)==middle_axis
691  //obb_rotational_matrix.col(2)==minor_axis
692 
693  //Trasforming the point cloud in the (Centroid, ma-mi-mi_axis) reference
694  //with homogeneous matrix
695  //[R^t , -R^t*Centroid ]
696  //[0 , 1 ]
697  Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
698  transform.topLeftCorner(3, 3) = obb_rotational_matrix.transpose();
699  transform.topRightCorner(3, 1) =-transform.topLeftCorner(3, 3)*centroid;
700 
701  //when Scalar==double on a Windows 10 machine and MSVS:
702  //if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse
703  Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
704  Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
705  obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
706  obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
707 
708  if (cloud.is_dense)
709  {
710  const auto& point = cloud[0];
711  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
712  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
713 
714  obb_min_pointx = obb_max_pointx = P(0);
715  obb_min_pointy = obb_max_pointy = P(1);
716  obb_min_pointz = obb_max_pointz = P(2);
717 
718  for (size_t i=1; i<cloud.size();++i)
719  {
720  const auto& point = cloud[i];
721  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
722  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
723 
724  if (P(0) < obb_min_pointx)
725  obb_min_pointx = P(0);
726  else if (P(0) > obb_max_pointx)
727  obb_max_pointx = P(0);
728  if (P(1) < obb_min_pointy)
729  obb_min_pointy = P(1);
730  else if (P(1) > obb_max_pointy)
731  obb_max_pointy = P(1);
732  if (P(2) < obb_min_pointz)
733  obb_min_pointz = P(2);
734  else if (P(2) > obb_max_pointz)
735  obb_max_pointz = P(2);
736  }
737  }
738  else
739  {
740  size_t i = 0;
741  for (; i < cloud.size(); ++i)
742  {
743  const auto& point = cloud[i];
744  if (!isFinite(point))
745  continue;
746  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
747  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
748 
749  obb_min_pointx = obb_max_pointx = P(0);
750  obb_min_pointy = obb_max_pointy = P(1);
751  obb_min_pointz = obb_max_pointz = P(2);
752  ++i;
753  break;
754  }
755 
756  for (; i<cloud.size();++i)
757  {
758  const auto& point = cloud[i];
759  if (!isFinite(point))
760  continue;
761  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
762  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
763 
764  if (P(0) < obb_min_pointx)
765  obb_min_pointx = P(0);
766  else if (P(0) > obb_max_pointx)
767  obb_max_pointx = P(0);
768  if (P(1) < obb_min_pointy)
769  obb_min_pointy = P(1);
770  else if (P(1) > obb_max_pointy)
771  obb_max_pointy = P(1);
772  if (P(2) < obb_min_pointz)
773  obb_min_pointz = P(2);
774  else if (P(2) > obb_max_pointz)
775  obb_max_pointz = P(2);
776  }
777 
778  }
779 
780  const Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))
781  shift((obb_max_pointx + obb_min_pointx) / 2.0f,
782  (obb_max_pointy + obb_min_pointy) / 2.0f,
783  (obb_max_pointz + obb_min_pointz) / 2.0f);
784 
785  obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
786  obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
787  obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
788 
789  obb_center = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
790 
791  return (point_count);
792 }
793 
794 
795 template <typename PointT, typename Scalar> inline unsigned int
796 computeCentroidAndOBB (const pcl::PointCloud<PointT> &cloud,
797  const Indices &indices,
798  Eigen::Matrix<Scalar, 3, 1> &centroid,
799  Eigen::Matrix<Scalar, 3, 1> &obb_center,
800  Eigen::Matrix<Scalar, 3, 1> &obb_dimensions,
801  Eigen::Matrix<Scalar, 3, 3> &obb_rotational_matrix)
802 {
803  Eigen::Matrix<Scalar, 3, 3> covariance_matrix;
804  Eigen::Matrix<Scalar, 4, 1> centroid4;
805  const auto point_count = computeMeanAndCovarianceMatrix(cloud, indices, covariance_matrix, centroid4);
806  if (!point_count)
807  return (0);
808  centroid(0) = centroid4(0);
809  centroid(1) = centroid4(1);
810  centroid(2) = centroid4(2);
811 
812  const Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 3, 3>> evd(covariance_matrix);
813  const Eigen::Matrix<Scalar, 3, 3> eigenvectors_ = evd.eigenvectors();
814  const Eigen::Matrix<Scalar, 3, 1> minor_axis = eigenvectors_.col(0);//the eigenvectors do not need to be normalized (they are already)
815  const Eigen::Matrix<Scalar, 3, 1> middle_axis = eigenvectors_.col(1);
816  // Enforce right hand rule:
817  const Eigen::Matrix<Scalar, 3, 1> major_axis = middle_axis.cross(minor_axis);
818 
819  obb_rotational_matrix <<
820  major_axis(0), middle_axis(0), minor_axis(0),
821  major_axis(1), middle_axis(1), minor_axis(1),
822  major_axis(2), middle_axis(2), minor_axis(2);
823  //obb_rotational_matrix.col(0)==major_axis
824  //obb_rotational_matrix.col(1)==middle_axis
825  //obb_rotational_matrix.col(2)==minor_axis
826 
827  //Trasforming the point cloud in the (Centroid, ma-mi-mi_axis) reference
828  //with homogeneous matrix
829  //[R^t , -R^t*Centroid ]
830  //[0 , 1 ]
831  Eigen::Matrix<Scalar, 4, 4> transform = Eigen::Matrix<Scalar, 4, 4>::Identity();
832  transform.topLeftCorner(3, 3) = obb_rotational_matrix.transpose();
833  transform.topRightCorner(3, 1) =-transform.topLeftCorner(3, 3)*centroid;
834 
835  //when Scalar==double on a Windows 10 machine and MSVS:
836  //if you substitute the following Scalars with floats you get a 20% worse processing time, if with 2 PointT 55% worse
837  Scalar obb_min_pointx, obb_min_pointy, obb_min_pointz;
838  Scalar obb_max_pointx, obb_max_pointy, obb_max_pointz;
839  obb_min_pointx = obb_min_pointy = obb_min_pointz = std::numeric_limits<Scalar>::max();
840  obb_max_pointx = obb_max_pointy = obb_max_pointz = std::numeric_limits<Scalar>::min();
841 
842  if (cloud.is_dense)
843  {
844  const auto& point = cloud[indices[0]];
845  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
846  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
847 
848  obb_min_pointx = obb_max_pointx = P(0);
849  obb_min_pointy = obb_max_pointy = P(1);
850  obb_min_pointz = obb_max_pointz = P(2);
851 
852  for (size_t i=1; i<indices.size();++i)
853  {
854  const auto & point = cloud[indices[i]];
855 
856  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
857  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
858 
859  if (P(0) < obb_min_pointx)
860  obb_min_pointx = P(0);
861  else if (P(0) > obb_max_pointx)
862  obb_max_pointx = P(0);
863  if (P(1) < obb_min_pointy)
864  obb_min_pointy = P(1);
865  else if (P(1) > obb_max_pointy)
866  obb_max_pointy = P(1);
867  if (P(2) < obb_min_pointz)
868  obb_min_pointz = P(2);
869  else if (P(2) > obb_max_pointz)
870  obb_max_pointz = P(2);
871  }
872  }
873  else
874  {
875  size_t i = 0;
876  for (; i<indices.size();++i)
877  {
878  const auto& point = cloud[indices[i]];
879  if (!isFinite(point))
880  continue;
881  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
882  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
883 
884  obb_min_pointx = obb_max_pointx = P(0);
885  obb_min_pointy = obb_max_pointy = P(1);
886  obb_min_pointz = obb_max_pointz = P(2);
887  ++i;
888  break;
889  }
890 
891  for (; i<indices.size();++i)
892  {
893  const auto& point = cloud[indices[i]];
894  if (!isFinite(point))
895  continue;
896 
897  Eigen::Matrix<Scalar, 4, 1> P0(static_cast<Scalar>(point.x), static_cast<Scalar>(point.y) , static_cast<Scalar>(point.z), 1.0);
898  Eigen::Matrix<Scalar, 4, 1> P = transform * P0;
899 
900  if (P(0) < obb_min_pointx)
901  obb_min_pointx = P(0);
902  else if (P(0) > obb_max_pointx)
903  obb_max_pointx = P(0);
904  if (P(1) < obb_min_pointy)
905  obb_min_pointy = P(1);
906  else if (P(1) > obb_max_pointy)
907  obb_max_pointy = P(1);
908  if (P(2) < obb_min_pointz)
909  obb_min_pointz = P(2);
910  else if (P(2) > obb_max_pointz)
911  obb_max_pointz = P(2);
912  }
913 
914  }
915 
916  const Eigen::Matrix<Scalar, 3, 1> //shift between point cloud centroid and OBB center (position of the OBB center relative to (p.c.centroid, major_axis, middle_axis, minor_axis))
917  shift((obb_max_pointx + obb_min_pointx) / 2.0f,
918  (obb_max_pointy + obb_min_pointy) / 2.0f,
919  (obb_max_pointz + obb_min_pointz) / 2.0f);
920 
921  obb_dimensions(0) = obb_max_pointx - obb_min_pointx;
922  obb_dimensions(1) = obb_max_pointy - obb_min_pointy;
923  obb_dimensions(2) = obb_max_pointz - obb_min_pointz;
924 
925  obb_center = centroid + obb_rotational_matrix * shift;//position of the OBB center in the same reference Oxyz of the point cloud
926 
927  return (point_count);
928 }
929 
930 
931 
932 template <typename PointT, typename Scalar> void
933 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
934  const Eigen::Matrix<Scalar, 4, 1> &centroid,
935  pcl::PointCloud<PointT> &cloud_out,
936  int npts)
937 {
938  // Calculate the number of points if not given
939  if (npts == 0)
940  {
941  while (cloud_iterator.isValid ())
942  {
943  ++npts;
944  ++cloud_iterator;
945  }
946  cloud_iterator.reset ();
947  }
948 
949  int i = 0;
950  cloud_out.resize (npts);
951  // Subtract the centroid from cloud_in
952  while (cloud_iterator.isValid ())
953  {
954  cloud_out[i].x = cloud_iterator->x - centroid[0];
955  cloud_out[i].y = cloud_iterator->y - centroid[1];
956  cloud_out[i].z = cloud_iterator->z - centroid[2];
957  ++i;
958  ++cloud_iterator;
959  }
960  cloud_out.width = cloud_out.size ();
961  cloud_out.height = 1;
962 }
963 
964 
965 template <typename PointT, typename Scalar> void
966 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
967  const Eigen::Matrix<Scalar, 4, 1> &centroid,
968  pcl::PointCloud<PointT> &cloud_out)
969 {
970  cloud_out = cloud_in;
971 
972  // Subtract the centroid from cloud_in
973  for (auto& point: cloud_out)
974  {
975  point.x -= static_cast<float> (centroid[0]);
976  point.y -= static_cast<float> (centroid[1]);
977  point.z -= static_cast<float> (centroid[2]);
978  }
979 }
980 
981 
982 template <typename PointT, typename Scalar> void
983 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
984  const Indices &indices,
985  const Eigen::Matrix<Scalar, 4, 1> &centroid,
986  pcl::PointCloud<PointT> &cloud_out)
987 {
988  cloud_out.header = cloud_in.header;
989  cloud_out.is_dense = cloud_in.is_dense;
990  if (indices.size () == cloud_in.size ())
991  {
992  cloud_out.width = cloud_in.width;
993  cloud_out.height = cloud_in.height;
994  }
995  else
996  {
997  cloud_out.width = indices.size ();
998  cloud_out.height = 1;
999  }
1000  cloud_out.resize (indices.size ());
1001 
1002  // Subtract the centroid from cloud_in
1003  for (std::size_t i = 0; i < indices.size (); ++i)
1004  {
1005  cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
1006  cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
1007  cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
1008  }
1009 }
1010 
1011 
1012 template <typename PointT, typename Scalar> void
1013 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
1014  const pcl::PointIndices& indices,
1015  const Eigen::Matrix<Scalar, 4, 1> &centroid,
1016  pcl::PointCloud<PointT> &cloud_out)
1017 {
1018  return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
1019 }
1020 
1021 
1022 template <typename PointT, typename Scalar> void
1023 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
1024  const Eigen::Matrix<Scalar, 4, 1> &centroid,
1025  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
1026  int npts)
1027 {
1028  // Calculate the number of points if not given
1029  if (npts == 0)
1030  {
1031  while (cloud_iterator.isValid ())
1032  {
1033  ++npts;
1034  ++cloud_iterator;
1035  }
1036  cloud_iterator.reset ();
1037  }
1038 
1039  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
1040 
1041  int i = 0;
1042  while (cloud_iterator.isValid ())
1043  {
1044  cloud_out (0, i) = cloud_iterator->x - centroid[0];
1045  cloud_out (1, i) = cloud_iterator->y - centroid[1];
1046  cloud_out (2, i) = cloud_iterator->z - centroid[2];
1047  ++i;
1048  ++cloud_iterator;
1049  }
1050 }
1051 
1052 
1053 template <typename PointT, typename Scalar> void
1054 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
1055  const Eigen::Matrix<Scalar, 4, 1> &centroid,
1056  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1057 {
1058  std::size_t npts = cloud_in.size ();
1059 
1060  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
1061 
1062  for (std::size_t i = 0; i < npts; ++i)
1063  {
1064  cloud_out (0, i) = cloud_in[i].x - centroid[0];
1065  cloud_out (1, i) = cloud_in[i].y - centroid[1];
1066  cloud_out (2, i) = cloud_in[i].z - centroid[2];
1067  // One column at a time
1068  //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
1069  }
1070 
1071  // Make sure we zero the 4th dimension out (1 row, N columns)
1072  //cloud_out.block (3, 0, 1, npts).setZero ();
1073 }
1074 
1075 
1076 template <typename PointT, typename Scalar> void
1077 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
1078  const Indices &indices,
1079  const Eigen::Matrix<Scalar, 4, 1> &centroid,
1080  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1081 {
1082  std::size_t npts = indices.size ();
1083 
1084  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
1085 
1086  for (std::size_t i = 0; i < npts; ++i)
1087  {
1088  cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
1089  cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
1090  cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
1091  // One column at a time
1092  //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
1093  }
1094 
1095  // Make sure we zero the 4th dimension out (1 row, N columns)
1096  //cloud_out.block (3, 0, 1, npts).setZero ();
1097 }
1098 
1099 
1100 template <typename PointT, typename Scalar> void
1101 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
1102  const pcl::PointIndices &indices,
1103  const Eigen::Matrix<Scalar, 4, 1> &centroid,
1104  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
1105 {
1106  return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
1107 }
1108 
1109 
1110 template <typename PointT, typename Scalar> inline void
1111 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
1112  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
1113 {
1114  using FieldList = typename pcl::traits::fieldList<PointT>::type;
1115 
1116  // Get the size of the fields
1117  centroid.setZero (boost::mpl::size<FieldList>::value);
1118 
1119  if (cloud.empty ())
1120  return;
1121 
1122  // Iterate over each point
1123  for (const auto& pt: cloud)
1124  {
1125  // Iterate over each dimension
1126  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
1127  }
1128  centroid /= static_cast<Scalar> (cloud.size ());
1129 }
1130 
1131 
1132 template <typename PointT, typename Scalar> inline void
1133 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
1134  const Indices &indices,
1135  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
1136 {
1137  using FieldList = typename pcl::traits::fieldList<PointT>::type;
1138 
1139  // Get the size of the fields
1140  centroid.setZero (boost::mpl::size<FieldList>::value);
1141 
1142  if (indices.empty ())
1143  return;
1144 
1145  // Iterate over each point
1146  for (const auto& index: indices)
1147  {
1148  // Iterate over each dimension
1149  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
1150  }
1151  centroid /= static_cast<Scalar> (indices.size ());
1152 }
1153 
1154 
1155 template <typename PointT, typename Scalar> inline void
1156 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
1157  const pcl::PointIndices &indices,
1158  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
1159 {
1160  return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
1161 }
1162 
1163 template <typename PointT> void
1164 CentroidPoint<PointT>::add (const PointT& point)
1165 {
1166  // Invoke add point on each accumulator
1167  boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
1168  ++num_points_;
1169 }
1170 
1171 template <typename PointT>
1172 template <typename PointOutT> void
1173 CentroidPoint<PointT>::get (PointOutT& point) const
1174 {
1175  if (num_points_ != 0)
1176  {
1177  // Filter accumulators so that only those that are compatible with
1178  // both PointT and requested point type remain
1179  auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
1180  // Invoke get point on each accumulator in filtered list
1181  boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
1182  }
1183 }
1184 
1185 
1186 template <typename PointInT, typename PointOutT> std::size_t
1187 computeCentroid (const pcl::PointCloud<PointInT>& cloud,
1188  PointOutT& centroid)
1189 {
1190  pcl::CentroidPoint<PointInT> cp;
1191 
1192  if (cloud.is_dense)
1193  for (const auto& point: cloud)
1194  cp.add (point);
1195  else
1196  for (const auto& point: cloud)
1197  if (pcl::isFinite (point))
1198  cp.add (point);
1199 
1200  cp.get (centroid);
1201  return (cp.getSize ());
1202 }
1203 
1204 
1205 template <typename PointInT, typename PointOutT> std::size_t
1206 computeCentroid (const pcl::PointCloud<PointInT>& cloud,
1207  const Indices& indices,
1208  PointOutT& centroid)
1209 {
1210  pcl::CentroidPoint<PointInT> cp;
1211 
1212  if (cloud.is_dense)
1213  for (const auto &index : indices)
1214  cp.add (cloud[index]);
1215  else
1216  for (const auto &index : indices)
1217  if (pcl::isFinite (cloud[index]))
1218  cp.add (cloud[index]);
1219 
1220  cp.get (centroid);
1221  return (cp.getSize ());
1222 }
1223 
1224 } // namespace pcl
1225 
centroid.h
Define methods for centroid estimation and covariance matrix calculus.
pcl::CentroidPoint
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1077
pcl::CentroidPoint::get
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:1173
pcl::CentroidPoint::add
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:1164
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition: cloud_iterator.h:121
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:446
pcl::PointCloud::is_dense
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
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::computeCentroidAndOBB
unsigned int computeCentroidAndOBB(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 1 > &centroid, Eigen::Matrix< Scalar, 3, 1 > &obb_center, Eigen::Matrix< Scalar, 3, 1 > &obb_dimensions, Eigen::Matrix< Scalar, 3, 3 > &obb_rotational_matrix)
Compute centroid, OBB (Oriented Bounding Box), PCA axes of a given set of points.
Definition: centroid.hpp:663
pcl::computeCentroid
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:1187
pcl::computeNDCentroid
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:1111
pcl::computeMeanAndCovarianceMatrix
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:509
pcl::demeanPointCloud
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:933
pcl::computeCovarianceMatrixNormalized
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:269
pcl::computeCovarianceMatrix
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:192
pcl::compute3DCentroid
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:57
pcl::K
@ K
Definition: norms.h:54
pcl::isFinite
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
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::NdCentroidFunctor
Helper functor structure for n-D centroid estimation.
Definition: centroid.h:898
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623