00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_COMMON_IMPL_CENTROID_H_
00039 #define PCL_COMMON_IMPL_CENTROID_H_
00040
00041 #include <pcl/ros/conversions.h>
00042 #include <boost/mpl/size.hpp>
00043
00045 template <typename PointT> inline unsigned int
00046 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f ¢roid)
00047 {
00048 if (cloud.points.empty ())
00049 return (0);
00050
00051
00052 centroid.setZero ();
00053
00054
00055 if (cloud.is_dense)
00056 {
00057 for (size_t i = 0; i < cloud.points.size (); ++i)
00058 centroid += cloud.points[i].getVector4fMap ();
00059 centroid[3] = 0;
00060 centroid /= static_cast<float> (cloud.points.size ());
00061
00062 return (static_cast<unsigned int> (cloud.points.size ()));
00063 }
00064
00065 else
00066 {
00067 unsigned cp = 0;
00068 for (size_t i = 0; i < cloud.points.size (); ++i)
00069 {
00070
00071 if (!isFinite (cloud [i]))
00072 continue;
00073
00074 centroid += cloud[i].getVector4fMap ();
00075 ++cp;
00076 }
00077 centroid[3] = 0;
00078 centroid /= static_cast<float> (cp);
00079
00080 return (cp);
00081 }
00082 }
00083
00085 template <typename PointT> inline unsigned int
00086 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00087 Eigen::Vector4f ¢roid)
00088 {
00089 if (indices.empty ())
00090 return (0);
00091
00092
00093 centroid.setZero ();
00094
00095 if (cloud.is_dense)
00096 {
00097 for (size_t i = 0; i < indices.size (); ++i)
00098 centroid += cloud.points[indices[i]].getVector4fMap ();
00099 centroid[3] = 0;
00100 centroid /= static_cast<float> (indices.size ());
00101 return (static_cast<unsigned int> (indices.size ()));
00102 }
00103
00104 else
00105 {
00106 unsigned cp = 0;
00107 for (size_t i = 0; i < indices.size (); ++i)
00108 {
00109
00110 if (!isFinite (cloud [indices[i]]))
00111 continue;
00112
00113 centroid += cloud[indices[i]].getVector4fMap ();
00114 ++cp;
00115 }
00116 centroid[3] = 0.0f;
00117 centroid /= static_cast<float> (cp);
00118 return (cp);
00119 }
00120 }
00121
00123 template <typename PointT> inline unsigned int
00124 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
00125 const pcl::PointIndices &indices, Eigen::Vector4f ¢roid)
00126 {
00127 return (pcl::compute3DCentroid<PointT> (cloud, indices.indices, centroid));
00128 }
00129
00131 template <typename PointT> inline unsigned
00132 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00133 const Eigen::Vector4f ¢roid,
00134 Eigen::Matrix3f &covariance_matrix)
00135 {
00136 if (cloud.points.empty ())
00137 return (0);
00138
00139
00140 covariance_matrix.setZero ();
00141
00142 unsigned point_count;
00143
00144 if (cloud.is_dense)
00145 {
00146 point_count = static_cast<unsigned> (cloud.size ());
00147
00148 for (size_t i = 0; i < point_count; ++i)
00149 {
00150 Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;
00151
00152 covariance_matrix (1, 1) += pt.y () * pt.y ();
00153 covariance_matrix (1, 2) += pt.y () * pt.z ();
00154
00155 covariance_matrix (2, 2) += pt.z () * pt.z ();
00156
00157 pt *= pt.x ();
00158 covariance_matrix (0, 0) += pt.x ();
00159 covariance_matrix (0, 1) += pt.y ();
00160 covariance_matrix (0, 2) += pt.z ();
00161 }
00162 }
00163
00164 else
00165 {
00166 point_count = 0;
00167
00168 for (size_t i = 0; i < cloud.size (); ++i)
00169 {
00170
00171 if (!isFinite (cloud [i]))
00172 continue;
00173
00174 Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;
00175
00176 covariance_matrix (1, 1) += pt.y () * pt.y ();
00177 covariance_matrix (1, 2) += pt.y () * pt.z ();
00178
00179 covariance_matrix (2, 2) += pt.z () * pt.z ();
00180
00181 pt *= pt.x ();
00182 covariance_matrix (0, 0) += pt.x ();
00183 covariance_matrix (0, 1) += pt.y ();
00184 covariance_matrix (0, 2) += pt.z ();
00185 ++point_count;
00186 }
00187 }
00188 covariance_matrix (1, 0) = covariance_matrix (0, 1);
00189 covariance_matrix (2, 0) = covariance_matrix (0, 2);
00190 covariance_matrix (2, 1) = covariance_matrix (1, 2);
00191
00192 return (point_count);
00193 }
00194
00196 template <typename PointT> inline unsigned int
00197 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00198 const Eigen::Vector4f ¢roid,
00199 Eigen::Matrix3f &covariance_matrix)
00200 {
00201 unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix);
00202 if (point_count != 0)
00203 covariance_matrix /= static_cast<float> (point_count);
00204 return (point_count);
00205 }
00206
00208 template <typename PointT> inline unsigned int
00209 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00210 const std::vector<int> &indices,
00211 const Eigen::Vector4f ¢roid,
00212 Eigen::Matrix3f &covariance_matrix)
00213 {
00214 if (indices.empty ())
00215 return (0);
00216
00217
00218 covariance_matrix.setZero ();
00219
00220 size_t point_count;
00221
00222 if (cloud.is_dense)
00223 {
00224 point_count = indices.size ();
00225
00226 for (size_t i = 0; i < point_count; ++i)
00227 {
00228 Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
00229
00230 covariance_matrix (1, 1) += pt.y () * pt.y ();
00231 covariance_matrix (1, 2) += pt.y () * pt.z ();
00232
00233 covariance_matrix (2, 2) += pt.z () * pt.z ();
00234
00235 pt *= pt.x ();
00236 covariance_matrix (0, 0) += pt.x ();
00237 covariance_matrix (0, 1) += pt.y ();
00238 covariance_matrix (0, 2) += pt.z ();
00239 }
00240 }
00241
00242 else
00243 {
00244 point_count = 0;
00245
00246 for (size_t i = 0; i < indices.size (); ++i)
00247 {
00248
00249 if (!isFinite (cloud[indices[i]]))
00250 continue;
00251
00252 Eigen::Vector4f pt = cloud[indices[i]].getVector4fMap () - centroid;
00253
00254 covariance_matrix (1, 1) += pt.y () * pt.y ();
00255 covariance_matrix (1, 2) += pt.y () * pt.z ();
00256
00257 covariance_matrix (2, 2) += pt.z () * pt.z ();
00258
00259 pt *= pt.x ();
00260 covariance_matrix (0, 0) += pt.x ();
00261 covariance_matrix (0, 1) += pt.y ();
00262 covariance_matrix (0, 2) += pt.z ();
00263 ++point_count;
00264 }
00265 }
00266 covariance_matrix (1, 0) = covariance_matrix (0, 1);
00267 covariance_matrix (2, 0) = covariance_matrix (0, 2);
00268 covariance_matrix (2, 1) = covariance_matrix (1, 2);
00269 return (static_cast<unsigned int> (point_count));
00270 }
00271
00273 template <typename PointT> inline unsigned int
00274 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00275 const pcl::PointIndices &indices,
00276 const Eigen::Vector4f ¢roid,
00277 Eigen::Matrix3f &covariance_matrix)
00278 {
00279 return (pcl::computeCovarianceMatrix<PointT> (cloud, indices.indices, centroid, covariance_matrix));
00280 }
00281
00283 template <typename PointT> inline unsigned int
00284 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00285 const std::vector<int> &indices,
00286 const Eigen::Vector4f ¢roid,
00287 Eigen::Matrix3f &covariance_matrix)
00288 {
00289 unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix);
00290 if (point_count != 0)
00291 covariance_matrix /= static_cast<float> (point_count);
00292
00293 return (point_count);
00294 }
00295
00297 template <typename PointT> inline unsigned int
00298 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud,
00299 const pcl::PointIndices &indices,
00300 const Eigen::Vector4f ¢roid,
00301 Eigen::Matrix3f &covariance_matrix)
00302 {
00303 unsigned int point_count = pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix);
00304 if (point_count != 0)
00305 covariance_matrix /= static_cast<int>(point_count);
00306
00307 return point_count;
00308 }
00309
00311 template <typename PointT> inline unsigned int
00312 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00313 Eigen::Matrix3f &covariance_matrix)
00314 {
00315
00316 Eigen::Matrix<float, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 6, Eigen::RowMajor>::Zero ();
00317
00318 unsigned int point_count;
00319 if (cloud.is_dense)
00320 {
00321 point_count = static_cast<unsigned int> (cloud.size ());
00322
00323 for (size_t i = 0; i < point_count; ++i)
00324 {
00325 accu [0] += cloud[i].x * cloud[i].x;
00326 accu [1] += cloud[i].x * cloud[i].y;
00327 accu [2] += cloud[i].x * cloud[i].z;
00328 accu [3] += cloud[i].y * cloud[i].y;
00329 accu [4] += cloud[i].y * cloud[i].z;
00330 accu [5] += cloud[i].z * cloud[i].z;
00331 }
00332 }
00333 else
00334 {
00335 point_count = 0;
00336 for (size_t i = 0; i < cloud.points.size (); ++i)
00337 {
00338 if (!isFinite (cloud[i]))
00339 continue;
00340
00341 accu [0] += cloud[i].x * cloud[i].x;
00342 accu [1] += cloud[i].x * cloud[i].y;
00343 accu [2] += cloud[i].x * cloud[i].z;
00344 accu [3] += cloud[i].y * cloud[i].y;
00345 accu [4] += cloud[i].y * cloud[i].z;
00346 accu [5] += cloud[i].z * cloud[i].z;
00347 ++point_count;
00348 }
00349 }
00350
00351 if (point_count != 0)
00352 {
00353 accu /= static_cast<float> (point_count);
00354 covariance_matrix.coeffRef (0) = accu [0];
00355 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
00356 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
00357 covariance_matrix.coeffRef (4) = accu [3];
00358 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
00359 covariance_matrix.coeffRef (8) = accu [5];
00360 }
00361 return (point_count);
00362 }
00363
00365 template <typename PointT> inline unsigned int
00366 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00367 const std::vector<int> &indices,
00368 Eigen::Matrix3f &covariance_matrix)
00369 {
00370 Eigen::Matrix<float, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 6, Eigen::RowMajor>::Zero ();
00371
00372 unsigned int point_count;
00373 if (cloud.is_dense)
00374 {
00375 point_count = static_cast<unsigned int> (indices.size ());
00376 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00377 {
00378
00379 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00380 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00381 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00382 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00383 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00384 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00385 }
00386 }
00387 else
00388 {
00389 point_count = 0;
00390 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00391 {
00392 if (!isFinite (cloud[*iIt]))
00393 continue;
00394
00395 ++point_count;
00396 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00397 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00398 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00399 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00400 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00401 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00402 }
00403 }
00404 if (point_count != 0)
00405 {
00406 accu /= static_cast<float> (point_count);
00407 covariance_matrix.coeffRef (0) = accu [0];
00408 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
00409 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
00410 covariance_matrix.coeffRef (4) = accu [3];
00411 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
00412 covariance_matrix.coeffRef (8) = accu [5];
00413 }
00414 return (point_count);
00415 }
00416
00417 template <typename PointT> inline unsigned int
00418 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00419 const pcl::PointIndices &indices,
00420 Eigen::Matrix3f &covariance_matrix)
00421 {
00422 return computeCovarianceMatrix (cloud, indices.indices, covariance_matrix);
00423 }
00424
00426 template <typename PointT> inline unsigned int
00427 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00428 Eigen::Matrix3d &covariance_matrix)
00429 {
00430
00431 Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();
00432
00433 unsigned int point_count;
00434 if (cloud.is_dense)
00435 {
00436 point_count = cloud.size ();
00437
00438 for (size_t i = 0; i < point_count; ++i)
00439 {
00440 accu [0] += cloud[i].x * cloud[i].x;
00441 accu [1] += cloud[i].x * cloud[i].y;
00442 accu [2] += cloud[i].x * cloud[i].z;
00443 accu [3] += cloud[i].y * cloud[i].y;
00444 accu [4] += cloud[i].y * cloud[i].z;
00445 accu [5] += cloud[i].z * cloud[i].z;
00446 }
00447 }
00448 else
00449 {
00450 point_count = 0;
00451 for (size_t i = 0; i < cloud.points.size (); ++i)
00452 {
00453 if (!isFinite (cloud[i]))
00454 continue;
00455
00456 accu [0] += cloud[i].x * cloud[i].x;
00457 accu [1] += cloud[i].x * cloud[i].y;
00458 accu [2] += cloud[i].x * cloud[i].z;
00459 accu [3] += cloud[i].y * cloud[i].y;
00460 accu [4] += cloud[i].y * cloud[i].z;
00461 accu [5] += cloud[i].z * cloud[i].z;
00462 ++point_count;
00463 }
00464 }
00465
00466 if (point_count != 0)
00467 {
00468 accu /= static_cast<double> (point_count);
00469 covariance_matrix.coeffRef (0) = accu [0];
00470 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
00471 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
00472 covariance_matrix.coeffRef (4) = accu [3];
00473 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
00474 covariance_matrix.coeffRef (8) = accu [5];
00475 }
00476 return (point_count);
00477 }
00478
00480 template <typename PointT> inline unsigned int
00481 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00482 const std::vector<int> &indices,
00483 Eigen::Matrix3d &covariance_matrix)
00484 {
00485
00486 Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();
00487
00488 unsigned int point_count;
00489 if (cloud.is_dense)
00490 {
00491 point_count = indices.size ();
00492 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00493 {
00494
00495 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00496 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00497 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00498 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00499 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00500 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00501 }
00502 }
00503 else
00504 {
00505 point_count = 0;
00506 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00507 {
00508 if (!isFinite (cloud[*iIt]))
00509 continue;
00510
00511 ++point_count;
00512 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00513 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00514 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00515 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00516 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00517 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00518 }
00519 }
00520 if (point_count != 0)
00521 {
00522 accu /= static_cast<double> (point_count);
00523 covariance_matrix.coeffRef (0) = accu [0];
00524 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
00525 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
00526 covariance_matrix.coeffRef (4) = accu [3];
00527 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
00528 covariance_matrix.coeffRef (8) = accu [5];
00529 }
00530 return (point_count);
00531 }
00532
00533 template <typename PointT> inline unsigned int
00534 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00535 const pcl::PointIndices &indices,
00536 Eigen::Matrix3d &covariance_matrix)
00537 {
00538 return computeCovarianceMatrix (cloud, indices.indices, covariance_matrix);
00539 }
00540
00542 template <typename PointT> inline unsigned int
00543 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00544 Eigen::Matrix3f &covariance_matrix,
00545 Eigen::Vector4f ¢roid)
00546 {
00547
00548 Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
00549 size_t point_count;
00550 if (cloud.is_dense)
00551 {
00552 point_count = cloud.size ();
00553
00554 for (size_t i = 0; i < point_count; ++i)
00555 {
00556 accu [0] += cloud[i].x * cloud[i].x;
00557 accu [1] += cloud[i].x * cloud[i].y;
00558 accu [2] += cloud[i].x * cloud[i].z;
00559 accu [3] += cloud[i].y * cloud[i].y;
00560 accu [4] += cloud[i].y * cloud[i].z;
00561 accu [5] += cloud[i].z * cloud[i].z;
00562 accu [6] += cloud[i].x;
00563 accu [7] += cloud[i].y;
00564 accu [8] += cloud[i].z;
00565 }
00566 }
00567 else
00568 {
00569 point_count = 0;
00570 for (size_t i = 0; i < cloud.points.size (); ++i)
00571 {
00572 if (!isFinite (cloud[i]))
00573 continue;
00574
00575 accu [0] += cloud[i].x * cloud[i].x;
00576 accu [1] += cloud[i].x * cloud[i].y;
00577 accu [2] += cloud[i].x * cloud[i].z;
00578 accu [3] += cloud[i].y * cloud[i].y;
00579 accu [4] += cloud[i].y * cloud[i].z;
00580 accu [5] += cloud[i].z * cloud[i].z;
00581 accu [6] += cloud[i].x;
00582 accu [7] += cloud[i].y;
00583 accu [8] += cloud[i].z;
00584 ++point_count;
00585 }
00586 }
00587 accu /= static_cast<float> (point_count);
00588 if (point_count != 0)
00589 {
00590
00591 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
00592 centroid[3] = 0;
00593 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
00594 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
00595 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
00596 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
00597 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
00598 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
00599 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
00600 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
00601 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
00602 }
00603 return (static_cast<unsigned int> (point_count));
00604 }
00605
00607 template <typename PointT> inline unsigned int
00608 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00609 const std::vector<int> &indices,
00610 Eigen::Matrix3f &covariance_matrix,
00611 Eigen::Vector4f ¢roid)
00612 {
00613
00614 Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
00615 size_t point_count;
00616 if (cloud.is_dense)
00617 {
00618 point_count = indices.size ();
00619 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00620 {
00621
00622 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00623 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00624 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00625 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00626 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00627 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00628 accu [6] += cloud[*iIt].x;
00629 accu [7] += cloud[*iIt].y;
00630 accu [8] += cloud[*iIt].z;
00631 }
00632 }
00633 else
00634 {
00635 point_count = 0;
00636 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00637 {
00638 if (!isFinite (cloud[*iIt]))
00639 continue;
00640
00641 ++point_count;
00642 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00643 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00644 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00645 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00646 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00647 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00648 accu [6] += cloud[*iIt].x;
00649 accu [7] += cloud[*iIt].y;
00650 accu [8] += cloud[*iIt].z;
00651 }
00652 }
00653
00654 accu /= static_cast<float> (point_count);
00655
00656
00657
00658 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
00659 centroid[3] = 0;
00660 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
00661 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
00662 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
00663 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
00664 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
00665 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
00666 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
00667 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
00668 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
00669
00670 return (static_cast<unsigned int> (point_count));
00671 }
00672
00674 template <typename PointT> inline unsigned int
00675 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00676 const pcl::PointIndices &indices,
00677 Eigen::Matrix3f &covariance_matrix,
00678 Eigen::Vector4f ¢roid)
00679 {
00680 return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
00681 }
00682
00684 template <typename PointT> inline unsigned int
00685 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00686 Eigen::Matrix3d &covariance_matrix,
00687 Eigen::Vector4d ¢roid)
00688 {
00689
00690 Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
00691 unsigned int point_count;
00692 if (cloud.is_dense)
00693 {
00694 point_count = cloud.size ();
00695
00696 for (size_t i = 0; i < point_count; ++i)
00697 {
00698 accu [0] += cloud[i].x * cloud[i].x;
00699 accu [1] += cloud[i].x * cloud[i].y;
00700 accu [2] += cloud[i].x * cloud[i].z;
00701 accu [3] += cloud[i].y * cloud[i].y;
00702 accu [4] += cloud[i].y * cloud[i].z;
00703 accu [5] += cloud[i].z * cloud[i].z;
00704 accu [6] += cloud[i].x;
00705 accu [7] += cloud[i].y;
00706 accu [8] += cloud[i].z;
00707 }
00708 }
00709 else
00710 {
00711 point_count = 0;
00712 for (size_t i = 0; i < cloud.points.size (); ++i)
00713 {
00714 if (!isFinite (cloud[i]))
00715 continue;
00716
00717 accu [0] += cloud[i].x * cloud[i].x;
00718 accu [1] += cloud[i].x * cloud[i].y;
00719 accu [2] += cloud[i].x * cloud[i].z;
00720 accu [3] += cloud[i].y * cloud[i].y;
00721 accu [4] += cloud[i].y * cloud[i].z;
00722 accu [5] += cloud[i].z * cloud[i].z;
00723 accu [6] += cloud[i].x;
00724 accu [7] += cloud[i].y;
00725 accu [8] += cloud[i].z;
00726 ++point_count;
00727 }
00728 }
00729
00730 if (point_count != 0)
00731 {
00732 accu /= static_cast<double> (point_count);
00733 centroid.head<3> () = accu.tail<3> ();
00734 centroid[3] = 0;
00735 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
00736 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
00737 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
00738 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
00739 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
00740 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
00741 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
00742 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
00743 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
00744 }
00745 return (point_count);
00746 }
00747
00749 template <typename PointT> inline unsigned int
00750 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00751 const std::vector<int> &indices,
00752 Eigen::Matrix3d &covariance_matrix,
00753 Eigen::Vector4d ¢roid)
00754 {
00755
00756 Eigen::Matrix<double, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 9, Eigen::RowMajor>::Zero ();
00757 unsigned point_count;
00758 if (cloud.is_dense)
00759 {
00760 point_count = indices.size ();
00761 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00762 {
00763
00764 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00765 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00766 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00767 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00768 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00769 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00770 accu [6] += cloud[*iIt].x;
00771 accu [7] += cloud[*iIt].y;
00772 accu [8] += cloud[*iIt].z;
00773 }
00774 }
00775 else
00776 {
00777 point_count = 0;
00778 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
00779 {
00780 if (!isFinite (cloud[*iIt]))
00781 continue;
00782
00783 ++point_count;
00784 accu [0] += cloud[*iIt].x * cloud[*iIt].x;
00785 accu [1] += cloud[*iIt].x * cloud[*iIt].y;
00786 accu [2] += cloud[*iIt].x * cloud[*iIt].z;
00787 accu [3] += cloud[*iIt].y * cloud[*iIt].y;
00788 accu [4] += cloud[*iIt].y * cloud[*iIt].z;
00789 accu [5] += cloud[*iIt].z * cloud[*iIt].z;
00790 accu [6] += cloud[*iIt].x;
00791 accu [7] += cloud[*iIt].y;
00792 accu [8] += cloud[*iIt].z;
00793 }
00794 }
00795
00796 if (point_count != 0)
00797 {
00798 accu /= static_cast<double> (point_count);
00799 Eigen::Vector3f vec = accu.tail<3> ();
00800 centroid.head<3> () = vec;
00801 centroid[3] = 0;
00802 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
00803 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
00804 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
00805 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
00806 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
00807 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
00808 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
00809 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
00810 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);
00811 }
00812 return (point_count);
00813 }
00814
00816 template <typename PointT> inline unsigned int
00817 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
00818 const pcl::PointIndices &indices,
00819 Eigen::Matrix3d &covariance_matrix,
00820 Eigen::Vector4d ¢roid)
00821 {
00822 return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid));
00823 }
00825 template <typename PointT> void
00826 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00827 const Eigen::Vector4f ¢roid,
00828 pcl::PointCloud<PointT> &cloud_out)
00829 {
00830 cloud_out = cloud_in;
00831
00832
00833 for (size_t i = 0; i < cloud_in.points.size (); ++i)
00834 cloud_out.points[i].getVector4fMap () -= centroid;
00835 }
00836
00838 template <typename PointT> void
00839 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00840 const std::vector<int> &indices,
00841 const Eigen::Vector4f ¢roid,
00842 pcl::PointCloud<PointT> &cloud_out)
00843 {
00844 cloud_out.header = cloud_in.header;
00845 cloud_out.is_dense = cloud_in.is_dense;
00846 if (indices.size () == cloud_in.points.size ())
00847 {
00848 cloud_out.width = cloud_in.width;
00849 cloud_out.height = cloud_in.height;
00850 }
00851 else
00852 {
00853 cloud_out.width = static_cast<uint32_t> (indices.size ());
00854 cloud_out.height = 1;
00855 }
00856 cloud_out.points.resize (indices.size ());
00857
00858
00859 for (size_t i = 0; i < indices.size (); ++i)
00860 cloud_out.points[i].getVector4fMap () = cloud_in.points[indices[i]].getVector4fMap () -
00861 centroid;
00862 }
00863
00865 template <typename PointT> void
00866 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00867 const Eigen::Vector4f ¢roid,
00868 Eigen::MatrixXf &cloud_out)
00869 {
00870 size_t npts = cloud_in.points.size ();
00871
00872 cloud_out = Eigen::MatrixXf::Zero (4, npts);
00873
00874 for (size_t i = 0; i < npts; ++i)
00875
00876 cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid;
00877
00878
00879 cloud_out.block (3, 0, 1, npts).setZero ();
00880 }
00881
00883 template <typename PointT> void
00884 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00885 const std::vector<int> &indices,
00886 const Eigen::Vector4f ¢roid,
00887 Eigen::MatrixXf &cloud_out)
00888 {
00889 size_t npts = indices.size ();
00890
00891 cloud_out = Eigen::MatrixXf::Zero (4, npts);
00892
00893 for (size_t i = 0; i < npts; ++i)
00894
00895 cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid;
00896
00897
00898 cloud_out.block (3, 0, 1, npts).setZero ();
00899 }
00900
00902 template <typename PointT> void
00903 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
00904 const pcl::PointIndices &indices,
00905 const Eigen::Vector4f ¢roid,
00906 Eigen::MatrixXf &cloud_out)
00907 {
00908 return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out));
00909 }
00910
00912 template <typename PointT> inline void
00913 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf ¢roid)
00914 {
00915 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00916
00917
00918 centroid.setZero (boost::mpl::size<FieldList>::value);
00919
00920 if (cloud.points.empty ())
00921 return;
00922
00923 int size = static_cast<int> (cloud.points.size ());
00924 for (int i = 0; i < size; ++i)
00925 {
00926
00927 pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[i], centroid));
00928 }
00929 centroid /= static_cast<float> (size);
00930 }
00931
00933 template <typename PointT> inline void
00934 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00935 Eigen::VectorXf ¢roid)
00936 {
00937 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00938
00939
00940 centroid.setZero (boost::mpl::size<FieldList>::value);
00941
00942 if (indices.empty ())
00943 return;
00944
00945 int nr_points = static_cast<int> (indices.size ());
00946 for (int i = 0; i < nr_points; ++i)
00947 {
00948
00949 pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[indices[i]], centroid));
00950 }
00951 centroid /= static_cast<float> (nr_points);
00952 }
00953
00955 template <typename PointT> inline void
00956 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
00957 const pcl::PointIndices &indices, Eigen::VectorXf ¢roid)
00958 {
00959 return (pcl::computeNDCentroid<PointT> (cloud, indices.indices, centroid));
00960 }
00961
00962 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_
00963