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