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) 2010, 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 */ 00038 00039 #ifndef PCL_COMMON_CENTROID_H_ 00040 #define PCL_COMMON_CENTROID_H_ 00041 00042 #include <pcl/point_cloud.h> 00043 #include <pcl/point_traits.h> 00044 #include <pcl/PointIndices.h> 00045 #include <pcl/cloud_iterator.h> 00046 00047 /** 00048 * \file pcl/common/centroid.h 00049 * Define methods for centroid estimation and covariance matrix calculus 00050 * \ingroup common 00051 */ 00052 00053 /*@{*/ 00054 namespace pcl 00055 { 00056 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector. 00057 * \param[in] cloud_iterator an iterator over the input point cloud 00058 * \param[out] centroid the output centroid 00059 * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. 00060 * \note if return value is 0, the centroid is not changed, thus not valid. 00061 * \ingroup common 00062 */ 00063 template <typename PointT, typename Scalar> inline unsigned int 00064 compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator, 00065 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00066 00067 template <typename PointT> inline unsigned int 00068 compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator, 00069 Eigen::Vector4f ¢roid) 00070 { 00071 return (compute3DCentroid <PointT, float> (cloud_iterator, centroid)); 00072 } 00073 00074 template <typename PointT> inline unsigned int 00075 compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator, 00076 Eigen::Vector4d ¢roid) 00077 { 00078 return (compute3DCentroid <PointT, double> (cloud_iterator, centroid)); 00079 } 00080 00081 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector. 00082 * \param[in] cloud the input point cloud 00083 * \param[out] centroid the output centroid 00084 * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. 00085 * \note if return value is 0, the centroid is not changed, thus not valid. 00086 * \ingroup common 00087 */ 00088 template <typename PointT, typename Scalar> inline unsigned int 00089 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00090 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00091 00092 template <typename PointT> inline unsigned int 00093 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00094 Eigen::Vector4f ¢roid) 00095 { 00096 return (compute3DCentroid <PointT, float> (cloud, centroid)); 00097 } 00098 00099 template <typename PointT> inline unsigned int 00100 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00101 Eigen::Vector4d ¢roid) 00102 { 00103 return (compute3DCentroid <PointT, double> (cloud, centroid)); 00104 } 00105 00106 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and 00107 * return it as a 3D vector. 00108 * \param[in] cloud the input point cloud 00109 * \param[in] indices the point cloud indices that need to be used 00110 * \param[out] centroid the output centroid 00111 * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. 00112 * \note if return value is 0, the centroid is not changed, thus not valid. 00113 * \ingroup common 00114 */ 00115 template <typename PointT, typename Scalar> inline unsigned int 00116 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00117 const std::vector<int> &indices, 00118 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00119 00120 template <typename PointT> inline unsigned int 00121 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00122 const std::vector<int> &indices, 00123 Eigen::Vector4f ¢roid) 00124 { 00125 return (compute3DCentroid <PointT, float> (cloud, indices, centroid)); 00126 } 00127 00128 template <typename PointT> inline unsigned int 00129 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00130 const std::vector<int> &indices, 00131 Eigen::Vector4d ¢roid) 00132 { 00133 return (compute3DCentroid <PointT, double> (cloud, indices, centroid)); 00134 } 00135 00136 /** \brief Compute the 3D (X-Y-Z) centroid of a set of points using their indices and 00137 * return it as a 3D vector. 00138 * \param[in] cloud the input point cloud 00139 * \param[in] indices the point cloud indices that need to be used 00140 * \param[out] centroid the output centroid 00141 * \return number of valid point used to determine the centroid. In case of dense point clouds, this is the same as the size of input cloud. 00142 * \note if return value is 0, the centroid is not changed, thus not valid. 00143 * \ingroup common 00144 */ 00145 template <typename PointT, typename Scalar> inline unsigned int 00146 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00147 const pcl::PointIndices &indices, 00148 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00149 00150 template <typename PointT> inline unsigned int 00151 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00152 const pcl::PointIndices &indices, 00153 Eigen::Vector4f ¢roid) 00154 { 00155 return (compute3DCentroid <PointT, float> (cloud, indices, centroid)); 00156 } 00157 00158 template <typename PointT> inline unsigned int 00159 compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00160 const pcl::PointIndices &indices, 00161 Eigen::Vector4d ¢roid) 00162 { 00163 return (compute3DCentroid <PointT, double> (cloud, indices, centroid)); 00164 } 00165 00166 /** \brief Compute the 3x3 covariance matrix of a given set of points. 00167 * The result is returned as a Eigen::Matrix3f. 00168 * Note: the covariance matrix is not normalized with the number of 00169 * points. For a normalized covariance, please use 00170 * computeNormalizedCovarianceMatrix. 00171 * \param[in] cloud the input point cloud 00172 * \param[in] centroid the centroid of the set of points in the cloud 00173 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00174 * \return number of valid point used to determine the covariance matrix. 00175 * In case of dense point clouds, this is the same as the size of input cloud. 00176 * \note if return value is 0, the covariance matrix is not changed, thus not valid. 00177 * \ingroup common 00178 */ 00179 template <typename PointT, typename Scalar> inline unsigned int 00180 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00181 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00182 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00183 00184 template <typename PointT> inline unsigned int 00185 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00186 const Eigen::Vector4f ¢roid, 00187 Eigen::Matrix3f &covariance_matrix) 00188 { 00189 return (computeCovarianceMatrix<PointT, float> (cloud, centroid, covariance_matrix)); 00190 } 00191 00192 template <typename PointT> inline unsigned int 00193 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00194 const Eigen::Vector4d ¢roid, 00195 Eigen::Matrix3d &covariance_matrix) 00196 { 00197 return (computeCovarianceMatrix<PointT, double> (cloud, centroid, covariance_matrix)); 00198 } 00199 00200 /** \brief Compute normalized the 3x3 covariance matrix of a given set of points. 00201 * The result is returned as a Eigen::Matrix3f. 00202 * Normalized means that every entry has been divided by the number of points in the point cloud. 00203 * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix 00204 * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate 00205 * the covariance matrix and is returned by the computeCovarianceMatrix function. 00206 * \param[in] cloud the input point cloud 00207 * \param[in] centroid the centroid of the set of points in the cloud 00208 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00209 * \return number of valid point used to determine the covariance matrix. 00210 * In case of dense point clouds, this is the same as the size of input cloud. 00211 * \ingroup common 00212 */ 00213 template <typename PointT, typename Scalar> inline unsigned int 00214 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00215 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00216 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00217 00218 template <typename PointT> inline unsigned int 00219 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00220 const Eigen::Vector4f ¢roid, 00221 Eigen::Matrix3f &covariance_matrix) 00222 { 00223 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, centroid, covariance_matrix)); 00224 } 00225 00226 template <typename PointT> inline unsigned int 00227 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00228 const Eigen::Vector4d ¢roid, 00229 Eigen::Matrix3d &covariance_matrix) 00230 { 00231 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, centroid, covariance_matrix)); 00232 } 00233 00234 /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices. 00235 * The result is returned as a Eigen::Matrix3f. 00236 * Note: the covariance matrix is not normalized with the number of 00237 * points. For a normalized covariance, please use 00238 * computeNormalizedCovarianceMatrix. 00239 * \param[in] cloud the input point cloud 00240 * \param[in] indices the point cloud indices that need to be used 00241 * \param[in] centroid the centroid of the set of points in the cloud 00242 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00243 * \return number of valid point used to determine the covariance matrix. 00244 * In case of dense point clouds, this is the same as the size of input cloud. 00245 * \ingroup common 00246 */ 00247 template <typename PointT, typename Scalar> inline unsigned int 00248 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00249 const std::vector<int> &indices, 00250 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00251 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00252 00253 template <typename PointT> inline unsigned int 00254 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00255 const std::vector<int> &indices, 00256 const Eigen::Vector4f ¢roid, 00257 Eigen::Matrix3f &covariance_matrix) 00258 { 00259 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix)); 00260 } 00261 00262 template <typename PointT> inline unsigned int 00263 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00264 const std::vector<int> &indices, 00265 const Eigen::Vector4d ¢roid, 00266 Eigen::Matrix3d &covariance_matrix) 00267 { 00268 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix)); 00269 } 00270 00271 /** \brief Compute the 3x3 covariance matrix of a given set of points using their indices. 00272 * The result is returned as a Eigen::Matrix3f. 00273 * Note: the covariance matrix is not normalized with the number of 00274 * points. For a normalized covariance, please use 00275 * computeNormalizedCovarianceMatrix. 00276 * \param[in] cloud the input point cloud 00277 * \param[in] indices the point cloud indices that need to be used 00278 * \param[in] centroid the centroid of the set of points in the cloud 00279 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00280 * \return number of valid point used to determine the covariance matrix. 00281 * In case of dense point clouds, this is the same as the size of input cloud. 00282 * \ingroup common 00283 */ 00284 template <typename PointT, typename Scalar> inline unsigned int 00285 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00286 const pcl::PointIndices &indices, 00287 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00288 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00289 00290 template <typename PointT> inline unsigned int 00291 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00292 const pcl::PointIndices &indices, 00293 const Eigen::Vector4f ¢roid, 00294 Eigen::Matrix3f &covariance_matrix) 00295 { 00296 return (computeCovarianceMatrix<PointT, float> (cloud, indices, centroid, covariance_matrix)); 00297 } 00298 00299 template <typename PointT> inline unsigned int 00300 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00301 const pcl::PointIndices &indices, 00302 const Eigen::Vector4d ¢roid, 00303 Eigen::Matrix3d &covariance_matrix) 00304 { 00305 return (computeCovarianceMatrix<PointT, double> (cloud, indices, centroid, covariance_matrix)); 00306 } 00307 00308 /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using 00309 * their indices. 00310 * The result is returned as a Eigen::Matrix3f. 00311 * Normalized means that every entry has been divided by the number of entries in indices. 00312 * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix 00313 * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate 00314 * the covariance matrix and is returned by the computeCovarianceMatrix function. 00315 * \param[in] cloud the input point cloud 00316 * \param[in] indices the point cloud indices that need to be used 00317 * \param[in] centroid the centroid of the set of points in the cloud 00318 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00319 * \return number of valid point used to determine the covariance matrix. 00320 * In case of dense point clouds, this is the same as the size of input cloud. 00321 * \ingroup common 00322 */ 00323 template <typename PointT, typename Scalar> inline unsigned int 00324 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00325 const std::vector<int> &indices, 00326 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00327 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00328 00329 template <typename PointT> inline unsigned int 00330 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00331 const std::vector<int> &indices, 00332 const Eigen::Vector4f ¢roid, 00333 Eigen::Matrix3f &covariance_matrix) 00334 { 00335 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix)); 00336 } 00337 00338 template <typename PointT> inline unsigned int 00339 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00340 const std::vector<int> &indices, 00341 const Eigen::Vector4d ¢roid, 00342 Eigen::Matrix3d &covariance_matrix) 00343 { 00344 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix)); 00345 } 00346 00347 /** \brief Compute the normalized 3x3 covariance matrix of a given set of points using 00348 * their indices. The result is returned as a Eigen::Matrix3f. 00349 * Normalized means that every entry has been divided by the number of entries in indices. 00350 * For small number of points, or if you want explicitely the sample-variance, use computeCovarianceMatrix 00351 * and scale the covariance matrix with 1 / (n-1), where n is the number of points used to calculate 00352 * the covariance matrix and is returned by the computeCovarianceMatrix function. 00353 * \param[in] cloud the input point cloud 00354 * \param[in] indices the point cloud indices that need to be used 00355 * \param[in] centroid the centroid of the set of points in the cloud 00356 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00357 * \return number of valid point used to determine the covariance matrix. 00358 * In case of dense point clouds, this is the same as the size of input cloud. 00359 * \ingroup common 00360 */ 00361 template <typename PointT, typename Scalar> inline unsigned int 00362 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00363 const pcl::PointIndices &indices, 00364 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00365 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00366 00367 template <typename PointT> inline unsigned int 00368 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00369 const pcl::PointIndices &indices, 00370 const Eigen::Vector4f ¢roid, 00371 Eigen::Matrix3f &covariance_matrix) 00372 { 00373 return (computeCovarianceMatrixNormalized<PointT, float> (cloud, indices, centroid, covariance_matrix)); 00374 } 00375 00376 template <typename PointT> inline unsigned int 00377 computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00378 const pcl::PointIndices &indices, 00379 const Eigen::Vector4d ¢roid, 00380 Eigen::Matrix3d &covariance_matrix) 00381 { 00382 return (computeCovarianceMatrixNormalized<PointT, double> (cloud, indices, centroid, covariance_matrix)); 00383 } 00384 00385 /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop. 00386 * Normalized means that every entry has been divided by the number of entries in indices. 00387 * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix 00388 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. 00389 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. 00390 * \param[in] cloud the input point cloud 00391 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00392 * \param[out] centroid the centroid of the set of points in the cloud 00393 * \return number of valid point used to determine the covariance matrix. 00394 * In case of dense point clouds, this is the same as the size of input cloud. 00395 * \ingroup common 00396 */ 00397 template <typename PointT, typename Scalar> inline unsigned int 00398 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00399 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, 00400 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00401 00402 template <typename PointT> inline unsigned int 00403 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00404 Eigen::Matrix3f &covariance_matrix, 00405 Eigen::Vector4f ¢roid) 00406 { 00407 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, covariance_matrix, centroid)); 00408 } 00409 00410 template <typename PointT> inline unsigned int 00411 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00412 Eigen::Matrix3d &covariance_matrix, 00413 Eigen::Vector4d ¢roid) 00414 { 00415 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, covariance_matrix, centroid)); 00416 } 00417 00418 /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop. 00419 * Normalized means that every entry has been divided by the number of entries in indices. 00420 * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix 00421 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. 00422 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. 00423 * \param[in] cloud the input point cloud 00424 * \param[in] indices subset of points given by their indices 00425 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00426 * \param[out] centroid the centroid of the set of points in the cloud 00427 * \return number of valid point used to determine the covariance matrix. 00428 * In case of dense point clouds, this is the same as the size of input cloud. 00429 * \ingroup common 00430 */ 00431 template <typename PointT, typename Scalar> inline unsigned int 00432 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00433 const std::vector<int> &indices, 00434 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, 00435 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00436 00437 template <typename PointT> inline unsigned int 00438 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00439 const std::vector<int> &indices, 00440 Eigen::Matrix3f &covariance_matrix, 00441 Eigen::Vector4f ¢roid) 00442 { 00443 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid)); 00444 } 00445 00446 template <typename PointT> inline unsigned int 00447 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00448 const std::vector<int> &indices, 00449 Eigen::Matrix3d &covariance_matrix, 00450 Eigen::Vector4d ¢roid) 00451 { 00452 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid)); 00453 } 00454 00455 /** \brief Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single loop. 00456 * Normalized means that every entry has been divided by the number of entries in indices. 00457 * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix 00458 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. 00459 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. 00460 * \param[in] cloud the input point cloud 00461 * \param[in] indices subset of points given by their indices 00462 * \param[out] centroid the centroid of the set of points in the cloud 00463 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00464 * \return number of valid point used to determine the covariance matrix. 00465 * In case of dense point clouds, this is the same as the size of input cloud. 00466 * \ingroup common 00467 */ 00468 template <typename PointT, typename Scalar> inline unsigned int 00469 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00470 const pcl::PointIndices &indices, 00471 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, 00472 Eigen::Matrix<Scalar, 4, 1> ¢roid); 00473 00474 template <typename PointT> inline unsigned int 00475 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00476 const pcl::PointIndices &indices, 00477 Eigen::Matrix3f &covariance_matrix, 00478 Eigen::Vector4f ¢roid) 00479 { 00480 return (computeMeanAndCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix, centroid)); 00481 } 00482 00483 template <typename PointT> inline unsigned int 00484 computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00485 const pcl::PointIndices &indices, 00486 Eigen::Matrix3d &covariance_matrix, 00487 Eigen::Vector4d ¢roid) 00488 { 00489 return (computeMeanAndCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix, centroid)); 00490 } 00491 00492 /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud. 00493 * Normalized means that every entry has been divided by the number of entries in indices. 00494 * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix 00495 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. 00496 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. 00497 * \param[in] cloud the input point cloud 00498 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00499 * \return number of valid point used to determine the covariance matrix. 00500 * In case of dense point clouds, this is the same as the size of input cloud. 00501 * \ingroup common 00502 */ 00503 template <typename PointT, typename Scalar> inline unsigned int 00504 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00505 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00506 00507 template <typename PointT> inline unsigned int 00508 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00509 Eigen::Matrix3f &covariance_matrix) 00510 { 00511 return (computeCovarianceMatrix<PointT, float> (cloud, covariance_matrix)); 00512 } 00513 00514 template <typename PointT> inline unsigned int 00515 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00516 Eigen::Matrix3d &covariance_matrix) 00517 { 00518 return (computeCovarianceMatrix<PointT, double> (cloud, covariance_matrix)); 00519 } 00520 00521 /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud. 00522 * Normalized means that every entry has been divided by the number of entries in indices. 00523 * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix 00524 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. 00525 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. 00526 * \param[in] cloud the input point cloud 00527 * \param[in] indices subset of points given by their indices 00528 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00529 * \return number of valid point used to determine the covariance matrix. 00530 * In case of dense point clouds, this is the same as the size of input cloud. 00531 * \ingroup common 00532 */ 00533 template <typename PointT, typename Scalar> inline unsigned int 00534 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00535 const std::vector<int> &indices, 00536 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00537 00538 template <typename PointT> inline unsigned int 00539 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00540 const std::vector<int> &indices, 00541 Eigen::Matrix3f &covariance_matrix) 00542 { 00543 return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix)); 00544 } 00545 00546 template <typename PointT> inline unsigned int 00547 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00548 const std::vector<int> &indices, 00549 Eigen::Matrix3d &covariance_matrix) 00550 { 00551 return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix)); 00552 } 00553 00554 /** \brief Compute the normalized 3x3 covariance matrix for a already demeaned point cloud. 00555 * Normalized means that every entry has been divided by the number of entries in indices. 00556 * For small number of points, or if you want explicitely the sample-variance, scale the covariance matrix 00557 * with n / (n-1), where n is the number of points used to calculate the covariance matrix and is returned by this function. 00558 * \note This method is theoretically exact. However using float for internal calculations reduces the accuracy but increases the efficiency. 00559 * \param[in] cloud the input point cloud 00560 * \param[in] indices subset of points given by their indices 00561 * \param[out] covariance_matrix the resultant 3x3 covariance matrix 00562 * \return number of valid point used to determine the covariance matrix. 00563 * In case of dense point clouds, this is the same as the size of input cloud. 00564 * \ingroup common 00565 */ 00566 template <typename PointT, typename Scalar> inline unsigned int 00567 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00568 const pcl::PointIndices &indices, 00569 Eigen::Matrix<Scalar, 3, 3> &covariance_matrix); 00570 00571 template <typename PointT> inline unsigned int 00572 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00573 const pcl::PointIndices &indices, 00574 Eigen::Matrix3f &covariance_matrix) 00575 { 00576 return (computeCovarianceMatrix<PointT, float> (cloud, indices, covariance_matrix)); 00577 } 00578 00579 template <typename PointT> inline unsigned int 00580 computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00581 const pcl::PointIndices &indices, 00582 Eigen::Matrix3d &covariance_matrix) 00583 { 00584 return (computeCovarianceMatrix<PointT, double> (cloud, indices, covariance_matrix)); 00585 } 00586 00587 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation 00588 * \param[in] cloud_iterator an iterator over the input point cloud 00589 * \param[in] centroid the centroid of the point cloud 00590 * \param[out] cloud_out the resultant output point cloud 00591 * \param[in] npts the number of samples guaranteed to be left in the input cloud, accessible by the iterator. If not given, it will be calculated. 00592 * \ingroup common 00593 */ 00594 template <typename PointT, typename Scalar> void 00595 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00596 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00597 pcl::PointCloud<PointT> &cloud_out, 00598 int npts = 0); 00599 00600 template <typename PointT> void 00601 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00602 const Eigen::Vector4f ¢roid, 00603 pcl::PointCloud<PointT> &cloud_out, 00604 int npts = 0) 00605 { 00606 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts)); 00607 } 00608 00609 template <typename PointT> void 00610 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00611 const Eigen::Vector4d ¢roid, 00612 pcl::PointCloud<PointT> &cloud_out, 00613 int npts = 0) 00614 { 00615 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts)); 00616 } 00617 00618 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation 00619 * \param[in] cloud_in the input point cloud 00620 * \param[in] centroid the centroid of the point cloud 00621 * \param[out] cloud_out the resultant output point cloud 00622 * \ingroup common 00623 */ 00624 template <typename PointT, typename Scalar> void 00625 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00626 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00627 pcl::PointCloud<PointT> &cloud_out); 00628 00629 template <typename PointT> void 00630 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00631 const Eigen::Vector4f ¢roid, 00632 pcl::PointCloud<PointT> &cloud_out) 00633 { 00634 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out)); 00635 } 00636 00637 template <typename PointT> void 00638 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00639 const Eigen::Vector4d ¢roid, 00640 pcl::PointCloud<PointT> &cloud_out) 00641 { 00642 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out)); 00643 } 00644 00645 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation 00646 * \param[in] cloud_in the input point cloud 00647 * \param[in] indices the set of point indices to use from the input point cloud 00648 * \param[out] centroid the centroid of the point cloud 00649 * \param cloud_out the resultant output point cloud 00650 * \ingroup common 00651 */ 00652 template <typename PointT, typename Scalar> void 00653 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00654 const std::vector<int> &indices, 00655 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00656 pcl::PointCloud<PointT> &cloud_out); 00657 00658 template <typename PointT> void 00659 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00660 const std::vector<int> &indices, 00661 const Eigen::Vector4f ¢roid, 00662 pcl::PointCloud<PointT> &cloud_out) 00663 { 00664 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out)); 00665 } 00666 00667 template <typename PointT> void 00668 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00669 const std::vector<int> &indices, 00670 const Eigen::Vector4d ¢roid, 00671 pcl::PointCloud<PointT> &cloud_out) 00672 { 00673 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out)); 00674 } 00675 00676 /** \brief Subtract a centroid from a point cloud and return the de-meaned representation 00677 * \param[in] cloud_in the input point cloud 00678 * \param[in] indices the set of point indices to use from the input point cloud 00679 * \param[out] centroid the centroid of the point cloud 00680 * \param cloud_out the resultant output point cloud 00681 * \ingroup common 00682 */ 00683 template <typename PointT, typename Scalar> void 00684 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00685 const pcl::PointIndices& indices, 00686 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00687 pcl::PointCloud<PointT> &cloud_out); 00688 00689 template <typename PointT> void 00690 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00691 const pcl::PointIndices& indices, 00692 const Eigen::Vector4f ¢roid, 00693 pcl::PointCloud<PointT> &cloud_out) 00694 { 00695 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out)); 00696 } 00697 00698 template <typename PointT> void 00699 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00700 const pcl::PointIndices& indices, 00701 const Eigen::Vector4d ¢roid, 00702 pcl::PointCloud<PointT> &cloud_out) 00703 { 00704 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out)); 00705 } 00706 00707 /** \brief Subtract a centroid from a point cloud and return the de-meaned 00708 * representation as an Eigen matrix 00709 * \param[in] cloud_iterator an iterator over the input point cloud 00710 * \param[in] centroid the centroid of the point cloud 00711 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as 00712 * an Eigen matrix (4 rows, N pts columns) 00713 * \param[in] npts the number of samples guaranteed to be left in the input cloud, accessible by the iterator. If not given, it will be calculated. 00714 * \ingroup common 00715 */ 00716 template <typename PointT, typename Scalar> void 00717 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00718 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00719 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out, 00720 int npts = 0); 00721 00722 template <typename PointT> void 00723 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00724 const Eigen::Vector4f ¢roid, 00725 Eigen::MatrixXf &cloud_out, 00726 int npts = 0) 00727 { 00728 return (demeanPointCloud<PointT, float> (cloud_iterator, centroid, cloud_out, npts)); 00729 } 00730 00731 template <typename PointT> void 00732 demeanPointCloud (ConstCloudIterator<PointT> &cloud_iterator, 00733 const Eigen::Vector4d ¢roid, 00734 Eigen::MatrixXd &cloud_out, 00735 int npts = 0) 00736 { 00737 return (demeanPointCloud<PointT, double> (cloud_iterator, centroid, cloud_out, npts)); 00738 } 00739 00740 /** \brief Subtract a centroid from a point cloud and return the de-meaned 00741 * representation as an Eigen matrix 00742 * \param[in] cloud_in the input point cloud 00743 * \param[in] centroid the centroid of the point cloud 00744 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as 00745 * an Eigen matrix (4 rows, N pts columns) 00746 * \ingroup common 00747 */ 00748 template <typename PointT, typename Scalar> void 00749 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00750 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00751 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out); 00752 00753 template <typename PointT> void 00754 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00755 const Eigen::Vector4f ¢roid, 00756 Eigen::MatrixXf &cloud_out) 00757 { 00758 return (demeanPointCloud<PointT, float> (cloud_in, centroid, cloud_out)); 00759 } 00760 00761 template <typename PointT> void 00762 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00763 const Eigen::Vector4d ¢roid, 00764 Eigen::MatrixXd &cloud_out) 00765 { 00766 return (demeanPointCloud<PointT, double> (cloud_in, centroid, cloud_out)); 00767 } 00768 00769 /** \brief Subtract a centroid from a point cloud and return the de-meaned 00770 * representation as an Eigen matrix 00771 * \param[in] cloud_in the input point cloud 00772 * \param[in] indices the set of point indices to use from the input point cloud 00773 * \param[in] centroid the centroid of the point cloud 00774 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as 00775 * an Eigen matrix (4 rows, N pts columns) 00776 * \ingroup common 00777 */ 00778 template <typename PointT, typename Scalar> void 00779 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00780 const std::vector<int> &indices, 00781 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00782 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out); 00783 00784 template <typename PointT> void 00785 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00786 const std::vector<int> &indices, 00787 const Eigen::Vector4f ¢roid, 00788 Eigen::MatrixXf &cloud_out) 00789 { 00790 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out)); 00791 } 00792 00793 template <typename PointT> void 00794 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00795 const std::vector<int> &indices, 00796 const Eigen::Vector4d ¢roid, 00797 Eigen::MatrixXd &cloud_out) 00798 { 00799 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out)); 00800 } 00801 00802 /** \brief Subtract a centroid from a point cloud and return the de-meaned 00803 * representation as an Eigen matrix 00804 * \param[in] cloud_in the input point cloud 00805 * \param[in] indices the set of point indices to use from the input point cloud 00806 * \param[in] centroid the centroid of the point cloud 00807 * \param[out] cloud_out the resultant output XYZ0 dimensions of \a cloud_in as 00808 * an Eigen matrix (4 rows, N pts columns) 00809 * \ingroup common 00810 */ 00811 template <typename PointT, typename Scalar> void 00812 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00813 const pcl::PointIndices& indices, 00814 const Eigen::Matrix<Scalar, 4, 1> ¢roid, 00815 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out); 00816 00817 template <typename PointT> void 00818 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00819 const pcl::PointIndices& indices, 00820 const Eigen::Vector4f ¢roid, 00821 Eigen::MatrixXf &cloud_out) 00822 { 00823 return (demeanPointCloud<PointT, float> (cloud_in, indices, centroid, cloud_out)); 00824 } 00825 00826 template <typename PointT> void 00827 demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00828 const pcl::PointIndices& indices, 00829 const Eigen::Vector4d ¢roid, 00830 Eigen::MatrixXd &cloud_out) 00831 { 00832 return (demeanPointCloud<PointT, double> (cloud_in, indices, centroid, cloud_out)); 00833 } 00834 00835 /** \brief Helper functor structure for n-D centroid estimation. */ 00836 template<typename PointT, typename Scalar> 00837 struct NdCentroidFunctor 00838 { 00839 typedef typename traits::POD<PointT>::type Pod; 00840 00841 NdCentroidFunctor (const PointT &p, Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid) 00842 : f_idx_ (0), 00843 centroid_ (centroid), 00844 p_ (reinterpret_cast<const Pod&>(p)) { } 00845 00846 template<typename Key> inline void operator() () 00847 { 00848 typedef typename pcl::traits::datatype<PointT, Key>::type T; 00849 const uint8_t* raw_ptr = reinterpret_cast<const uint8_t*>(&p_) + pcl::traits::offset<PointT, Key>::value; 00850 const T* data_ptr = reinterpret_cast<const T*>(raw_ptr); 00851 00852 // Check if the value is invalid 00853 if (!pcl_isfinite (*data_ptr)) 00854 { 00855 f_idx_++; 00856 return; 00857 } 00858 00859 centroid_[f_idx_++] += *data_ptr; 00860 } 00861 00862 private: 00863 int f_idx_; 00864 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid_; 00865 const Pod &p_; 00866 }; 00867 00868 /** \brief General, all purpose nD centroid estimation for a set of points using their 00869 * indices. 00870 * \param cloud the input point cloud 00871 * \param centroid the output centroid 00872 * \ingroup common 00873 */ 00874 template <typename PointT, typename Scalar> inline void 00875 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00876 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid); 00877 00878 template <typename PointT> inline void 00879 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00880 Eigen::VectorXf ¢roid) 00881 { 00882 return (computeNDCentroid<PointT, float> (cloud, centroid)); 00883 } 00884 00885 template <typename PointT> inline void 00886 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00887 Eigen::VectorXd ¢roid) 00888 { 00889 return (computeNDCentroid<PointT, double> (cloud, centroid)); 00890 } 00891 00892 /** \brief General, all purpose nD centroid estimation for a set of points using their 00893 * indices. 00894 * \param cloud the input point cloud 00895 * \param indices the point cloud indices that need to be used 00896 * \param centroid the output centroid 00897 * \ingroup common 00898 */ 00899 template <typename PointT, typename Scalar> inline void 00900 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00901 const std::vector<int> &indices, 00902 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid); 00903 00904 template <typename PointT> inline void 00905 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00906 const std::vector<int> &indices, 00907 Eigen::VectorXf ¢roid) 00908 { 00909 return (computeNDCentroid<PointT, float> (cloud, indices, centroid)); 00910 } 00911 00912 template <typename PointT> inline void 00913 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00914 const std::vector<int> &indices, 00915 Eigen::VectorXd ¢roid) 00916 { 00917 return (computeNDCentroid<PointT, double> (cloud, indices, centroid)); 00918 } 00919 00920 /** \brief General, all purpose nD centroid estimation for a set of points using their 00921 * indices. 00922 * \param cloud the input point cloud 00923 * \param indices the point cloud indices that need to be used 00924 * \param centroid the output centroid 00925 * \ingroup common 00926 */ 00927 template <typename PointT, typename Scalar> inline void 00928 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00929 const pcl::PointIndices &indices, 00930 Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid); 00931 00932 template <typename PointT> inline void 00933 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00934 const pcl::PointIndices &indices, 00935 Eigen::VectorXf ¢roid) 00936 { 00937 return (computeNDCentroid<PointT, float> (cloud, indices, centroid)); 00938 } 00939 00940 template <typename PointT> inline void 00941 computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00942 const pcl::PointIndices &indices, 00943 Eigen::VectorXd ¢roid) 00944 { 00945 return (computeNDCentroid<PointT, double> (cloud, indices, centroid)); 00946 } 00947 00948 } 00949 /*@}*/ 00950 #include <pcl/common/impl/centroid.hpp> 00951 00952 #endif //#ifndef PCL_COMMON_CENTROID_H_