Point Cloud Library (PCL)  1.11.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  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
490  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
491  std::size_t point_count;
492  if (cloud.is_dense)
493  {
494  point_count = cloud.size ();
495  // For each point in the cloud
496  for (const auto& point: cloud)
497  {
498  accu [0] += point.x * point.x;
499  accu [1] += point.x * point.y;
500  accu [2] += point.x * point.z;
501  accu [3] += point.y * point.y; // 4
502  accu [4] += point.y * point.z; // 5
503  accu [5] += point.z * point.z; // 8
504  accu [6] += point.x;
505  accu [7] += point.y;
506  accu [8] += point.z;
507  }
508  }
509  else
510  {
511  point_count = 0;
512  for (const auto& point: cloud)
513  {
514  if (!isFinite (point))
515  continue;
516 
517  accu [0] += point.x * point.x;
518  accu [1] += point.x * point.y;
519  accu [2] += point.x * point.z;
520  accu [3] += point.y * point.y;
521  accu [4] += point.y * point.z;
522  accu [5] += point.z * point.z;
523  accu [6] += point.x;
524  accu [7] += point.y;
525  accu [8] += point.z;
526  ++point_count;
527  }
528  }
529  accu /= static_cast<Scalar> (point_count);
530  if (point_count != 0)
531  {
532  //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
533  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
534  centroid[3] = 1;
535  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
536  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
537  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
538  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
539  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
540  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
541  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
542  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
543  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
544  }
545  return (static_cast<unsigned int> (point_count));
546 }
547 
548 
549 template <typename PointT, typename Scalar> inline unsigned int
550 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
551  const Indices &indices,
552  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
553  Eigen::Matrix<Scalar, 4, 1> &centroid)
554 {
555  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
556  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
557  std::size_t point_count;
558  if (cloud.is_dense)
559  {
560  point_count = indices.size ();
561  for (const auto &index : indices)
562  {
563  //const PointT& point = cloud[*iIt];
564  accu [0] += cloud[index].x * cloud[index].x;
565  accu [1] += cloud[index].x * cloud[index].y;
566  accu [2] += cloud[index].x * cloud[index].z;
567  accu [3] += cloud[index].y * cloud[index].y;
568  accu [4] += cloud[index].y * cloud[index].z;
569  accu [5] += cloud[index].z * cloud[index].z;
570  accu [6] += cloud[index].x;
571  accu [7] += cloud[index].y;
572  accu [8] += cloud[index].z;
573  }
574  }
575  else
576  {
577  point_count = 0;
578  for (const auto &index : indices)
579  {
580  if (!isFinite (cloud[index]))
581  continue;
582 
583  ++point_count;
584  accu [0] += cloud[index].x * cloud[index].x;
585  accu [1] += cloud[index].x * cloud[index].y;
586  accu [2] += cloud[index].x * cloud[index].z;
587  accu [3] += cloud[index].y * cloud[index].y; // 4
588  accu [4] += cloud[index].y * cloud[index].z; // 5
589  accu [5] += cloud[index].z * cloud[index].z; // 8
590  accu [6] += cloud[index].x;
591  accu [7] += cloud[index].y;
592  accu [8] += cloud[index].z;
593  }
594  }
595 
596  accu /= static_cast<Scalar> (point_count);
597  //Eigen::Vector3f vec = accu.tail<3> ();
598  //centroid.head<3> () = vec;//= accu.tail<3> ();
599  //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
600  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
601  centroid[3] = 1;
602  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
603  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
604  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
605  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
606  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
607  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
608  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
609  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
610  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
611 
612  return (static_cast<unsigned int> (point_count));
613 }
614 
615 
616 template <typename PointT, typename Scalar> inline unsigned int
617 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
618  const pcl::PointIndices &indices,
619  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
620  Eigen::Matrix<Scalar, 4, 1> &centroid)
621 {
622  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
623 }
624 
625 
626 template <typename PointT, typename Scalar> void
627 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
628  const Eigen::Matrix<Scalar, 4, 1> &centroid,
629  pcl::PointCloud<PointT> &cloud_out,
630  int npts)
631 {
632  // Calculate the number of points if not given
633  if (npts == 0)
634  {
635  while (cloud_iterator.isValid ())
636  {
637  ++npts;
638  ++cloud_iterator;
639  }
640  cloud_iterator.reset ();
641  }
642 
643  int i = 0;
644  cloud_out.resize (npts);
645  // Subtract the centroid from cloud_in
646  while (cloud_iterator.isValid ())
647  {
648  cloud_out[i].x = cloud_iterator->x - centroid[0];
649  cloud_out[i].y = cloud_iterator->y - centroid[1];
650  cloud_out[i].z = cloud_iterator->z - centroid[2];
651  ++i;
652  ++cloud_iterator;
653  }
654  cloud_out.width = cloud_out.size ();
655  cloud_out.height = 1;
656 }
657 
658 
659 template <typename PointT, typename Scalar> void
660 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
661  const Eigen::Matrix<Scalar, 4, 1> &centroid,
662  pcl::PointCloud<PointT> &cloud_out)
663 {
664  cloud_out = cloud_in;
665 
666  // Subtract the centroid from cloud_in
667  for (auto& point: cloud_out)
668  {
669  point.x -= static_cast<float> (centroid[0]);
670  point.y -= static_cast<float> (centroid[1]);
671  point.z -= static_cast<float> (centroid[2]);
672  }
673 }
674 
675 
676 template <typename PointT, typename Scalar> void
677 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
678  const Indices &indices,
679  const Eigen::Matrix<Scalar, 4, 1> &centroid,
680  pcl::PointCloud<PointT> &cloud_out)
681 {
682  cloud_out.header = cloud_in.header;
683  cloud_out.is_dense = cloud_in.is_dense;
684  if (indices.size () == cloud_in.size ())
685  {
686  cloud_out.width = cloud_in.width;
687  cloud_out.height = cloud_in.height;
688  }
689  else
690  {
691  cloud_out.width = indices.size ();
692  cloud_out.height = 1;
693  }
694  cloud_out.resize (indices.size ());
695 
696  // Subtract the centroid from cloud_in
697  for (std::size_t i = 0; i < indices.size (); ++i)
698  {
699  cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
700  cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
701  cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
702  }
703 }
704 
705 
706 template <typename PointT, typename Scalar> void
707 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
708  const pcl::PointIndices& indices,
709  const Eigen::Matrix<Scalar, 4, 1> &centroid,
710  pcl::PointCloud<PointT> &cloud_out)
711 {
712  return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
713 }
714 
715 
716 template <typename PointT, typename Scalar> void
717 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator,
718  const Eigen::Matrix<Scalar, 4, 1> &centroid,
719  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
720  int npts)
721 {
722  // Calculate the number of points if not given
723  if (npts == 0)
724  {
725  while (cloud_iterator.isValid ())
726  {
727  ++npts;
728  ++cloud_iterator;
729  }
730  cloud_iterator.reset ();
731  }
732 
733  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
734 
735  int i = 0;
736  while (cloud_iterator.isValid ())
737  {
738  cloud_out (0, i) = cloud_iterator->x - centroid[0];
739  cloud_out (1, i) = cloud_iterator->y - centroid[1];
740  cloud_out (2, i) = cloud_iterator->z - centroid[2];
741  ++i;
742  ++cloud_iterator;
743  }
744 }
745 
746 
747 template <typename PointT, typename Scalar> void
748 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
749  const Eigen::Matrix<Scalar, 4, 1> &centroid,
750  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
751 {
752  std::size_t npts = cloud_in.size ();
753 
754  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
755 
756  for (std::size_t i = 0; i < npts; ++i)
757  {
758  cloud_out (0, i) = cloud_in[i].x - centroid[0];
759  cloud_out (1, i) = cloud_in[i].y - centroid[1];
760  cloud_out (2, i) = cloud_in[i].z - centroid[2];
761  // One column at a time
762  //cloud_out.block<4, 1> (0, i) = cloud_in[i].getVector4fMap () - centroid;
763  }
764 
765  // Make sure we zero the 4th dimension out (1 row, N columns)
766  //cloud_out.block (3, 0, 1, npts).setZero ();
767 }
768 
769 
770 template <typename PointT, typename Scalar> void
771 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
772  const Indices &indices,
773  const Eigen::Matrix<Scalar, 4, 1> &centroid,
774  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
775 {
776  std::size_t npts = indices.size ();
777 
778  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
779 
780  for (std::size_t i = 0; i < npts; ++i)
781  {
782  cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
783  cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
784  cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
785  // One column at a time
786  //cloud_out.block<4, 1> (0, i) = cloud_in[indices[i]].getVector4fMap () - centroid;
787  }
788 
789  // Make sure we zero the 4th dimension out (1 row, N columns)
790  //cloud_out.block (3, 0, 1, npts).setZero ();
791 }
792 
793 
794 template <typename PointT, typename Scalar> void
795 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
796  const pcl::PointIndices &indices,
797  const Eigen::Matrix<Scalar, 4, 1> &centroid,
798  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
799 {
800  return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
801 }
802 
803 
804 template <typename PointT, typename Scalar> inline void
805 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
806  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
807 {
808  using FieldList = typename pcl::traits::fieldList<PointT>::type;
809 
810  // Get the size of the fields
811  centroid.setZero (boost::mpl::size<FieldList>::value);
812 
813  if (cloud.empty ())
814  return;
815 
816  // Iterate over each point
817  for (const auto& pt: cloud)
818  {
819  // Iterate over each dimension
820  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
821  }
822  centroid /= static_cast<Scalar> (cloud.size ());
823 }
824 
825 
826 template <typename PointT, typename Scalar> inline void
827 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
828  const Indices &indices,
829  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
830 {
831  using FieldList = typename pcl::traits::fieldList<PointT>::type;
832 
833  // Get the size of the fields
834  centroid.setZero (boost::mpl::size<FieldList>::value);
835 
836  if (indices.empty ())
837  return;
838 
839  // Iterate over each point
840  for (const auto& index: indices)
841  {
842  // Iterate over each dimension
843  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
844  }
845  centroid /= static_cast<Scalar> (indices.size ());
846 }
847 
848 
849 template <typename PointT, typename Scalar> inline void
850 computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
851  const pcl::PointIndices &indices,
852  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
853 {
854  return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
855 }
856 
857 template <typename PointT> void
858 CentroidPoint<PointT>::add (const PointT& point)
859 {
860  // Invoke add point on each accumulator
861  boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
862  ++num_points_;
863 }
864 
865 template <typename PointT>
866 template <typename PointOutT> void
867 CentroidPoint<PointT>::get (PointOutT& point) const
868 {
869  if (num_points_ != 0)
870  {
871  // Filter accumulators so that only those that are compatible with
872  // both PointT and requested point type remain
873  auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
874  // Invoke get point on each accumulator in filtered list
875  boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
876  }
877 }
878 
879 
880 template <typename PointInT, typename PointOutT> std::size_t
881 computeCentroid (const pcl::PointCloud<PointInT>& cloud,
882  PointOutT& centroid)
883 {
884  pcl::CentroidPoint<PointInT> cp;
885 
886  if (cloud.is_dense)
887  for (const auto& point: cloud)
888  cp.add (point);
889  else
890  for (const auto& point: cloud)
891  if (pcl::isFinite (point))
892  cp.add (point);
893 
894  cp.get (centroid);
895  return (cp.getSize ());
896 }
897 
898 
899 template <typename PointInT, typename PointOutT> std::size_t
900 computeCentroid (const pcl::PointCloud<PointInT>& cloud,
901  const Indices& indices,
902  PointOutT& centroid)
903 {
904  pcl::CentroidPoint<PointInT> cp;
905 
906  if (cloud.is_dense)
907  for (const auto &index : indices)
908  cp.add (cloud[index]);
909  else
910  for (const auto &index : indices)
911  if (pcl::isFinite (cloud[index]))
912  cp.add (cloud[index]);
913 
914  cp.get (centroid);
915  return (cp.getSize ());
916 }
917 
918 } // namespace pcl
919 
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:881
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::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:394
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:440
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:627
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:628
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::CentroidPoint::get
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:867
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:397
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointIndices
Definition: PointIndices.h:11
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:386
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
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:805
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:858
centroid.h