Point Cloud Library (PCL)  1.8.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  else
104  {
105  unsigned cp = 0;
106  for (size_t i = 0; i < cloud.size (); ++i)
107  {
108  // Check if the point is invalid
109  if (!isFinite (cloud [i]))
110  continue;
111 
112  centroid[0] += cloud[i].x;
113  centroid[1] += cloud[i].y;
114  centroid[2] += cloud[i].z;
115  ++cp;
116  }
117  centroid /= static_cast<Scalar> (cp);
118  centroid[3] = 1;
119 
120  return (cp);
121  }
122 }
123 
124 ///////////////////////////////////////////////////////////////////////////////////////////
125 template <typename PointT, typename Scalar> inline unsigned int
127  const std::vector<int> &indices,
128  Eigen::Matrix<Scalar, 4, 1> &centroid)
129 {
130  if (indices.empty ())
131  return (0);
132 
133  // Initialize to 0
134  centroid.setZero ();
135  // If the data is dense, we don't need to check for NaN
136  if (cloud.is_dense)
137  {
138  for (size_t i = 0; i < indices.size (); ++i)
139  {
140  centroid[0] += cloud[indices[i]].x;
141  centroid[1] += cloud[indices[i]].y;
142  centroid[2] += cloud[indices[i]].z;
143  }
144  centroid /= static_cast<Scalar> (indices.size ());
145  centroid[3] = 1;
146  return (static_cast<unsigned int> (indices.size ()));
147  }
148  // NaN or Inf values could exist => check for them
149  else
150  {
151  unsigned cp = 0;
152  for (size_t i = 0; i < indices.size (); ++i)
153  {
154  // Check if the point is invalid
155  if (!isFinite (cloud [indices[i]]))
156  continue;
157 
158  centroid[0] += cloud[indices[i]].x;
159  centroid[1] += cloud[indices[i]].y;
160  centroid[2] += cloud[indices[i]].z;
161  ++cp;
162  }
163  centroid /= static_cast<Scalar> (cp);
164  centroid[3] = 1;
165  return (cp);
166  }
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 (size_t i = 0; i < point_count; ++i)
197  {
198  Eigen::Matrix<Scalar, 4, 1> pt;
199  pt[0] = cloud[i].x - centroid[0];
200  pt[1] = cloud[i].y - centroid[1];
201  pt[2] = cloud[i].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 (size_t i = 0; i < cloud.size (); ++i)
220  {
221  // Check if the point is invalid
222  if (!isFinite (cloud [i]))
223  continue;
224 
225  Eigen::Matrix<Scalar, 4, 1> pt;
226  pt[0] = cloud[i].x - centroid[0];
227  pt[1] = cloud[i].y - centroid[1];
228  pt[2] = cloud[i].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  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 (size_t i = 0; i < point_count; ++i)
281  {
282  Eigen::Matrix<Scalar, 4, 1> pt;
283  pt[0] = cloud[indices[i]].x - centroid[0];
284  pt[1] = cloud[indices[i]].y - centroid[1];
285  pt[2] = cloud[indices[i]].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 (size_t i = 0; i < indices.size (); ++i)
304  {
305  // Check if the point is invalid
306  if (!isFinite (cloud[indices[i]]))
307  continue;
308 
309  Eigen::Matrix<Scalar, 4, 1> pt;
310  pt[0] = cloud[indices[i]].x - centroid[0];
311  pt[1] = cloud[indices[i]].y - centroid[1];
312  pt[2] = cloud[indices[i]].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 (size_t i = 0; i < point_count; ++i)
384  {
385  accu [0] += cloud[i].x * cloud[i].x;
386  accu [1] += cloud[i].x * cloud[i].y;
387  accu [2] += cloud[i].x * cloud[i].z;
388  accu [3] += cloud[i].y * cloud[i].y;
389  accu [4] += cloud[i].y * cloud[i].z;
390  accu [5] += cloud[i].z * cloud[i].z;
391  }
392  }
393  else
394  {
395  point_count = 0;
396  for (size_t i = 0; i < cloud.size (); ++i)
397  {
398  if (!isFinite (cloud[i]))
399  continue;
400 
401  accu [0] += cloud[i].x * cloud[i].x;
402  accu [1] += cloud[i].x * cloud[i].y;
403  accu [2] += cloud[i].x * cloud[i].z;
404  accu [3] += cloud[i].y * cloud[i].y;
405  accu [4] += cloud[i].y * cloud[i].z;
406  accu [5] += cloud[i].z * cloud[i].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 (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
438  {
439  //const PointT& point = cloud[*iIt];
440  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
441  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
442  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
443  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
444  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
445  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
446  }
447  }
448  else
449  {
450  point_count = 0;
451  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
452  {
453  if (!isFinite (cloud[*iIt]))
454  continue;
455 
456  ++point_count;
457  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
458  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
459  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
460  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
461  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
462  accu [5] += cloud[*iIt].z * cloud[*iIt].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  size_t point_count;
496  if (cloud.is_dense)
497  {
498  point_count = cloud.size ();
499  // For each point in the cloud
500  for (size_t i = 0; i < point_count; ++i)
501  {
502  accu [0] += cloud[i].x * cloud[i].x;
503  accu [1] += cloud[i].x * cloud[i].y;
504  accu [2] += cloud[i].x * cloud[i].z;
505  accu [3] += cloud[i].y * cloud[i].y; // 4
506  accu [4] += cloud[i].y * cloud[i].z; // 5
507  accu [5] += cloud[i].z * cloud[i].z; // 8
508  accu [6] += cloud[i].x;
509  accu [7] += cloud[i].y;
510  accu [8] += cloud[i].z;
511  }
512  }
513  else
514  {
515  point_count = 0;
516  for (size_t i = 0; i < cloud.size (); ++i)
517  {
518  if (!isFinite (cloud[i]))
519  continue;
520 
521  accu [0] += cloud[i].x * cloud[i].x;
522  accu [1] += cloud[i].x * cloud[i].y;
523  accu [2] += cloud[i].x * cloud[i].z;
524  accu [3] += cloud[i].y * cloud[i].y;
525  accu [4] += cloud[i].y * cloud[i].z;
526  accu [5] += cloud[i].z * cloud[i].z;
527  accu [6] += cloud[i].x;
528  accu [7] += cloud[i].y;
529  accu [8] += cloud[i].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  size_t point_count;
562  if (cloud.is_dense)
563  {
564  point_count = indices.size ();
565  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
566  {
567  //const PointT& point = cloud[*iIt];
568  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
569  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
570  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
571  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
572  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
573  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
574  accu [6] += cloud[*iIt].x;
575  accu [7] += cloud[*iIt].y;
576  accu [8] += cloud[*iIt].z;
577  }
578  }
579  else
580  {
581  point_count = 0;
582  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
583  {
584  if (!isFinite (cloud[*iIt]))
585  continue;
586 
587  ++point_count;
588  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
589  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
590  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
591  accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4
592  accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5
593  accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8
594  accu [6] += cloud[*iIt].x;
595  accu [7] += cloud[*iIt].y;
596  accu [8] += cloud[*iIt].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 (size_t i = 0; i < cloud_in.points.size (); ++i)
672  {
673  cloud_out[i].x -= static_cast<float> (centroid[0]);
674  cloud_out[i].y -= static_cast<float> (centroid[1]);
675  cloud_out[i].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<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 (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  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 (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  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 (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  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
813 
814  // Get the size of the fields
815  centroid.setZero (boost::mpl::size<FieldList>::value);
816 
817  if (cloud.empty ())
818  return;
819  // Iterate over each point
820  int size = static_cast<int> (cloud.size ());
821  for (int i = 0; i < size; ++i)
822  {
823  // Iterate over each dimension
824  pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[i], centroid));
825  }
826  centroid /= static_cast<Scalar> (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  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
836 
837  // Get the size of the fields
838  centroid.setZero (boost::mpl::size<FieldList>::value);
839 
840  if (indices.empty ())
841  return;
842  // Iterate over each point
843  int nr_points = static_cast<int> (indices.size ());
844  for (int i = 0; i < nr_points; ++i)
845  {
846  // Iterate over each dimension
847  pcl::for_each_type <FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[indices[i]], centroid));
848  }
849  centroid /= static_cast<Scalar> (nr_points);
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 /////////////////////////////////////////////////////////////////////////////////////////////
862 template <typename PointInT, typename PointOutT> size_t
864  PointOutT& centroid)
865 {
867 
868  if (cloud.is_dense)
869  for (size_t i = 0; i < cloud.size (); ++i)
870  cp.add (cloud[i]);
871  else
872  for (size_t i = 0; i < cloud.size (); ++i)
873  if (pcl::isFinite (cloud[i]))
874  cp.add (cloud[i]);
875 
876  cp.get (centroid);
877  return (cp.getSize ());
878 }
879 
880 /////////////////////////////////////////////////////////////////////////////////////////////
881 template <typename PointInT, typename PointOutT> size_t
883  const std::vector<int>& indices,
884  PointOutT& centroid)
885 {
887 
888  if (cloud.is_dense)
889  for (size_t i = 0; i < indices.size (); ++i)
890  cp.add (cloud[indices[i]]);
891  else
892  for (size_t i = 0; i < indices.size (); ++i)
893  if (pcl::isFinite (cloud[indices[i]]))
894  cp.add (cloud[indices[i]]);
895 
896  cp.get (centroid);
897  return (cp.getSize ());
898 }
899 
900 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_
901 
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:54
Iterator class for point clouds with or without given indices.
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:863
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::vector< int > indices
Definition: PointIndices.h:19
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:841
size_t getSize() const
Get the total number of points that were added.
Definition: centroid.h:1083
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
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.h:1052
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
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
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.
bool empty() const
Definition: point_cloud.h:450
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
size_t size() const
Definition: point_cloud.h:448
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.h:1069
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:1037
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415