Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
centroid.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009-present, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id: centroid.hpp 6126 2012-07-03 20:19:58Z aichim $
35  *
36  */
37 
38 #ifndef PCL_COMMON_IMPL_CENTROID_H_
39 #define PCL_COMMON_IMPL_CENTROID_H_
40 
41 #include <pcl/ros/conversions.h>
42 #include <boost/mpl/size.hpp>
43 
45 template <typename PointT> inline unsigned int
46 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &centroid)
47 {
48  if (cloud.points.empty ())
49  return (0);
50 
51  // Initialize to 0
52  centroid.setZero ();
53  // For each point in the cloud
54  // If the data is dense, we don't need to check for NaN
55  if (cloud.is_dense)
56  {
57  for (size_t i = 0; i < cloud.points.size (); ++i)
58  centroid += cloud.points[i].getVector4fMap ();
59  centroid[3] = 0;
60  centroid /= static_cast<float> (cloud.points.size ());
61 
62  return (static_cast<unsigned int> (cloud.points.size ()));
63  }
64  // NaN or Inf values could exist => check for them
65  else
66  {
67  unsigned cp = 0;
68  for (size_t i = 0; i < cloud.points.size (); ++i)
69  {
70  // Check if the point is invalid
71  if (!isFinite (cloud [i]))
72  continue;
73 
74  centroid += cloud[i].getVector4fMap ();
75  ++cp;
76  }
77  centroid[3] = 0;
78  centroid /= static_cast<float> (cp);
79 
80  return (cp);
81  }
82 }
83 
85 template <typename PointT> inline unsigned int
86 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
87  Eigen::Vector4f &centroid)
88 {
89  if (indices.empty ())
90  return (0);
91 
92  // Initialize to 0
93  centroid.setZero ();
94  // If the data is dense, we don't need to check for NaN
95  if (cloud.is_dense)
96  {
97  for (size_t i = 0; i < indices.size (); ++i)
98  centroid += cloud.points[indices[i]].getVector4fMap ();
99  centroid[3] = 0;
100  centroid /= static_cast<float> (indices.size ());
101  return (static_cast<unsigned int> (indices.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 < indices.size (); ++i)
108  {
109  // Check if the point is invalid
110  if (!isFinite (cloud [indices[i]]))
111  continue;
112 
113  centroid += cloud[indices[i]].getVector4fMap ();
114  ++cp;
115  }
116  centroid[3] = 0.0f;
117  centroid /= static_cast<float> (cp);
118  return (cp);
119  }
120 }
121 
123 template <typename PointT> inline unsigned int
124 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
125  const pcl::PointIndices &indices, Eigen::Vector4f &centroid)
126 {
127  return (pcl::compute3DCentroid<PointT> (cloud, indices.indices, centroid));
128 }
129 
131 template <typename PointT> inline unsigned
132 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
133  const Eigen::Vector4f &centroid,
134  Eigen::Matrix3f &covariance_matrix)
135 {
136  if (cloud.points.empty ())
137  return (0);
138 
139  // Initialize to 0
140  covariance_matrix.setZero ();
141 
142  unsigned point_count;
143  // If the data is dense, we don't need to check for NaN
144  if (cloud.is_dense)
145  {
146  point_count = static_cast<unsigned> (cloud.size ());
147  // For each point in the cloud
148  for (size_t i = 0; i < point_count; ++i)
149  {
150  Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;
151 
152  covariance_matrix (1, 1) += pt.y () * pt.y ();
153  covariance_matrix (1, 2) += pt.y () * pt.z ();
154 
155  covariance_matrix (2, 2) += pt.z () * pt.z ();
156 
157  pt *= pt.x ();
158  covariance_matrix (0, 0) += pt.x ();
159  covariance_matrix (0, 1) += pt.y ();
160  covariance_matrix (0, 2) += pt.z ();
161  }
162  }
163  // NaN or Inf values could exist => check for them
164  else
165  {
166  point_count = 0;
167  // For each point in the cloud
168  for (size_t i = 0; i < cloud.size (); ++i)
169  {
170  // Check if the point is invalid
171  if (!isFinite (cloud [i]))
172  continue;
173 
174  Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;
175 
176  covariance_matrix (1, 1) += pt.y () * pt.y ();
177  covariance_matrix (1, 2) += pt.y () * pt.z ();
178 
179  covariance_matrix (2, 2) += pt.z () * pt.z ();
180 
181  pt *= pt.x ();
182  covariance_matrix (0, 0) += pt.x ();
183  covariance_matrix (0, 1) += pt.y ();
184  covariance_matrix (0, 2) += pt.z ();
185  ++point_count;
186  }
187  }
188  covariance_matrix (1, 0) = covariance_matrix (0, 1);
189  covariance_matrix (2, 0) = covariance_matrix (0, 2);
190  covariance_matrix (2, 1) = covariance_matrix (1, 2);
191 
192  return (point_count);
193 }
194 
196 template <typename PointT> inline unsigned int
197 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
198  const Eigen::Vector4f &centroid,
199  Eigen::Matrix3f &covariance_matrix)
200 {
201  unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
202  if (point_count != 0)
203  covariance_matrix /= static_cast<float> (point_count);
204  return (point_count);
205 }
206 
208 template <typename PointT> inline unsigned int
209 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
210  const std::vector<int> &indices,
211  const Eigen::Vector4f &centroid,
212  Eigen::Matrix3f &covariance_matrix)
213 {
214  if (indices.empty ())
215  return (0);
216 
217  // Initialize to 0
218  covariance_matrix.setZero ();
219 
220  size_t point_count;
221  // If the data is dense, we don't need to check for NaN
222  if (cloud.is_dense)
223  {
224  point_count = indices.size ();
225  // For each point in the cloud
226  for (size_t i = 0; i < point_count; ++i)
227  {
228  Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
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  }
240  }
241  // NaN or Inf values could exist => check for them
242  else
243  {
244  point_count = 0;
245  // For each point in the cloud
246  for (size_t i = 0; i < indices.size (); ++i)
247  {
248  // Check if the point is invalid
249  if (!isFinite (cloud[indices[i]]))
250  continue;
251 
252  Eigen::Vector4f pt = cloud[indices[i]].getVector4fMap () - centroid;
253 
254  covariance_matrix (1, 1) += pt.y () * pt.y ();
255  covariance_matrix (1, 2) += pt.y () * pt.z ();
256 
257  covariance_matrix (2, 2) += pt.z () * pt.z ();
258 
259  pt *= pt.x ();
260  covariance_matrix (0, 0) += pt.x ();
261  covariance_matrix (0, 1) += pt.y ();
262  covariance_matrix (0, 2) += pt.z ();
263  ++point_count;
264  }
265  }
266  covariance_matrix (1, 0) = covariance_matrix (0, 1);
267  covariance_matrix (2, 0) = covariance_matrix (0, 2);
268  covariance_matrix (2, 1) = covariance_matrix (1, 2);
269  return (static_cast<unsigned int> (point_count));
270 }
271 
273 template <typename PointT> inline unsigned int
274 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
275  const pcl::PointIndices &indices,
276  const Eigen::Vector4f &centroid,
277  Eigen::Matrix3f &covariance_matrix)
278 {
279  return (pcl::computeCovarianceMatrix<PointT> (cloud, indices.indices, centroid, covariance_matrix));
280 }
281 
283 template <typename PointT> inline unsigned int
284 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
285  const std::vector<int> &indices,
286  const Eigen::Vector4f &centroid,
287  Eigen::Matrix3f &covariance_matrix)
288 {
289  unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
290  if (point_count != 0)
291  covariance_matrix /= static_cast<float> (point_count);
292 
293  return (point_count);
294 }
295 
297 template <typename PointT> inline unsigned int
298 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
299  const pcl::PointIndices &indices,
300  const Eigen::Vector4f &centroid,
301  Eigen::Matrix3f &covariance_matrix)
302 {
303  unsigned int point_count = pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix);
304  if (point_count != 0)
305  covariance_matrix /= static_cast<int>(point_count);
306 
307  return point_count;
308 }
309 
311 template <typename PointT> inline unsigned int
312 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
313  Eigen::Matrix3f &covariance_matrix)
314 {
315  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
316  Eigen::Matrix<float, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 6, Eigen::RowMajor>::Zero ();
317 
318  unsigned int point_count;
319  if (cloud.is_dense)
320  {
321  point_count = static_cast<unsigned int> (cloud.size ());
322  // For each point in the cloud
323  for (size_t i = 0; i < point_count; ++i)
324  {
325  accu [0] += cloud[i].x * cloud[i].x;
326  accu [1] += cloud[i].x * cloud[i].y;
327  accu [2] += cloud[i].x * cloud[i].z;
328  accu [3] += cloud[i].y * cloud[i].y;
329  accu [4] += cloud[i].y * cloud[i].z;
330  accu [5] += cloud[i].z * cloud[i].z;
331  }
332  }
333  else
334  {
335  point_count = 0;
336  for (size_t i = 0; i < cloud.points.size (); ++i)
337  {
338  if (!isFinite (cloud[i]))
339  continue;
340 
341  accu [0] += cloud[i].x * cloud[i].x;
342  accu [1] += cloud[i].x * cloud[i].y;
343  accu [2] += cloud[i].x * cloud[i].z;
344  accu [3] += cloud[i].y * cloud[i].y;
345  accu [4] += cloud[i].y * cloud[i].z;
346  accu [5] += cloud[i].z * cloud[i].z;
347  ++point_count;
348  }
349  }
350 
351  if (point_count != 0)
352  {
353  accu /= static_cast<float> (point_count);
354  covariance_matrix.coeffRef (0) = accu [0];
355  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
356  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
357  covariance_matrix.coeffRef (4) = accu [3];
358  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
359  covariance_matrix.coeffRef (8) = accu [5];
360  }
361  return (point_count);
362 }
363 
365 template <typename PointT> inline unsigned int
366 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
367  const std::vector<int> &indices,
368  Eigen::Matrix3f &covariance_matrix)
369 {
370  Eigen::Matrix<float, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 6, Eigen::RowMajor>::Zero ();
371 
372  unsigned int point_count;
373  if (cloud.is_dense)
374  {
375  point_count = static_cast<unsigned int> (indices.size ());
376  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
377  {
378  //const PointT& point = cloud[*iIt];
379  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
380  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
381  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
382  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
383  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
384  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
385  }
386  }
387  else
388  {
389  point_count = 0;
390  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
391  {
392  if (!isFinite (cloud[*iIt]))
393  continue;
394 
395  ++point_count;
396  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
397  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
398  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
399  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
400  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
401  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
402  }
403  }
404  if (point_count != 0)
405  {
406  accu /= static_cast<float> (point_count);
407  covariance_matrix.coeffRef (0) = accu [0];
408  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
409  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
410  covariance_matrix.coeffRef (4) = accu [3];
411  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
412  covariance_matrix.coeffRef (8) = accu [5];
413  }
414  return (point_count);
415 }
416 
417 template <typename PointT> inline unsigned int
418 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
419  const pcl::PointIndices &indices,
420  Eigen::Matrix3f &covariance_matrix)
421 {
422  return computeCovarianceMatrix (cloud, indices.indices, covariance_matrix);
423 }
424 
426 template <typename PointT> inline unsigned int
427 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
428  Eigen::Matrix3d &covariance_matrix)
429 {
430  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
431  Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();
432 
433  unsigned int point_count;
434  if (cloud.is_dense)
435  {
436  point_count = cloud.size ();
437  // For each point in the cloud
438  for (size_t i = 0; i < point_count; ++i)
439  {
440  accu [0] += cloud[i].x * cloud[i].x;
441  accu [1] += cloud[i].x * cloud[i].y;
442  accu [2] += cloud[i].x * cloud[i].z;
443  accu [3] += cloud[i].y * cloud[i].y;
444  accu [4] += cloud[i].y * cloud[i].z;
445  accu [5] += cloud[i].z * cloud[i].z;
446  }
447  }
448  else
449  {
450  point_count = 0;
451  for (size_t i = 0; i < cloud.points.size (); ++i)
452  {
453  if (!isFinite (cloud[i]))
454  continue;
455 
456  accu [0] += cloud[i].x * cloud[i].x;
457  accu [1] += cloud[i].x * cloud[i].y;
458  accu [2] += cloud[i].x * cloud[i].z;
459  accu [3] += cloud[i].y * cloud[i].y;
460  accu [4] += cloud[i].y * cloud[i].z;
461  accu [5] += cloud[i].z * cloud[i].z;
462  ++point_count;
463  }
464  }
465 
466  if (point_count != 0)
467  {
468  accu /= static_cast<double> (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 
480 template <typename PointT> inline unsigned int
481 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
482  const std::vector<int> &indices,
483  Eigen::Matrix3d &covariance_matrix)
484 {
485  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
486  Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();
487 
488  unsigned int point_count;
489  if (cloud.is_dense)
490  {
491  point_count = indices.size ();
492  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
493  {
494  //const PointT& point = cloud[*iIt];
495  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
496  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
497  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
498  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
499  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
500  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
501  }
502  }
503  else
504  {
505  point_count = 0;
506  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
507  {
508  if (!isFinite (cloud[*iIt]))
509  continue;
510 
511  ++point_count;
512  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
513  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
514  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
515  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
516  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
517  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
518  }
519  }
520  if (point_count != 0)
521  {
522  accu /= static_cast<double> (point_count);
523  covariance_matrix.coeffRef (0) = accu [0];
524  covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
525  covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
526  covariance_matrix.coeffRef (4) = accu [3];
527  covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
528  covariance_matrix.coeffRef (8) = accu [5];
529  }
530  return (point_count);
531 }
532 
533 template <typename PointT> inline unsigned int
534 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
535  const pcl::PointIndices &indices,
536  Eigen::Matrix3d &covariance_matrix)
537 {
538  return computeCovarianceMatrix (cloud, indices.indices, covariance_matrix);
539 }
540 
542 template <typename PointT> inline unsigned int
543 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
544  Eigen::Matrix3f &covariance_matrix,
545  Eigen::Vector4f &centroid)
546 {
547  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
548  Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
549  size_t point_count;
550  if (cloud.is_dense)
551  {
552  point_count = cloud.size ();
553  // For each point in the cloud
554  for (size_t i = 0; i < point_count; ++i)
555  {
556  accu [0] += cloud[i].x * cloud[i].x;
557  accu [1] += cloud[i].x * cloud[i].y;
558  accu [2] += cloud[i].x * cloud[i].z;
559  accu [3] += cloud[i].y * cloud[i].y; // 4
560  accu [4] += cloud[i].y * cloud[i].z; // 5
561  accu [5] += cloud[i].z * cloud[i].z; // 8
562  accu [6] += cloud[i].x;
563  accu [7] += cloud[i].y;
564  accu [8] += cloud[i].z;
565  }
566  }
567  else
568  {
569  point_count = 0;
570  for (size_t i = 0; i < cloud.points.size (); ++i)
571  {
572  if (!isFinite (cloud[i]))
573  continue;
574 
575  accu [0] += cloud[i].x * cloud[i].x;
576  accu [1] += cloud[i].x * cloud[i].y;
577  accu [2] += cloud[i].x * cloud[i].z;
578  accu [3] += cloud[i].y * cloud[i].y;
579  accu [4] += cloud[i].y * cloud[i].z;
580  accu [5] += cloud[i].z * cloud[i].z;
581  accu [6] += cloud[i].x;
582  accu [7] += cloud[i].y;
583  accu [8] += cloud[i].z;
584  ++point_count;
585  }
586  }
587  accu /= static_cast<float> (point_count);
588  if (point_count != 0)
589  {
590  //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
591  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
592  centroid[3] = 0;
593  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
594  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
595  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
596  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
597  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
598  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
599  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
600  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
601  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
602  }
603  return (static_cast<unsigned int> (point_count));
604 }
605 
607 template <typename PointT> inline unsigned int
608 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
609  const std::vector<int> &indices,
610  Eigen::Matrix3f &covariance_matrix,
611  Eigen::Vector4f &centroid)
612 {
613  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
614  Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
615  size_t point_count;
616  if (cloud.is_dense)
617  {
618  point_count = indices.size ();
619  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
620  {
621  //const PointT& point = cloud[*iIt];
622  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
623  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
624  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
625  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
626  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
627  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
628  accu [6] += cloud[*iIt].x;
629  accu [7] += cloud[*iIt].y;
630  accu [8] += cloud[*iIt].z;
631  }
632  }
633  else
634  {
635  point_count = 0;
636  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
637  {
638  if (!isFinite (cloud[*iIt]))
639  continue;
640 
641  ++point_count;
642  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
643  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
644  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
645  accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4
646  accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5
647  accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8
648  accu [6] += cloud[*iIt].x;
649  accu [7] += cloud[*iIt].y;
650  accu [8] += cloud[*iIt].z;
651  }
652  }
653 
654  accu /= static_cast<float> (point_count);
655  //Eigen::Vector3f vec = accu.tail<3> ();
656  //centroid.head<3> () = vec;//= accu.tail<3> ();
657  //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0
658  centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
659  centroid[3] = 0;
660  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
661  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
662  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
663  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
664  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
665  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
666  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
667  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
668  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
669 
670  return (static_cast<unsigned int> (point_count));
671 }
672 
674 template <typename PointT> inline unsigned int
675 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
676  const pcl::PointIndices &indices,
677  Eigen::Matrix3f &covariance_matrix,
678  Eigen::Vector4f &centroid)
679 {
680  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
681 }
682 
684 template <typename PointT> inline unsigned int
685 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
686  Eigen::Matrix3d &covariance_matrix,
687  Eigen::Vector4d &centroid)
688 {
689  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
690  Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
691  unsigned int point_count;
692  if (cloud.is_dense)
693  {
694  point_count = cloud.size ();
695  // For each point in the cloud
696  for (size_t i = 0; i < point_count; ++i)
697  {
698  accu [0] += cloud[i].x * cloud[i].x;
699  accu [1] += cloud[i].x * cloud[i].y;
700  accu [2] += cloud[i].x * cloud[i].z;
701  accu [3] += cloud[i].y * cloud[i].y;
702  accu [4] += cloud[i].y * cloud[i].z;
703  accu [5] += cloud[i].z * cloud[i].z;
704  accu [6] += cloud[i].x;
705  accu [7] += cloud[i].y;
706  accu [8] += cloud[i].z;
707  }
708  }
709  else
710  {
711  point_count = 0;
712  for (size_t i = 0; i < cloud.points.size (); ++i)
713  {
714  if (!isFinite (cloud[i]))
715  continue;
716 
717  accu [0] += cloud[i].x * cloud[i].x;
718  accu [1] += cloud[i].x * cloud[i].y;
719  accu [2] += cloud[i].x * cloud[i].z;
720  accu [3] += cloud[i].y * cloud[i].y;
721  accu [4] += cloud[i].y * cloud[i].z;
722  accu [5] += cloud[i].z * cloud[i].z;
723  accu [6] += cloud[i].x;
724  accu [7] += cloud[i].y;
725  accu [8] += cloud[i].z;
726  ++point_count;
727  }
728  }
729 
730  if (point_count != 0)
731  {
732  accu /= static_cast<double> (point_count);
733  centroid.head<3> () = accu.tail<3> ();
734  centroid[3] = 0;
735  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
736  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
737  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
738  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
739  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
740  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
741  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
742  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
743  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
744  }
745  return (point_count);
746 }
747 
749 template <typename PointT> inline unsigned int
750 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
751  const std::vector<int> &indices,
752  Eigen::Matrix3d &covariance_matrix,
753  Eigen::Vector4d &centroid)
754 {
755  // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
756  Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
757  unsigned point_count;
758  if (cloud.is_dense)
759  {
760  point_count = indices.size ();
761  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
762  {
763  //const PointT& point = cloud[*iIt];
764  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
765  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
766  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
767  accu [3] += cloud[*iIt].y * cloud[*iIt].y;
768  accu [4] += cloud[*iIt].y * cloud[*iIt].z;
769  accu [5] += cloud[*iIt].z * cloud[*iIt].z;
770  accu [6] += cloud[*iIt].x;
771  accu [7] += cloud[*iIt].y;
772  accu [8] += cloud[*iIt].z;
773  }
774  }
775  else
776  {
777  point_count = 0;
778  for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
779  {
780  if (!isFinite (cloud[*iIt]))
781  continue;
782 
783  ++point_count;
784  accu [0] += cloud[*iIt].x * cloud[*iIt].x;
785  accu [1] += cloud[*iIt].x * cloud[*iIt].y;
786  accu [2] += cloud[*iIt].x * cloud[*iIt].z;
787  accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4
788  accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5
789  accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8
790  accu [6] += cloud[*iIt].x;
791  accu [7] += cloud[*iIt].y;
792  accu [8] += cloud[*iIt].z;
793  }
794  }
795 
796  if (point_count != 0)
797  {
798  accu /= static_cast<double> (point_count);
799  Eigen::Vector3f vec = accu.tail<3> ();
800  centroid.head<3> () = vec;//= accu.tail<3> ();
801  centroid[3] = 0;
802  covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
803  covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
804  covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
805  covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
806  covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
807  covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
808  covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
809  covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
810  covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
811  }
812  return (point_count);
813 }
814 
816 template <typename PointT> inline unsigned int
817 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
818  const pcl::PointIndices &indices,
819  Eigen::Matrix3d &covariance_matrix,
820  Eigen::Vector4d &centroid)
821 {
822  return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
823 }
825 template <typename PointT> void
826 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
827  const Eigen::Vector4f &centroid,
828  pcl::PointCloud<PointT> &cloud_out)
829 {
830  cloud_out = cloud_in;
831 
832  // Subtract the centroid from cloud_in
833  for (size_t i = 0; i < cloud_in.points.size (); ++i)
834  cloud_out.points[i].getVector4fMap () -= centroid;
835 }
836 
838 template <typename PointT> void
839 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
840  const std::vector<int> &indices,
841  const Eigen::Vector4f &centroid,
842  pcl::PointCloud<PointT> &cloud_out)
843 {
844  cloud_out.header = cloud_in.header;
845  cloud_out.is_dense = cloud_in.is_dense;
846  if (indices.size () == cloud_in.points.size ())
847  {
848  cloud_out.width = cloud_in.width;
849  cloud_out.height = cloud_in.height;
850  }
851  else
852  {
853  cloud_out.width = static_cast<uint32_t> (indices.size ());
854  cloud_out.height = 1;
855  }
856  cloud_out.points.resize (indices.size ());
857 
858  // Subtract the centroid from cloud_in
859  for (size_t i = 0; i < indices.size (); ++i)
860  cloud_out.points[i].getVector4fMap () = cloud_in.points[indices[i]].getVector4fMap () -
861  centroid;
862 }
863 
865 template <typename PointT> void
866 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
867  const Eigen::Vector4f &centroid,
868  Eigen::MatrixXf &cloud_out)
869 {
870  size_t npts = cloud_in.points.size ();
871 
872  cloud_out = Eigen::MatrixXf::Zero (4, npts); // keep the data aligned
873 
874  for (size_t i = 0; i < npts; ++i)
875  // One column at a time
876  cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
877 
878  // Make sure we zero the 4th dimension out (1 row, N columns)
879  cloud_out.block (3, 0, 1, npts).setZero ();
880 }
881 
883 template <typename PointT> void
884 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
885  const std::vector<int> &indices,
886  const Eigen::Vector4f &centroid,
887  Eigen::MatrixXf &cloud_out)
888 {
889  size_t npts = indices.size ();
890 
891  cloud_out = Eigen::MatrixXf::Zero (4, npts); // keep the data aligned
892 
893  for (size_t i = 0; i < npts; ++i)
894  // One column at a time
895  cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid;
896 
897  // Make sure we zero the 4th dimension out (1 row, N columns)
898  cloud_out.block (3, 0, 1, npts).setZero ();
899 }
900 
902 template <typename PointT> void
903 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
904  const pcl::PointIndices &indices,
905  const Eigen::Vector4f &centroid,
906  Eigen::MatrixXf &cloud_out)
907 {
908  return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
909 }
910 
912 template <typename PointT> inline void
913 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf &centroid)
914 {
915  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
916 
917  // Get the size of the fields
918  centroid.setZero (boost::mpl::size<FieldList>::value);
919 
920  if (cloud.points.empty ())
921  return;
922  // Iterate over each point
923  int size = static_cast<int> (cloud.points.size ());
924  for (int i = 0; i < size; ++i)
925  {
926  // Iterate over each dimension
927  pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[i], centroid));
928  }
929  centroid /= static_cast<float> (size);
930 }
931 
933 template <typename PointT> inline void
934 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
935  Eigen::VectorXf &centroid)
936 {
937  typedef typename pcl::traits::fieldList<PointT>::type FieldList;
938 
939  // Get the size of the fields
940  centroid.setZero (boost::mpl::size<FieldList>::value);
941 
942  if (indices.empty ())
943  return;
944  // Iterate over each point
945  int nr_points = static_cast<int> (indices.size ());
946  for (int i = 0; i < nr_points; ++i)
947  {
948  // Iterate over each dimension
949  pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[indices[i]], centroid));
950  }
951  centroid /= static_cast<float> (nr_points);
952 }
953 
955 template <typename PointT> inline void
956 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
957  const pcl::PointIndices &indices, Eigen::VectorXf &centroid)
958 {
959  return (pcl::computeNDCentroid<PointT> (cloud, indices.indices, centroid));
960 }
961 
962 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_
963 
Generated on Fri Mar 8 2013 12:50:44 for Point Cloud Library (PCL) by   doxygen 1.8.3.1