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