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
56 compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
57  Eigen::Matrix<Scalar, 4, 1> &centroid)
58 {
59  // Initialize to 0
60  centroid.setZero ();
61 
62  unsigned 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  centroid[0] += cloud_iterator->x;
72  centroid[1] += cloud_iterator->y;
73  centroid[2] += cloud_iterator->z;
74  ++cp;
75  }
76  ++cloud_iterator;
77  }
78  centroid /= static_cast<Scalar> (cp);
79  centroid[3] = 1;
80  return (cp);
81 }
82 
83 
84 template <typename PointT, typename Scalar> inline unsigned int
85 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
86  Eigen::Matrix<Scalar, 4, 1> &centroid)
87 {
88  if (cloud.empty ())
89  return (0);
90 
91  // Initialize to 0
92  centroid.setZero ();
93  // For each point in the cloud
94  // If the data is dense, we don't need to check for NaN
95  if (cloud.is_dense)
96  {
97  for (const auto& point: cloud)
98  {
99  centroid[0] += point.x;
100  centroid[1] += point.y;
101  centroid[2] += point.z;
102  }
103  centroid /= static_cast<Scalar> (cloud.size ());
104  centroid[3] = 1;
105 
106  return (static_cast<unsigned int> (cloud.size ()));
107  }
108  // NaN or Inf values could exist => check for them
109  unsigned cp = 0;
110  for (const auto& point: cloud)
111  {
112  // Check if the point is invalid
113  if (!isFinite (point))
114  continue;
115 
116  centroid[0] += point.x;
117  centroid[1] += point.y;
118  centroid[2] += point.z;
119  ++cp;
120  }
121  centroid /= static_cast<Scalar> (cp);
122  centroid[3] = 1;
123 
124  return (cp);
125 }
126 
127 
128 template <typename PointT, typename Scalar> inline unsigned int
129 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
130  const Indices &indices,
131  Eigen::Matrix<Scalar, 4, 1> &centroid)
132 {
133  if (indices.empty ())
134  return (0);
135 
136  // Initialize to 0
137  centroid.setZero ();
138  // If the data is dense, we don't need to check for NaN
139  if (cloud.is_dense)
140  {
141  for (const auto& index : indices)
142  {
143  centroid[0] += cloud[index].x;
144  centroid[1] += cloud[index].y;
145  centroid[2] += cloud[index].z;
146  }
147  centroid /= static_cast<Scalar> (indices.size ());
148  centroid[3] = 1;
149  return (static_cast<unsigned int> (indices.size ()));
150  }
151  // NaN or Inf values could exist => check for them
152  unsigned cp = 0;
153  for (const auto& index : indices)
154  {
155  // Check if the point is invalid
156  if (!isFinite (cloud [index]))
157  continue;
158 
159  centroid[0] += cloud[index].x;
160  centroid[1] += cloud[index].y;
161  centroid[2] += cloud[index].z;
162  ++cp;
163  }
164  centroid /= static_cast<Scalar> (cp);
165  centroid[3] = 1;
166  return (cp);
167 }
168 
169 
170 template <typename PointT, typename Scalar> inline unsigned int
171 compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
172  const pcl::PointIndices &indices,
173  Eigen::Matrix<Scalar, 4, 1> &centroid)
174 {
175  return (pcl::compute3DCentroid (cloud, indices.indices, centroid));
176 }
177 
178 
179 template <typename PointT, typename Scalar> inline unsigned
180 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
181  const Eigen::Matrix<Scalar, 4, 1> &centroid,
182  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
183 {
184  if (cloud.empty ())
185  return (0);
186 
187  // Initialize to 0
188  covariance_matrix.setZero ();
189 
190  unsigned point_count;
191  // If the data is dense, we don't need to check for NaN
192  if (cloud.is_dense)
193  {
194  point_count = static_cast<unsigned> (cloud.size ());
195  // For each point in the cloud
196  for (const auto& point: cloud)
197  {
198  Eigen::Matrix<Scalar, 4, 1> pt;
199  pt[0] = point.x - centroid[0];
200  pt[1] = point.y - centroid[1];
201  pt[2] = point.z - centroid[2];
202 
203  covariance_matrix (1, 1) += pt.y () * pt.y ();
204  covariance_matrix (1, 2) += pt.y () * pt.z ();
205 
206  covariance_matrix (2, 2) += pt.z () * pt.z ();
207 
208  pt *= pt.x ();
209  covariance_matrix (0, 0) += pt.x ();
210  covariance_matrix (0, 1) += pt.y ();
211  covariance_matrix (0, 2) += pt.z ();
212  }
213  }
214  // NaN or Inf values could exist => check for them
215  else
216  {
217  point_count = 0;
218  // For each point in the cloud
219  for (const auto& point: cloud)
220  {
221  // Check if the point is invalid
222  if (!isFinite (point))
223  continue;
224 
225  Eigen::Matrix<Scalar, 4, 1> pt;
226  pt[0] = point.x - centroid[0];
227  pt[1] = point.y - centroid[1];
228  pt[2] = point.z - centroid[2];
229 
230  covariance_matrix (1, 1) += pt.y () * pt.y ();
231  covariance_matrix (1, 2) += pt.y () * pt.z ();
232 
233  covariance_matrix (2, 2) += pt.z () * pt.z ();
234 
235  pt *= pt.x ();
236  covariance_matrix (0, 0) += pt.x ();
237  covariance_matrix (0, 1) += pt.y ();
238  covariance_matrix (0, 2) += pt.z ();
239  ++point_count;
240  }
241  }
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);
245 
246  return (point_count);
247 }
248 
249 
250 template <typename PointT, typename Scalar> inline unsigned int
251 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
252  const Eigen::Matrix<Scalar, 4, 1> &centroid,
253  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
254 {
255  unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
256  if (point_count != 0)
257  covariance_matrix /= static_cast<Scalar> (point_count);
258  return (point_count);
259 }
260 
261 
262 template <typename PointT, typename Scalar> inline unsigned int
263 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
264  const Indices &indices,
265  const Eigen::Matrix<Scalar, 4, 1> &centroid,
266  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
267 {
268  if (indices.empty ())
269  return (0);
270 
271  // Initialize to 0
272  covariance_matrix.setZero ();
273 
274  std::size_t point_count;
275  // If the data is dense, we don't need to check for NaN
276  if (cloud.is_dense)
277  {
278  point_count = indices.size ();
279  // For each point in the cloud
280  for (const auto& idx: indices)
281  {
282  Eigen::Matrix<Scalar, 4, 1> pt;
283  pt[0] = cloud[idx].x - centroid[0];
284  pt[1] = cloud[idx].y - centroid[1];
285  pt[2] = cloud[idx].z - centroid[2];
286 
287  covariance_matrix (1, 1) += pt.y () * pt.y ();
288  covariance_matrix (1, 2) += pt.y () * pt.z ();
289 
290  covariance_matrix (2, 2) += pt.z () * pt.z ();
291 
292  pt *= pt.x ();
293  covariance_matrix (0, 0) += pt.x ();
294  covariance_matrix (0, 1) += pt.y ();
295  covariance_matrix (0, 2) += pt.z ();
296  }
297  }
298  // NaN or Inf values could exist => check for them
299  else
300  {
301  point_count = 0;
302  // For each point in the cloud
303  for (const auto &index : indices)
304  {
305  // Check if the point is invalid
306  if (!isFinite (cloud[index]))
307  continue;
308 
309  Eigen::Matrix<Scalar, 4, 1> pt;
310  pt[0] = cloud[index].x - centroid[0];
311  pt[1] = cloud[index].y - centroid[1];
312  pt[2] = cloud[index].z - centroid[2];
313 
314  covariance_matrix (1, 1) += pt.y () * pt.y ();
315  covariance_matrix (1, 2) += pt.y () * pt.z ();
316 
317  covariance_matrix (2, 2) += pt.z () * pt.z ();
318 
319  pt *= pt.x ();
320  covariance_matrix (0, 0) += pt.x ();
321  covariance_matrix (0, 1) += pt.y ();
322  covariance_matrix (0, 2) += pt.z ();
323  ++point_count;
324  }
325  }
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));
330 }
331 
332 
333 template <typename PointT, typename Scalar> inline unsigned int
334 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
335  const pcl::PointIndices &indices,
336  const Eigen::Matrix<Scalar, 4, 1> &centroid,
337  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
338 {
339  return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix));
340 }
341 
342 
343 template <typename PointT, typename Scalar> inline unsigned int
344 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
345  const Indices &indices,
346  const Eigen::Matrix<Scalar, 4, 1> &centroid,
347  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
348 {
349  unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
350  if (point_count != 0)
351  covariance_matrix /= static_cast<Scalar> (point_count);
352 
353  return (point_count);
354 }
355 
356 
357 template <typename PointT, typename Scalar> inline unsigned int
358 computeCovarianceMatrixNormalized (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 computeCovarianceMatrixNormalized(cloud, indices.indices, centroid, covariance_matrix);
364 }
365 
366 
367 template <typename PointT, typename Scalar> inline unsigned int
368 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
369  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
370 {
371  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
372  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
373 
374  unsigned int point_count;
375  if (cloud.is_dense)
376  {
377  point_count = static_cast<unsigned int> (cloud.size ());
378  // For each point in the cloud
379  for (const auto& point: cloud)
380  {
381  accu [0] += point.x * point.x;
382  accu [1] += point.x * point.y;
383  accu [2] += point.x * point.z;
384  accu [3] += point.y * point.y;
385  accu [4] += point.y * point.z;
386  accu [5] += point.z * point.z;
387  }
388  }
389  else
390  {
391  point_count = 0;
392  for (const auto& point: cloud)
393  {
394  if (!isFinite (point))
395  continue;
396 
397  accu [0] += point.x * point.x;
398  accu [1] += point.x * point.y;
399  accu [2] += point.x * point.z;
400  accu [3] += point.y * point.y;
401  accu [4] += point.y * point.z;
402  accu [5] += point.z * point.z;
403  ++point_count;
404  }
405  }
406 
407  if (point_count != 0)
408  {
409  accu /= static_cast<Scalar> (point_count);
410  covariance_matrix.coeffRef (0) = accu [0];
411  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
412  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
413  covariance_matrix.coeffRef (4) = accu [3];
414  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
415  covariance_matrix.coeffRef (8) = accu [5];
416  }
417  return (point_count);
418 }
419 
420 
421 template <typename PointT, typename Scalar> inline unsigned int
422 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
423  const Indices &indices,
424  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
425 {
426  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
427  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
428 
429  unsigned int point_count;
430  if (cloud.is_dense)
431  {
432  point_count = static_cast<unsigned int> (indices.size ());
433  for (const auto &index : indices)
434  {
435  //const PointT& point = cloud[*iIt];
436  accu [0] += cloud[index].x * cloud[index].x;
437  accu [1] += cloud[index].x * cloud[index].y;
438  accu [2] += cloud[index].x * cloud[index].z;
439  accu [3] += cloud[index].y * cloud[index].y;
440  accu [4] += cloud[index].y * cloud[index].z;
441  accu [5] += cloud[index].z * cloud[index].z;
442  }
443  }
444  else
445  {
446  point_count = 0;
447  for (const auto &index : indices)
448  {
449  if (!isFinite (cloud[index]))
450  continue;
451 
452  ++point_count;
453  accu [0] += cloud[index].x * cloud[index].x;
454  accu [1] += cloud[index].x * cloud[index].y;
455  accu [2] += cloud[index].x * cloud[index].z;
456  accu [3] += cloud[index].y * cloud[index].y;
457  accu [4] += cloud[index].y * cloud[index].z;
458  accu [5] += cloud[index].z * cloud[index].z;
459  }
460  }
461  if (point_count != 0)
462  {
463  accu /= static_cast<Scalar> (point_count);
464  covariance_matrix.coeffRef (0) = accu [0];
465  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
466  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
467  covariance_matrix.coeffRef (4) = accu [3];
468  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
469  covariance_matrix.coeffRef (8) = accu [5];
470  }
471  return (point_count);
472 }
473 
474 
475 template <typename PointT, typename Scalar> inline unsigned int
476 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
477  const pcl::PointIndices &indices,
478  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
479 {
480  return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
481 }
482 
483 
484 template <typename PointT, typename Scalar> inline unsigned int
485 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
486  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
487  Eigen::Matrix<Scalar, 4, 1> &centroid)
488 {
489  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
490  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
491  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
492  Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
493  for(const auto& point: cloud)
494  if(isFinite(point)) {
495  K.x() = point.x; K.y() = point.y; K.z() = point.z; break;
496  }
497  std::size_t point_count;
498  if (cloud.is_dense)
499  {
500  point_count = cloud.size ();
501  // For each point in the cloud
502  for (const auto& point: cloud)
503  {
504  Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
505  accu [0] += x * x;
506  accu [1] += x * y;
507  accu [2] += x * z;
508  accu [3] += y * y;
509  accu [4] += y * z;
510  accu [5] += z * z;
511  accu [6] += x;
512  accu [7] += y;
513  accu [8] += z;
514  }
515  }
516  else
517  {
518  point_count = 0;
519  for (const auto& point: cloud)
520  {
521  if (!isFinite (point))
522  continue;
523 
524  Scalar x = point.x - K.x(), y = point.y - K.y(), z = point.z - K.z();
525  accu [0] += x * x;
526  accu [1] += x * y;
527  accu [2] += x * z;
528  accu [3] += y * y;
529  accu [4] += y * z;
530  accu [5] += z * z;
531  accu [6] += x;
532  accu [7] += y;
533  accu [8] += z;
534  ++point_count;
535  }
536  }
537  if (point_count != 0)
538  {
539  accu /= static_cast<Scalar> (point_count);
540  centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
541  centroid[3] = 1;
542  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
543  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
544  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
545  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
546  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
547  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
548  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
549  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
550  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
551  }
552  return (static_cast<unsigned int> (point_count));
553 }
554 
555 
556 template <typename PointT, typename Scalar> inline unsigned int
557 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
558  const Indices &indices,
559  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
560  Eigen::Matrix<Scalar, 4, 1> &centroid)
561 {
562  // Shifted data/with estimate of mean. This gives very good accuracy and good performance.
563  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
564  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
565  Eigen::Matrix<Scalar, 3, 1> K(0.0, 0.0, 0.0);
566  for(const auto& index : indices)
567  if(isFinite(cloud[index])) {
568  K.x() = cloud[index].x; K.y() = cloud[index].y; K.z() = cloud[index].z; break;
569  }
570  std::size_t point_count;
571  if (cloud.is_dense)
572  {
573  point_count = indices.size ();
574  for (const auto &index : indices)
575  {
576  Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
577  accu [0] += x * x;
578  accu [1] += x * y;
579  accu [2] += x * z;
580  accu [3] += y * y;
581  accu [4] += y * z;
582  accu [5] += z * z;
583  accu [6] += x;
584  accu [7] += y;
585  accu [8] += z;
586  }
587  }
588  else
589  {
590  point_count = 0;
591  for (const auto &index : indices)
592  {
593  if (!isFinite (cloud[index]))
594  continue;
595 
596  ++point_count;
597  Scalar x = cloud[index].x - K.x(), y = cloud[index].y - K.y(), z = cloud[index].z - K.z();
598  accu [0] += x * x;
599  accu [1] += x * y;
600  accu [2] += x * z;
601  accu [3] += y * y;
602  accu [4] += y * z;
603  accu [5] += z * z;
604  accu [6] += x;
605  accu [7] += y;
606  accu [8] += z;
607  }
608  }
609 
610  if (point_count != 0)
611  {
612  accu /= static_cast<Scalar> (point_count);
613  centroid[0] = accu[6] + K.x(); centroid[1] = accu[7] + K.y(); centroid[2] = accu[8] + K.z();
614  centroid[3] = 1;
615  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
616  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
617  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
618  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
619  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
620  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
621  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
622  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
623  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
624  }
625  return (static_cast<unsigned int> (point_count));
626 }
627 
628 
629 template <typename PointT, typename Scalar> inline unsigned int
630 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
631  const pcl::PointIndices &indices,
632  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
633  Eigen::Matrix<Scalar, 4, 1> &centroid)
634 {
635  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
636 }
637 
638 
639 template <typename PointT, typename Scalar> void
640 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
641  const Eigen::Matrix<Scalar, 4, 1> &centroid,
642  pcl::PointCloud<PointT> &cloud_out,
643  int npts)
644 {
645  // Calculate the number of points if not given
646  if (npts == 0)
647  {
648  while (cloud_iterator.isValid ())
649  {
650  ++npts;
651  ++cloud_iterator;
652  }
653  cloud_iterator.reset ();
654  }
655 
656  int i = 0;
657  cloud_out.resize (npts);
658  // Subtract the centroid from cloud_in
659  while (cloud_iterator.isValid ())
660  {
661  cloud_out[i].x = cloud_iterator->x - centroid[0];
662  cloud_out[i].y = cloud_iterator->y - centroid[1];
663  cloud_out[i].z = cloud_iterator->z - centroid[2];
664  ++i;
665  ++cloud_iterator;
666  }
667  cloud_out.width = cloud_out.size ();
668  cloud_out.height = 1;
669 }
670 
671 
672 template <typename PointT, typename Scalar> void
673 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
674  const Eigen::Matrix<Scalar, 4, 1> &centroid,
675  pcl::PointCloud<PointT> &cloud_out)
676 {
677  cloud_out = cloud_in;
678 
679  // Subtract the centroid from cloud_in
680  for (auto& point: cloud_out)
681  {
682  point.x -= static_cast<float> (centroid[0]);
683  point.y -= static_cast<float> (centroid[1]);
684  point.z -= static_cast<float> (centroid[2]);
685  }
686 }
687 
688 
689 template <typename PointT, typename Scalar> void
690 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
691  const Indices &indices,
692  const Eigen::Matrix<Scalar, 4, 1> &centroid,
693  pcl::PointCloud<PointT> &cloud_out)
694 {
695  cloud_out.header = cloud_in.header;
696  cloud_out.is_dense = cloud_in.is_dense;
697  if (indices.size () == cloud_in.size ())
698  {
699  cloud_out.width = cloud_in.width;
700  cloud_out.height = cloud_in.height;
701  }
702  else
703  {
704  cloud_out.width = indices.size ();
705  cloud_out.height = 1;
706  }
707  cloud_out.resize (indices.size ());
708 
709  // Subtract the centroid from cloud_in
710  for (std::size_t i = 0; i < indices.size (); ++i)
711  {
712  cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
713  cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
714  cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
715  }
716 }
717 
718 
719 template <typename PointT, typename Scalar> void
720 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
721  const pcl::PointIndices& indices,
722  const Eigen::Matrix<Scalar, 4, 1> &centroid,
723  pcl::PointCloud<PointT> &cloud_out)
724 {
725  return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
726 }
727 
728 
729 template <typename PointT, typename Scalar> void
730 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
731  const Eigen::Matrix<Scalar, 4, 1> &centroid,
732  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
733  int npts)
734 {
735  // Calculate the number of points if not given
736  if (npts == 0)
737  {
738  while (cloud_iterator.isValid ())
739  {
740  ++npts;
741  ++cloud_iterator;
742  }
743  cloud_iterator.reset ();
744  }
745 
746  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
747 
748  int i = 0;
749  while (cloud_iterator.isValid ())
750  {
751  cloud_out (0, i) = cloud_iterator->x - centroid[0];
752  cloud_out (1, i) = cloud_iterator->y - centroid[1];
753  cloud_out (2, i) = cloud_iterator->z - centroid[2];
754  ++i;
755  ++cloud_iterator;
756  }
757 }
758 
759 
760 template <typename PointT, typename Scalar> void
761 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
762  const Eigen::Matrix<Scalar, 4, 1> &centroid,
763  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
764 {
765  std::size_t npts = cloud_in.size ();
766 
767  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
768 
769  for (std::size_t i = 0; i < npts; ++i)
770  {
771  cloud_out (0, i) = cloud_in[i].x - centroid[0];
772  cloud_out (1, i) = cloud_in[i].y - centroid[1];
773  cloud_out (2, i) = cloud_in[i].z - centroid[2];
774  // One column at a time
775  //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
776  }
777 
778  // Make sure we zero the 4th dimension out (1 row, N columns)
779  //cloud_out.block (3, 0, 1, npts).setZero ();
780 }
781 
782 
783 template <typename PointT, typename Scalar> void
784 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
785  const Indices &indices,
786  const Eigen::Matrix<Scalar, 4, 1> &centroid,
787  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
788 {
789  std::size_t npts = indices.size ();
790 
791  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
792 
793  for (std::size_t i = 0; i < npts; ++i)
794  {
795  cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
796  cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
797  cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
798  // One column at a time
799  //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
800  }
801 
802  // Make sure we zero the 4th dimension out (1 row, N columns)
803  //cloud_out.block (3, 0, 1, npts).setZero ();
804 }
805 
806 
807 template <typename PointT, typename Scalar> void
808 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
809  const pcl::PointIndices &indices,
810  const Eigen::Matrix<Scalar, 4, 1> &centroid,
811  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
812 {
813  return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
814 }
815 
816 
817 template <typename PointT, typename Scalar> inline void
818 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
819  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
820 {
821  using FieldList = typename pcl::traits::fieldList<PointT>::type;
822 
823  // Get the size of the fields
824  centroid.setZero (boost::mpl::size<FieldList>::value);
825 
826  if (cloud.empty ())
827  return;
828 
829  // Iterate over each point
830  for (const auto& pt: cloud)
831  {
832  // Iterate over each dimension
833  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
834  }
835  centroid /= static_cast<Scalar> (cloud.size ());
836 }
837 
838 
839 template <typename PointT, typename Scalar> inline void
840 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
841  const Indices &indices,
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 (indices.empty ())
850  return;
851 
852  // Iterate over each point
853  for (const auto& index: indices)
854  {
855  // Iterate over each dimension
856  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
857  }
858  centroid /= static_cast<Scalar> (indices.size ());
859 }
860 
861 
862 template <typename PointT, typename Scalar> inline void
863 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
864  const pcl::PointIndices &indices,
865  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
866 {
867  return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
868 }
869 
870 template <typename PointT> void
871 CentroidPoint<PointT>::add (const PointT& point)
872 {
873  // Invoke add point on each accumulator
874  boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
875  ++num_points_;
876 }
877 
878 template <typename PointT>
879 template <typename PointOutT> void
880 CentroidPoint<PointT>::get (PointOutT& point) const
881 {
882  if (num_points_ != 0)
883  {
884  // Filter accumulators so that only those that are compatible with
885  // both PointT and requested point type remain
886  auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
887  // Invoke get point on each accumulator in filtered list
888  boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
889  }
890 }
891 
892 
893 template <typename PointInT, typename PointOutT> std::size_t
894 computeCentroid (const pcl::PointCloud<PointInT>& cloud,
895  PointOutT& centroid)
896 {
897  pcl::CentroidPoint<PointInT> cp;
898 
899  if (cloud.is_dense)
900  for (const auto& point: cloud)
901  cp.add (point);
902  else
903  for (const auto& point: cloud)
904  if (pcl::isFinite (point))
905  cp.add (point);
906 
907  cp.get (centroid);
908  return (cp.getSize ());
909 }
910 
911 
912 template <typename PointInT, typename PointOutT> std::size_t
913 computeCentroid (const pcl::PointCloud<PointInT>& cloud,
914  const Indices& indices,
915  PointOutT& centroid)
916 {
917  pcl::CentroidPoint<PointInT> cp;
918 
919  if (cloud.is_dense)
920  for (const auto &index : indices)
921  cp.add (cloud[index]);
922  else
923  for (const auto &index : indices)
924  if (pcl::isFinite (cloud[index]))
925  cp.add (cloud[index]);
926 
927  cp.get (centroid);
928  return (cp.getSize ());
929 }
930 
931 } // namespace pcl
932 
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:894
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:485
pcl
Definition: convolution.h:46
pcl::K
@ K
Definition: norms.h:54
pcl::detail::GetPoint
Definition: accumulators.hpp:269
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
pcl::ConstCloudIterator::isValid
bool isValid() const
Definition: cloud_iterator.hpp:551
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::detail::AddPoint
Definition: accumulators.hpp:251
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:446
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
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:251
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:640
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:678
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::CentroidPoint::get
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:880
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:180
pcl::CentroidPoint
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1022
pcl::ConstCloudIterator::reset
void reset()
Definition: cloud_iterator.hpp:544
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::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
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:818
pcl::NdCentroidFunctor
Helper functor structure for n-D centroid estimation.
Definition: centroid.h:842
pcl::ConstCloudIterator
Iterator class for point clouds with or without given indices.
Definition: cloud_iterator.h:120
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:56
pcl::CentroidPoint::add
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:871
centroid.h