Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009-present, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_COMMON_IMPL_CENTROID_H_ 00042 #define PCL_COMMON_IMPL_CENTROID_H_ 00043 00044 #include <pcl/common/centroid.h> 00045 #include <pcl/conversions.h> 00046 #include <boost/mpl/size.hpp> 00047 00048 /////////////////////////////////////////////////////////////////////////////////////////// 00049 template <typename PointT, typename Scalar> inline unsigned int 00050 pcl::compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator, 00051 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00052 { 00053 // Initialize to 0 00054 centroid.setZero (); 00055 00056 unsigned cp = 0; 00057 00058 // For each point in the cloud 00059 // If the data is dense, we don't need to check for NaN 00060 while (cloud_iterator.isValid ()) 00061 { 00062 // Check if the point is invalid 00063 if (!pcl_isfinite (cloud_iterator->x) || 00064 !pcl_isfinite (cloud_iterator->y) || 00065 !pcl_isfinite (cloud_iterator->z)) 00066 continue; 00067 centroid[0] += cloud_iterator->x; 00068 centroid[1] += cloud_iterator->y; 00069 centroid[2] += cloud_iterator->z; 00070 ++cp; 00071 ++cloud_iterator; 00072 } 00073 centroid[3] = 0; 00074 centroid /= static_cast<Scalar> (cp); 00075 return (cp); 00076 } 00077 00078 /////////////////////////////////////////////////////////////////////////////////////////// 00079 template <typename PointT, typename Scalar> inline unsigned int 00080 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00081 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00082 { 00083 if (cloud.empty ()) 00084 return (0); 00085 00086 // Initialize to 0 00087 centroid.setZero (); 00088 // For each point in the cloud 00089 // If the data is dense, we don't need to check for NaN 00090 if (cloud.is_dense) 00091 { 00092 for (size_t i = 0; i < cloud.size (); ++i) 00093 { 00094 centroid[0] += cloud[i].x; 00095 centroid[1] += cloud[i].y; 00096 centroid[2] += cloud[i].z; 00097 } 00098 centroid[3] = 0; 00099 centroid /= static_cast<Scalar> (cloud.size ()); 00100 00101 return (static_cast<unsigned int> (cloud.size ())); 00102 } 00103 // NaN or Inf values could exist => check for them 00104 else 00105 { 00106 unsigned cp = 0; 00107 for (size_t i = 0; i < cloud.size (); ++i) 00108 { 00109 // Check if the point is invalid 00110 if (!isFinite (cloud [i])) 00111 continue; 00112 00113 centroid[0] += cloud[i].x; 00114 centroid[1] += cloud[i].y; 00115 centroid[2] += cloud[i].z; 00116 ++cp; 00117 } 00118 centroid[3] = 0; 00119 centroid /= static_cast<Scalar> (cp); 00120 00121 return (cp); 00122 } 00123 } 00124 00125 /////////////////////////////////////////////////////////////////////////////////////////// 00126 template <typename PointT, typename Scalar> inline unsigned int 00127 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00128 const std::vector<int> &indices, 00129 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00130 { 00131 if (indices.empty ()) 00132 return (0); 00133 00134 // Initialize to 0 00135 centroid.setZero (); 00136 // If the data is dense, we don't need to check for NaN 00137 if (cloud.is_dense) 00138 { 00139 for (size_t i = 0; i < indices.size (); ++i) 00140 { 00141 centroid[0] += cloud[indices[i]].x; 00142 centroid[1] += cloud[indices[i]].y; 00143 centroid[2] += cloud[indices[i]].z; 00144 } 00145 centroid[3] = 0; 00146 centroid /= static_cast<Scalar> (indices.size ()); 00147 return (static_cast<unsigned int> (indices.size ())); 00148 } 00149 // NaN or Inf values could exist => check for them 00150 else 00151 { 00152 unsigned cp = 0; 00153 for (size_t i = 0; i < indices.size (); ++i) 00154 { 00155 // Check if the point is invalid 00156 if (!isFinite (cloud [indices[i]])) 00157 continue; 00158 00159 centroid[0] += cloud[indices[i]].x; 00160 centroid[1] += cloud[indices[i]].y; 00161 centroid[2] += cloud[indices[i]].z; 00162 ++cp; 00163 } 00164 centroid[3] = 0; 00165 centroid /= static_cast<Scalar> (cp); 00166 return (cp); 00167 } 00168 } 00169 00170 ///////////////////////////////////////////////////////////////////////////////////////////// 00171 template <typename PointT, typename Scalar> inline unsigned int 00172 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00173 const pcl::PointIndices &indices, 00174 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00175 { 00176 return (pcl::compute3DCentroid (cloud, indices.indices, centroid)); 00177 } 00178 00179 ////////////////////////////////////////////////////////////////////////////////////////////// 00180 template <typename PointT, typename Scalar> inline unsigned 00181 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00182 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00183 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00184 { 00185 if (cloud.empty ()) 00186 return (0); 00187 00188 // Initialize to 0 00189 covariance_matrix.setZero (); 00190 00191 unsigned point_count; 00192 // If the data is dense, we don't need to check for NaN 00193 if (cloud.is_dense) 00194 { 00195 point_count = static_cast<unsigned> (cloud.size ()); 00196 // For each point in the cloud 00197 for (size_t i = 0; i < point_count; ++i) 00198 { 00199 Eigen::Matrix<Scalar, 4, 1> pt; 00200 pt[0] = cloud[i].x - centroid[0]; 00201 pt[1] = cloud[i].y - centroid[1]; 00202 pt[2] = cloud[i].z - centroid[2]; 00203 00204 covariance_matrix (1, 1) += pt.y () * pt.y (); 00205 covariance_matrix (1, 2) += pt.y () * pt.z (); 00206 00207 covariance_matrix (2, 2) += pt.z () * pt.z (); 00208 00209 pt *= pt.x (); 00210 covariance_matrix (0, 0) += pt.x (); 00211 covariance_matrix (0, 1) += pt.y (); 00212 covariance_matrix (0, 2) += pt.z (); 00213 } 00214 } 00215 // NaN or Inf values could exist => check for them 00216 else 00217 { 00218 point_count = 0; 00219 // For each point in the cloud 00220 for (size_t i = 0; i < cloud.size (); ++i) 00221 { 00222 // Check if the point is invalid 00223 if (!isFinite (cloud [i])) 00224 continue; 00225 00226 Eigen::Matrix<Scalar, 4, 1> pt; 00227 pt[0] = cloud[i].x - centroid[0]; 00228 pt[1] = cloud[i].y - centroid[1]; 00229 pt[2] = cloud[i].z - centroid[2]; 00230 00231 covariance_matrix (1, 1) += pt.y () * pt.y (); 00232 covariance_matrix (1, 2) += pt.y () * pt.z (); 00233 00234 covariance_matrix (2, 2) += pt.z () * pt.z (); 00235 00236 pt *= pt.x (); 00237 covariance_matrix (0, 0) += pt.x (); 00238 covariance_matrix (0, 1) += pt.y (); 00239 covariance_matrix (0, 2) += pt.z (); 00240 ++point_count; 00241 } 00242 } 00243 covariance_matrix (1, 0) = covariance_matrix (0, 1); 00244 covariance_matrix (2, 0) = covariance_matrix (0, 2); 00245 covariance_matrix (2, 1) = covariance_matrix (1, 2); 00246 00247 return (point_count); 00248 } 00249 00250 ////////////////////////////////////////////////////////////////////////////////////////////// 00251 template <typename PointT, typename Scalar> inline unsigned int 00252 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00253 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00254 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00255 { 00256 unsigned point_count = pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix); 00257 if (point_count != 0) 00258 covariance_matrix /= static_cast<Scalar> (point_count); 00259 return (point_count); 00260 } 00261 00262 ////////////////////////////////////////////////////////////////////////////////////////////// 00263 template <typename PointT, typename Scalar> inline unsigned int 00264 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00265 const std::vector<int> &indices, 00266 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00267 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00268 { 00269 if (indices.empty ()) 00270 return (0); 00271 00272 // Initialize to 0 00273 covariance_matrix.setZero (); 00274 00275 size_t point_count; 00276 // If the data is dense, we don't need to check for NaN 00277 if (cloud.is_dense) 00278 { 00279 point_count = indices.size (); 00280 // For each point in the cloud 00281 for (size_t i = 0; i < point_count; ++i) 00282 { 00283 Eigen::Matrix<Scalar, 4, 1> pt; 00284 pt[0] = cloud[indices[i]].x - centroid[0]; 00285 pt[1] = cloud[indices[i]].y - centroid[1]; 00286 pt[2] = cloud[indices[i]].z - centroid[2]; 00287 00288 covariance_matrix (1, 1) += pt.y () * pt.y (); 00289 covariance_matrix (1, 2) += pt.y () * pt.z (); 00290 00291 covariance_matrix (2, 2) += pt.z () * pt.z (); 00292 00293 pt *= pt.x (); 00294 covariance_matrix (0, 0) += pt.x (); 00295 covariance_matrix (0, 1) += pt.y (); 00296 covariance_matrix (0, 2) += pt.z (); 00297 } 00298 } 00299 // NaN or Inf values could exist => check for them 00300 else 00301 { 00302 point_count = 0; 00303 // For each point in the cloud 00304 for (size_t i = 0; i < indices.size (); ++i) 00305 { 00306 // Check if the point is invalid 00307 if (!isFinite (cloud[indices[i]])) 00308 continue; 00309 00310 Eigen::Matrix<Scalar, 4, 1> pt; 00311 pt[0] = cloud[indices[i]].x - centroid[0]; 00312 pt[1] = cloud[indices[i]].y - centroid[1]; 00313 pt[2] = cloud[indices[i]].z - centroid[2]; 00314 00315 covariance_matrix (1, 1) += pt.y () * pt.y (); 00316 covariance_matrix (1, 2) += pt.y () * pt.z (); 00317 00318 covariance_matrix (2, 2) += pt.z () * pt.z (); 00319 00320 pt *= pt.x (); 00321 covariance_matrix (0, 0) += pt.x (); 00322 covariance_matrix (0, 1) += pt.y (); 00323 covariance_matrix (0, 2) += pt.z (); 00324 ++point_count; 00325 } 00326 } 00327 covariance_matrix (1, 0) = covariance_matrix (0, 1); 00328 covariance_matrix (2, 0) = covariance_matrix (0, 2); 00329 covariance_matrix (2, 1) = covariance_matrix (1, 2); 00330 return (static_cast<unsigned int> (point_count)); 00331 } 00332 00333 ////////////////////////////////////////////////////////////////////////////////////////////// 00334 template <typename PointT, typename Scalar> inline unsigned int 00335 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00336 const pcl::PointIndices &indices, 00337 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00338 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00339 { 00340 return (pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix)); 00341 } 00342 00343 ////////////////////////////////////////////////////////////////////////////////////////////// 00344 template <typename PointT, typename Scalar> inline unsigned int 00345 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00346 const std::vector<int> &indices, 00347 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00348 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00349 { 00350 unsigned point_count = pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix); 00351 if (point_count != 0) 00352 covariance_matrix /= static_cast<Scalar> (point_count); 00353 00354 return (point_count); 00355 } 00356 00357 ////////////////////////////////////////////////////////////////////////////////////////////// 00358 template <typename PointT, typename Scalar> inline unsigned int 00359 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00360 const pcl::PointIndices &indices, 00361 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00362 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00363 { 00364 unsigned int point_count = pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix); 00365 if (point_count != 0) 00366 covariance_matrix /= static_cast<Scalar> (point_count); 00367 00368 return point_count; 00369 } 00370 00371 ////////////////////////////////////////////////////////////////////////////////////////////// 00372 template <typename PointT, typename Scalar> inline unsigned int 00373 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00374 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00375 { 00376 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer 00377 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero (); 00378 00379 unsigned int point_count; 00380 if (cloud.is_dense) 00381 { 00382 point_count = static_cast<unsigned int> (cloud.size ()); 00383 // For each point in the cloud 00384 for (size_t i = 0; i < point_count; ++i) 00385 { 00386 accu [0] += cloud[i].x * cloud[i].x; 00387 accu [1] += cloud[i].x * cloud[i].y; 00388 accu [2] += cloud[i].x * cloud[i].z; 00389 accu [3] += cloud[i].y * cloud[i].y; 00390 accu [4] += cloud[i].y * cloud[i].z; 00391 accu [5] += cloud[i].z * cloud[i].z; 00392 } 00393 } 00394 else 00395 { 00396 point_count = 0; 00397 for (size_t i = 0; i < cloud.size (); ++i) 00398 { 00399 if (!isFinite (cloud[i])) 00400 continue; 00401 00402 accu [0] += cloud[i].x * cloud[i].x; 00403 accu [1] += cloud[i].x * cloud[i].y; 00404 accu [2] += cloud[i].x * cloud[i].z; 00405 accu [3] += cloud[i].y * cloud[i].y; 00406 accu [4] += cloud[i].y * cloud[i].z; 00407 accu [5] += cloud[i].z * cloud[i].z; 00408 ++point_count; 00409 } 00410 } 00411 00412 if (point_count != 0) 00413 { 00414 accu /= static_cast<Scalar> (point_count); 00415 covariance_matrix.coeffRef (0) = accu [0]; 00416 covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; 00417 covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; 00418 covariance_matrix.coeffRef (4) = accu [3]; 00419 covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; 00420 covariance_matrix.coeffRef (8) = accu [5]; 00421 } 00422 return (point_count); 00423 } 00424 00425 ////////////////////////////////////////////////////////////////////////////////////////////// 00426 template <typename PointT, typename Scalar> inline unsigned int 00427 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00428 const std::vector<int> &indices, 00429 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00430 { 00431 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer 00432 Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero (); 00433 00434 unsigned int point_count; 00435 if (cloud.is_dense) 00436 { 00437 point_count = static_cast<unsigned int> (indices.size ()); 00438 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) 00439 { 00440 //const PointT& point = cloud[*iIt]; 00441 accu [0] += cloud[*iIt].x * cloud[*iIt].x; 00442 accu [1] += cloud[*iIt].x * cloud[*iIt].y; 00443 accu [2] += cloud[*iIt].x * cloud[*iIt].z; 00444 accu [3] += cloud[*iIt].y * cloud[*iIt].y; 00445 accu [4] += cloud[*iIt].y * cloud[*iIt].z; 00446 accu [5] += cloud[*iIt].z * cloud[*iIt].z; 00447 } 00448 } 00449 else 00450 { 00451 point_count = 0; 00452 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) 00453 { 00454 if (!isFinite (cloud[*iIt])) 00455 continue; 00456 00457 ++point_count; 00458 accu [0] += cloud[*iIt].x * cloud[*iIt].x; 00459 accu [1] += cloud[*iIt].x * cloud[*iIt].y; 00460 accu [2] += cloud[*iIt].x * cloud[*iIt].z; 00461 accu [3] += cloud[*iIt].y * cloud[*iIt].y; 00462 accu [4] += cloud[*iIt].y * cloud[*iIt].z; 00463 accu [5] += cloud[*iIt].z * cloud[*iIt].z; 00464 } 00465 } 00466 if (point_count != 0) 00467 { 00468 accu /= static_cast<Scalar> (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 00479 ////////////////////////////////////////////////////////////////////////////////////////////// 00480 template <typename PointT, typename Scalar> inline unsigned int 00481 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00482 const pcl::PointIndices &indices, 00483 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) 00484 { 00485 return (computeCovarianceMatrix (cloud, indices.indices, covariance_matrix)); 00486 } 00487 00488 ////////////////////////////////////////////////////////////////////////////////////////////// 00489 template <typename PointT, typename Scalar> inline unsigned int 00490 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00491 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, 00492 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00493 { 00494 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer 00495 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero (); 00496 size_t point_count; 00497 if (cloud.is_dense) 00498 { 00499 point_count = cloud.size (); 00500 // For each point in the cloud 00501 for (size_t i = 0; i < point_count; ++i) 00502 { 00503 accu [0] += cloud[i].x * cloud[i].x; 00504 accu [1] += cloud[i].x * cloud[i].y; 00505 accu [2] += cloud[i].x * cloud[i].z; 00506 accu [3] += cloud[i].y * cloud[i].y; // 4 00507 accu [4] += cloud[i].y * cloud[i].z; // 5 00508 accu [5] += cloud[i].z * cloud[i].z; // 8 00509 accu [6] += cloud[i].x; 00510 accu [7] += cloud[i].y; 00511 accu [8] += cloud[i].z; 00512 } 00513 } 00514 else 00515 { 00516 point_count = 0; 00517 for (size_t i = 0; i < cloud.size (); ++i) 00518 { 00519 if (!isFinite (cloud[i])) 00520 continue; 00521 00522 accu [0] += cloud[i].x * cloud[i].x; 00523 accu [1] += cloud[i].x * cloud[i].y; 00524 accu [2] += cloud[i].x * cloud[i].z; 00525 accu [3] += cloud[i].y * cloud[i].y; 00526 accu [4] += cloud[i].y * cloud[i].z; 00527 accu [5] += cloud[i].z * cloud[i].z; 00528 accu [6] += cloud[i].x; 00529 accu [7] += cloud[i].y; 00530 accu [8] += cloud[i].z; 00531 ++point_count; 00532 } 00533 } 00534 accu /= static_cast<Scalar> (point_count); 00535 if (point_count != 0) 00536 { 00537 //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0 00538 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; 00539 centroid[3] = 0; 00540 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; 00541 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; 00542 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; 00543 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; 00544 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; 00545 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; 00546 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); 00547 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); 00548 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); 00549 } 00550 return (static_cast<unsigned int> (point_count)); 00551 } 00552 00553 ////////////////////////////////////////////////////////////////////////////////////////////// 00554 template <typename PointT, typename Scalar> inline unsigned int 00555 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00556 const std::vector<int> &indices, 00557 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, 00558 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00559 { 00560 // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer 00561 Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero (); 00562 size_t point_count; 00563 if (cloud.is_dense) 00564 { 00565 point_count = indices.size (); 00566 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) 00567 { 00568 //const PointT& point = cloud[*iIt]; 00569 accu [0] += cloud[*iIt].x * cloud[*iIt].x; 00570 accu [1] += cloud[*iIt].x * cloud[*iIt].y; 00571 accu [2] += cloud[*iIt].x * cloud[*iIt].z; 00572 accu [3] += cloud[*iIt].y * cloud[*iIt].y; 00573 accu [4] += cloud[*iIt].y * cloud[*iIt].z; 00574 accu [5] += cloud[*iIt].z * cloud[*iIt].z; 00575 accu [6] += cloud[*iIt].x; 00576 accu [7] += cloud[*iIt].y; 00577 accu [8] += cloud[*iIt].z; 00578 } 00579 } 00580 else 00581 { 00582 point_count = 0; 00583 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) 00584 { 00585 if (!isFinite (cloud[*iIt])) 00586 continue; 00587 00588 ++point_count; 00589 accu [0] += cloud[*iIt].x * cloud[*iIt].x; 00590 accu [1] += cloud[*iIt].x * cloud[*iIt].y; 00591 accu [2] += cloud[*iIt].x * cloud[*iIt].z; 00592 accu [3] += cloud[*iIt].y * cloud[*iIt].y; // 4 00593 accu [4] += cloud[*iIt].y * cloud[*iIt].z; // 5 00594 accu [5] += cloud[*iIt].z * cloud[*iIt].z; // 8 00595 accu [6] += cloud[*iIt].x; 00596 accu [7] += cloud[*iIt].y; 00597 accu [8] += cloud[*iIt].z; 00598 } 00599 } 00600 00601 accu /= static_cast<Scalar> (point_count); 00602 //Eigen::Vector3f vec = accu.tail<3> (); 00603 //centroid.head<3> () = vec;//= accu.tail<3> (); 00604 //centroid.head<3> () = accu.tail<3> (); -- does not compile with Clang 3.0 00605 centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8]; 00606 centroid[3] = 0; 00607 covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6]; 00608 covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7]; 00609 covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8]; 00610 covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7]; 00611 covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8]; 00612 covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8]; 00613 covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1); 00614 covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2); 00615 covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5); 00616 00617 return (static_cast<unsigned int> (point_count)); 00618 } 00619 00620 ////////////////////////////////////////////////////////////////////////////////////////////// 00621 template <typename PointT, typename Scalar> inline unsigned int 00622 pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00623 const pcl::PointIndices &indices, 00624 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, 00625 Eigen::Matrix<Scalar, 4, 1> ¢roid) 00626 { 00627 return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid)); 00628 } 00629 00630 ////////////////////////////////////////////////////////////////////////////////////////////// 00631 template <typename PointT, typename Scalar> void 00632 pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00633 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00634 pcl::PointCloud<PointT> &cloud_out, 00635 int npts) 00636 { 00637 // Calculate the number of points if not given 00638 if (npts == 0) 00639 { 00640 while (cloud_iterator.isValid ()) 00641 { 00642 ++npts; 00643 ++cloud_iterator; 00644 } 00645 cloud_iterator.reset (); 00646 } 00647 00648 int i = 0; 00649 cloud_out.resize (npts); 00650 // Subtract the centroid from cloud_in 00651 while (cloud_iterator.isValid ()) 00652 { 00653 cloud_out[i].x = cloud_iterator->x - centroid[0]; 00654 cloud_out[i].y = cloud_iterator->y - centroid[1]; 00655 cloud_out[i].z = cloud_iterator->z - centroid[2]; 00656 ++i; 00657 ++cloud_iterator; 00658 } 00659 cloud_out.width = cloud_out.size (); 00660 cloud_out.height = 1; 00661 } 00662 00663 ////////////////////////////////////////////////////////////////////////////////////////////// 00664 template <typename PointT, typename Scalar> void 00665 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00666 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00667 pcl::PointCloud<PointT> &cloud_out) 00668 { 00669 cloud_out = cloud_in; 00670 00671 // Subtract the centroid from cloud_in 00672 for (size_t i = 0; i < cloud_in.points.size (); ++i) 00673 { 00674 cloud_out[i].x -= static_cast<float> (centroid[0]); 00675 cloud_out[i].y -= static_cast<float> (centroid[1]); 00676 cloud_out[i].z -= static_cast<float> (centroid[2]); 00677 } 00678 } 00679 00680 ////////////////////////////////////////////////////////////////////////////////////////////// 00681 template <typename PointT, typename Scalar> void 00682 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00683 const std::vector<int> &indices, 00684 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00685 pcl::PointCloud<PointT> &cloud_out) 00686 { 00687 cloud_out.header = cloud_in.header; 00688 cloud_out.is_dense = cloud_in.is_dense; 00689 if (indices.size () == cloud_in.points.size ()) 00690 { 00691 cloud_out.width = cloud_in.width; 00692 cloud_out.height = cloud_in.height; 00693 } 00694 else 00695 { 00696 cloud_out.width = static_cast<uint32_t> (indices.size ()); 00697 cloud_out.height = 1; 00698 } 00699 cloud_out.resize (indices.size ()); 00700 00701 // Subtract the centroid from cloud_in 00702 for (size_t i = 0; i < indices.size (); ++i) 00703 { 00704 cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]); 00705 cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]); 00706 cloud_out[i].z = static_cast<float> (cloud_in[indices[i]].z - centroid[2]); 00707 } 00708 } 00709 00710 ////////////////////////////////////////////////////////////////////////////////////////////// 00711 template <typename PointT, typename Scalar> void 00712 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00713 const pcl::PointIndices& indices, 00714 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00715 pcl::PointCloud<PointT> &cloud_out) 00716 { 00717 return (demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out)); 00718 } 00719 00720 ////////////////////////////////////////////////////////////////////////////////////////////// 00721 template <typename PointT, typename Scalar> void 00722 pcl::demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00723 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00724 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out, 00725 int npts) 00726 { 00727 // Calculate the number of points if not given 00728 if (npts == 0) 00729 { 00730 while (cloud_iterator.isValid ()) 00731 { 00732 ++npts; 00733 ++cloud_iterator; 00734 } 00735 cloud_iterator.reset (); 00736 } 00737 00738 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned 00739 00740 int i = 0; 00741 while (cloud_iterator.isValid ()) 00742 { 00743 cloud_out (0, i) = cloud_iterator->x - centroid[0]; 00744 cloud_out (1, i) = cloud_iterator->y - centroid[1]; 00745 cloud_out (2, i) = cloud_iterator->z - centroid[2]; 00746 ++i; 00747 ++cloud_iterator; 00748 } 00749 } 00750 00751 ////////////////////////////////////////////////////////////////////////////////////////////// 00752 template <typename PointT, typename Scalar> void 00753 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00754 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00755 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out) 00756 { 00757 size_t npts = cloud_in.size (); 00758 00759 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned 00760 00761 for (size_t i = 0; i < npts; ++i) 00762 { 00763 cloud_out (0, i) = cloud_in[i].x - centroid[0]; 00764 cloud_out (1, i) = cloud_in[i].y - centroid[1]; 00765 cloud_out (2, i) = cloud_in[i].z - centroid[2]; 00766 // One column at a time 00767 //cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid; 00768 } 00769 00770 // Make sure we zero the 4th dimension out (1 row, N columns) 00771 //cloud_out.block (3, 0, 1, npts).setZero (); 00772 } 00773 00774 ////////////////////////////////////////////////////////////////////////////////////////////// 00775 template <typename PointT, typename Scalar> void 00776 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00777 const std::vector<int> &indices, 00778 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00779 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out) 00780 { 00781 size_t npts = indices.size (); 00782 00783 cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned 00784 00785 for (size_t i = 0; i < npts; ++i) 00786 { 00787 cloud_out (0, i) = cloud_in[indices[i]].x - centroid[0]; 00788 cloud_out (1, i) = cloud_in[indices[i]].y - centroid[1]; 00789 cloud_out (2, i) = cloud_in[indices[i]].z - centroid[2]; 00790 // One column at a time 00791 //cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid; 00792 } 00793 00794 // Make sure we zero the 4th dimension out (1 row, N columns) 00795 //cloud_out.block (3, 0, 1, npts).setZero (); 00796 } 00797 00798 ////////////////////////////////////////////////////////////////////////////////////////////// 00799 template <typename PointT, typename Scalar> void 00800 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00801 const pcl::PointIndices &indices, 00802 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00803 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out) 00804 { 00805 return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out)); 00806 } 00807 00808 ////////////////////////////////////////////////////////////////////////////////////////////// 00809 template <typename PointT, typename Scalar> inline void 00810 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00811 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid) 00812 { 00813 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00814 00815 // Get the size of the fields 00816 centroid.setZero (boost::mpl::size<FieldList>::value); 00817 00818 if (cloud.empty ()) 00819 return; 00820 // Iterate over each point 00821 int size = static_cast<int> (cloud.size ()); 00822 for (int i = 0; i < size; ++i) 00823 { 00824 // Iterate over each dimension 00825 pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[i], centroid)); 00826 } 00827 centroid /= static_cast<Scalar> (size); 00828 } 00829 00830 ////////////////////////////////////////////////////////////////////////////////////////////// 00831 template <typename PointT, typename Scalar> inline void 00832 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00833 const std::vector<int> &indices, 00834 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid) 00835 { 00836 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00837 00838 // Get the size of the fields 00839 centroid.setZero (boost::mpl::size<FieldList>::value); 00840 00841 if (indices.empty ()) 00842 return; 00843 // Iterate over each point 00844 int nr_points = static_cast<int> (indices.size ()); 00845 for (int i = 0; i < nr_points; ++i) 00846 { 00847 // Iterate over each dimension 00848 pcl::for_each_type <FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[indices[i]], centroid)); 00849 } 00850 centroid /= static_cast<Scalar> (nr_points); 00851 } 00852 00853 ///////////////////////////////////////////////////////////////////////////////////////////// 00854 template <typename PointT, typename Scalar> inline void 00855 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00856 const pcl::PointIndices &indices, 00857 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid) 00858 { 00859 return (pcl::computeNDCentroid (cloud, indices.indices, centroid)); 00860 } 00861 00862 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_ 00863