Point Cloud Library (PCL)  1.11.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
centroid.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-present, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/common/centroid.h>
44 #include <pcl/conversions.h>
45 #include <pcl/common/point_tests.h> // for pcl::isFinite
46 
47 #include <boost/fusion/algorithm/transformation/filter_if.hpp> // for boost::fusion::filter_if
48 #include <boost/fusion/algorithm/iteration/for_each.hpp> // for boost::fusion::for_each
49 #include <boost/mpl/size.hpp> // for boost::mpl::size
50 
51 
52 namespace pcl
53 {
54 
55 template <typename PointT, typename Scalar> inline unsigned int
57  Eigen::Matrix<Scalar, 4, 1> &centroid)
58 {
59  // 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
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
130  const std::vector<int> &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 int& 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 int& 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
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
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
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
264  const std::vector<int> &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 int &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
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
345  const std::vector<int> &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
359  const pcl::PointIndices &indices,
360  const Eigen::Matrix<Scalar, 4, 1> &centroid,
361  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
362 {
363  unsigned int point_count = pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix);
364  if (point_count != 0)
365  covariance_matrix /= static_cast<Scalar> (point_count);
366 
367  return point_count;
368 }
369 
370 
371 template <typename PointT, typename Scalar> inline unsigned int
373  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
374 {
375  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
376  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
377 
378  unsigned int point_count;
379  if (cloud.is_dense)
380  {
381  point_count = static_cast<unsigned int> (cloud.size ());
382  // For each point in the cloud
383  for (const auto& point: cloud)
384  {
385  accu [0] += point.x * point.x;
386  accu [1] += point.x * point.y;
387  accu [2] += point.x * point.z;
388  accu [3] += point.y * point.y;
389  accu [4] += point.y * point.z;
390  accu [5] += point.z * point.z;
391  }
392  }
393  else
394  {
395  point_count = 0;
396  for (const auto& point: cloud)
397  {
398  if (!isFinite (point))
399  continue;
400 
401  accu [0] += point.x * point.x;
402  accu [1] += point.x * point.y;
403  accu [2] += point.x * point.z;
404  accu [3] += point.y * point.y;
405  accu [4] += point.y * point.z;
406  accu [5] += point.z * point.z;
407  ++point_count;
408  }
409  }
410 
411  if (point_count != 0)
412  {
413  accu /= static_cast<Scalar> (point_count);
414  covariance_matrix.coeffRef (0) = accu [0];
415  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
416  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
417  covariance_matrix.coeffRef (4) = accu [3];
418  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
419  covariance_matrix.coeffRef (8) = accu [5];
420  }
421  return (point_count);
422 }
423 
424 
425 template <typename PointT, typename Scalar> inline unsigned int
427  const std::vector<int> &indices,
428  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
429 {
430  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
431  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
432 
433  unsigned int point_count;
434  if (cloud.is_dense)
435  {
436  point_count = static_cast<unsigned int> (indices.size ());
437  for (const int &index : indices)
438  {
439  //const PointT& point = cloud[*iIt];
440  accu [0] += cloud[index].x * cloud[index].x;
441  accu [1] += cloud[index].x * cloud[index].y;
442  accu [2] += cloud[index].x * cloud[index].z;
443  accu [3] += cloud[index].y * cloud[index].y;
444  accu [4] += cloud[index].y * cloud[index].z;
445  accu [5] += cloud[index].z * cloud[index].z;
446  }
447  }
448  else
449  {
450  point_count = 0;
451  for (const int &index : indices)
452  {
453  if (!isFinite (cloud[index]))
454  continue;
455 
456  ++point_count;
457  accu [0] += cloud[index].x * cloud[index].x;
458  accu [1] += cloud[index].x * cloud[index].y;
459  accu [2] += cloud[index].x * cloud[index].z;
460  accu [3] += cloud[index].y * cloud[index].y;
461  accu [4] += cloud[index].y * cloud[index].z;
462  accu [5] += cloud[index].z * cloud[index].z;
463  }
464  }
465  if (point_count != 0)
466  {
467  accu /= static_cast<Scalar> (point_count);
468  covariance_matrix.coeffRef (0) = accu [0];
469  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
470  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
471  covariance_matrix.coeffRef (4) = accu [3];
472  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
473  covariance_matrix.coeffRef (8) = accu [5];
474  }
475  return (point_count);
476 }
477 
478 
479 template <typename PointT, typename Scalar> inline unsigned int
481  const pcl::PointIndices &indices,
482  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
483 {
484  return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix));
485 }
486 
487 
488 template <typename PointT, typename Scalar> inline unsigned int
490  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
491  Eigen::Matrix<Scalar, 4, 1> &centroid)
492 {
493  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
494  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
495  std::size_t point_count;
496  if (cloud.is_dense)
497  {
498  point_count = cloud.size ();
499  // For each point in the cloud
500  for (const auto& point: cloud)
501  {
502  accu [0] += point.x * point.x;
503  accu [1] += point.x * point.y;
504  accu [2] += point.x * point.z;
505  accu [3] += point.y * point.y; // 4
506  accu [4] += point.y * point.z; // 5
507  accu [5] += point.z * point.z; // 8
508  accu [6] += point.x;
509  accu [7] += point.y;
510  accu [8] += point.z;
511  }
512  }
513  else
514  {
515  point_count = 0;
516  for (const auto& point: cloud)
517  {
518  if (!isFinite (point))
519  continue;
520 
521  accu [0] += point.x * point.x;
522  accu [1] += point.x * point.y;
523  accu [2] += point.x * point.z;
524  accu [3] += point.y * point.y;
525  accu [4] += point.y * point.z;
526  accu [5] += point.z * point.z;
527  accu [6] += point.x;
528  accu [7] += point.y;
529  accu [8] += point.z;
530  ++point_count;
531  }
532  }
533  accu /= static_cast<Scalar> (point_count);
534  if (point_count != 0)
535  {
536  //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
537  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
538  centroid[3] = 1;
539  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
540  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
541  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
542  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
543  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
544  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
545  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
546  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
547  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
548  }
549  return (static_cast<unsigned int> (point_count));
550 }
551 
552 
553 template <typename PointT, typename Scalar> inline unsigned int
555  const std::vector<int> &indices,
556  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
557  Eigen::Matrix<Scalar, 4, 1> &centroid)
558 {
559  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
560  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
561  std::size_t point_count;
562  if (cloud.is_dense)
563  {
564  point_count = indices.size ();
565  for (const int &index : indices)
566  {
567  //const PointT& point = cloud[*iIt];
568  accu [0] += cloud[index].x * cloud[index].x;
569  accu [1] += cloud[index].x * cloud[index].y;
570  accu [2] += cloud[index].x * cloud[index].z;
571  accu [3] += cloud[index].y * cloud[index].y;
572  accu [4] += cloud[index].y * cloud[index].z;
573  accu [5] += cloud[index].z * cloud[index].z;
574  accu [6] += cloud[index].x;
575  accu [7] += cloud[index].y;
576  accu [8] += cloud[index].z;
577  }
578  }
579  else
580  {
581  point_count = 0;
582  for (const int &index : indices)
583  {
584  if (!isFinite (cloud[index]))
585  continue;
586 
587  ++point_count;
588  accu [0] += cloud[index].x * cloud[index].x;
589  accu [1] += cloud[index].x * cloud[index].y;
590  accu [2] += cloud[index].x * cloud[index].z;
591  accu [3] += cloud[index].y * cloud[index].y; // 4
592  accu [4] += cloud[index].y * cloud[index].z; // 5
593  accu [5] += cloud[index].z * cloud[index].z; // 8
594  accu [6] += cloud[index].x;
595  accu [7] += cloud[index].y;
596  accu [8] += cloud[index].z;
597  }
598  }
599 
600  accu /= static_cast<Scalar> (point_count);
601  //Eigen::Vector3f vec = accu.tail<3> ();
602  //centroid.head<3> () = vec;//= accu.tail<3> ();
603  //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
604  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
605  centroid[3] = 1;
606  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
607  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
608  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
609  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
610  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
611  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
612  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
613  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
614  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
615 
616  return (static_cast<unsigned int> (point_count));
617 }
618 
619 
620 template <typename PointT, typename Scalar> inline unsigned int
622  const pcl::PointIndices &indices,
623  Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
624  Eigen::Matrix<Scalar, 4, 1> &centroid)
625 {
626  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
627 }
628 
629 
630 template <typename PointT, typename Scalar> void
632  const Eigen::Matrix<Scalar, 4, 1> &centroid,
633  pcl::PointCloud<PointT> &cloud_out,
634  int npts)
635 {
636  // Calculate the number of points if not given
637  if (npts == 0)
638  {
639  while (cloud_iterator.isValid ())
640  {
641  ++npts;
642  ++cloud_iterator;
643  }
644  cloud_iterator.reset ();
645  }
646 
647  int i = 0;
648  cloud_out.resize (npts);
649  // Subtract the centroid from cloud_in
650  while (cloud_iterator.isValid ())
651  {
652  cloud_out[i].x = cloud_iterator->x - centroid[0];
653  cloud_out[i].y = cloud_iterator->y - centroid[1];
654  cloud_out[i].z = cloud_iterator->z - centroid[2];
655  ++i;
656  ++cloud_iterator;
657  }
658  cloud_out.width = cloud_out.size ();
659  cloud_out.height = 1;
660 }
661 
662 
663 template <typename PointT, typename Scalar> void
665  const Eigen::Matrix<Scalar, 4, 1> &centroid,
666  pcl::PointCloud<PointT> &cloud_out)
667 {
668  cloud_out = cloud_in;
669 
670  // Subtract the centroid from cloud_in
671  for (auto& point: cloud_out)
672  {
673  point.x -= static_cast<float> (centroid[0]);
674  point.y -= static_cast<float> (centroid[1]);
675  point.z -= static_cast<float> (centroid[2]);
676  }
677 }
678 
679 
680 template <typename PointT, typename Scalar> void
682  const std::vector<int> &indices,
683  const Eigen::Matrix<Scalar, 4, 1> &centroid,
684  pcl::PointCloud<PointT> &cloud_out)
685 {
686  cloud_out.header = cloud_in.header;
687  cloud_out.is_dense = cloud_in.is_dense;
688  if (indices.size () == cloud_in.points.size ())
689  {
690  cloud_out.width = cloud_in.width;
691  cloud_out.height = cloud_in.height;
692  }
693  else
694  {
695  cloud_out.width = static_cast<std::uint32_t> (indices.size ());
696  cloud_out.height = 1;
697  }
698  cloud_out.resize (indices.size ());
699 
700  // Subtract the centroid from cloud_in
701  for (std::size_t i = 0; i < indices.size (); ++i)
702  {
703  cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
704  cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
705  cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]);
706  }
707 }
708 
709 
710 template <typename PointT, typename Scalar> void
712  const pcl::PointIndices& indices,
713  const Eigen::Matrix<Scalar, 4, 1> &centroid,
714  pcl::PointCloud<PointT> &cloud_out)
715 {
716  return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
717 }
718 
719 
720 template <typename PointT, typename Scalar> void
722  const Eigen::Matrix<Scalar, 4, 1> &centroid,
723  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out,
724  int npts)
725 {
726  // Calculate the number of points if not given
727  if (npts == 0)
728  {
729  while (cloud_iterator.isValid ())
730  {
731  ++npts;
732  ++cloud_iterator;
733  }
734  cloud_iterator.reset ();
735  }
736 
737  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
738 
739  int i = 0;
740  while (cloud_iterator.isValid ())
741  {
742  cloud_out (0, i) = cloud_iterator->x - centroid[0];
743  cloud_out (1, i) = cloud_iterator->y - centroid[1];
744  cloud_out (2, i) = cloud_iterator->z - centroid[2];
745  ++i;
746  ++cloud_iterator;
747  }
748 }
749 
750 
751 template <typename PointT, typename Scalar> void
753  const Eigen::Matrix<Scalar, 4, 1> &centroid,
754  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
755 {
756  std::size_t npts = cloud_in.size ();
757 
758  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
759 
760  for (std::size_t i = 0; i < npts; ++i)
761  {
762  cloud_out (0, i) = cloud_in[i].x - centroid[0];
763  cloud_out (1, i) = cloud_in[i].y - centroid[1];
764  cloud_out (2, i) = cloud_in[i].z - centroid[2];
765  // One column at a time
766  //cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
767  }
768 
769  // Make sure we zero the 4th dimension out (1 row, N columns)
770  //cloud_out.block (3, 0, 1, npts).setZero ();
771 }
772 
773 
774 template <typename PointT, typename Scalar> void
776  const std::vector<int> &indices,
777  const Eigen::Matrix<Scalar, 4, 1> &centroid,
778  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
779 {
780  std::size_t npts = indices.size ();
781 
782  cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned
783 
784  for (std::size_t i = 0; i < npts; ++i)
785  {
786  cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0];
787  cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1];
788  cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2];
789  // One column at a time
790  //cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid;
791  }
792 
793  // Make sure we zero the 4th dimension out (1 row, N columns)
794  //cloud_out.block (3, 0, 1, npts).setZero ();
795 }
796 
797 
798 template <typename PointT, typename Scalar> void
800  const pcl::PointIndices &indices,
801  const Eigen::Matrix<Scalar, 4, 1> &centroid,
802  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
803 {
804  return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
805 }
806 
807 
808 template <typename PointT, typename Scalar> inline void
810  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
811 {
812  using FieldList = typename pcl::traits::fieldList<PointT>::type;
813 
814  // Get the size of the fields
815  centroid.setZero (boost::mpl::size<FieldList>::value);
816 
817  if (cloud.empty ())
818  return;
819 
820  // Iterate over each point
821  for (const auto& pt: cloud)
822  {
823  // Iterate over each dimension
824  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (pt, centroid));
825  }
826  centroid /= static_cast<Scalar> (cloud.size ());
827 }
828 
829 
830 template <typename PointT, typename Scalar> inline void
832  const std::vector<int> &indices,
833  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
834 {
835  using FieldList = typename pcl::traits::fieldList<PointT>::type;
836 
837  // Get the size of the fields
838  centroid.setZero (boost::mpl::size<FieldList>::value);
839 
840  if (indices.empty ())
841  return;
842 
843  // Iterate over each point
844  for (const auto& index: indices)
845  {
846  // Iterate over each dimension
847  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[index], centroid));
848  }
849  centroid /= static_cast<Scalar> (indices.size ());
850 }
851 
852 
853 template <typename PointT, typename Scalar> inline void
855  const pcl::PointIndices &indices,
856  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
857 {
858  return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
859 }
860 
861 template <typename PointT> void
863 {
864  // Invoke add point on each accumulator
865  boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
866  ++num_points_;
867 }
868 
869 template <typename PointT>
870 template <typename PointOutT> void
871 CentroidPoint<PointT>::get (PointOutT& point) const
872 {
873  if (num_points_ != 0)
874  {
875  // Filter accumulators so that only those that are compatible with
876  // both PointT and requested point type remain
877  auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
878  // Invoke get point on each accumulator in filtered list
879  boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
880  }
881 }
882 
883 
884 template <typename PointInT, typename PointOutT> std::size_t
886  PointOutT& centroid)
887 {
889 
890  if (cloud.is_dense)
891  for (const auto& point: cloud)
892  cp.add (point);
893  else
894  for (const auto& point: cloud)
895  if (pcl::isFinite (point))
896  cp.add (point);
897 
898  cp.get (centroid);
899  return (cp.getSize ());
900 }
901 
902 
903 template <typename PointInT, typename PointOutT> std::size_t
905  const std::vector<int>& indices,
906  PointOutT& centroid)
907 {
909 
910  if (cloud.is_dense)
911  for (const int &index : indices)
912  cp.add (cloud[index]);
913  else
914  for (const int &index : indices)
915  if (pcl::isFinite (cloud[index]))
916  cp.add (cloud[index]);
917 
918  cp.get (centroid);
919  return (cp.getSize ());
920 }
921 
922 } // namespace pcl
923 
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
Iterator class for point clouds with or without given indices.
std::size_t getSize() const
Get the total number of points that were added.
Definition: centroid.h:1050
std::size_t size() const
Definition: point_cloud.h:448
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:885
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:489
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
Helper functor structure for n-D centroid estimation.
Definition: centroid.h:842
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void add(const PointT &point)
Add a new point to the centroid computation.
Definition: centroid.hpp:862
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:631
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
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:809
bool empty() const
Definition: point_cloud.h:450
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
A point structure representing Euclidean xyz coordinates, and the RGB color.
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
void get(PointOutT &point) const
Retrieve the current centroid.
Definition: centroid.hpp:871
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:455
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
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:418
A generic class that computes the centroid of points fed to it.
Definition: centroid.h:1022
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407