Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009-present, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: centroid.hpp 3317 2011-12-02 11:17:47Z mdixon $ 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 void 00046 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f ¢roid) 00047 { 00048 // Initialize to 0 00049 centroid.setZero (); 00050 if (cloud.points.empty ()) 00051 return; 00052 // For each point in the cloud 00053 int cp = 0; 00054 00055 // If the data is dense, we don't need to check for NaN 00056 if (cloud.is_dense) 00057 { 00058 for (size_t i = 0; i < cloud.points.size (); ++i) 00059 centroid += cloud.points[i].getVector4fMap (); 00060 centroid[3] = 0; 00061 centroid /= cloud.points.size (); 00062 } 00063 // NaN or Inf values could exist => check for them 00064 else 00065 { 00066 for (size_t i = 0; i < cloud.points.size (); ++i) 00067 { 00068 // Check if the point is invalid 00069 if (!pcl_isfinite (cloud.points[i].x) || 00070 !pcl_isfinite (cloud.points[i].y) || 00071 !pcl_isfinite (cloud.points[i].z)) 00072 continue; 00073 00074 centroid += cloud.points[i].getVector4fMap (); 00075 cp++; 00076 } 00077 centroid[3] = 0; 00078 centroid /= cp; 00079 } 00080 } 00081 00083 template <typename PointT> inline void 00084 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 00085 Eigen::Vector4f ¢roid) 00086 { 00087 // Initialize to 0 00088 centroid.setZero (); 00089 if (indices.empty ()) 00090 return; 00091 // For each point in the cloud 00092 int cp = 0; 00093 00094 // If the data is dense, we don't need to check for NaN 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 /= (float) indices.size (); 00101 } 00102 // NaN or Inf values could exist => check for them 00103 else 00104 { 00105 for (size_t i = 0; i < indices.size (); ++i) 00106 { 00107 // Check if the point is invalid 00108 if (!pcl_isfinite (cloud.points[indices[i]].x) || 00109 !pcl_isfinite (cloud.points[indices[i]].y) || 00110 !pcl_isfinite (cloud.points[indices[i]].z)) 00111 continue; 00112 00113 centroid += cloud.points[indices[i]].getVector4fMap (); 00114 cp++; 00115 } 00116 centroid[3] = 0.0f; 00117 centroid /= (float) cp; 00118 } 00119 } 00120 00122 template <typename PointT> inline void 00123 pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 00124 const pcl::PointIndices &indices, Eigen::Vector4f ¢roid) 00125 { 00126 return (pcl::compute3DCentroid<PointT> (cloud, indices.indices, centroid)); 00127 } 00128 00130 template <typename PointT> inline void 00131 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00132 const Eigen::Vector4f ¢roid, 00133 Eigen::Matrix3f &covariance_matrix) 00134 { 00135 // Initialize to 0 00136 covariance_matrix.setZero (); 00137 00138 if (cloud.points.empty ()) 00139 return; 00140 // If the data is dense, we don't need to check for NaN 00141 if (cloud.is_dense) 00142 { 00143 // For each point in the cloud 00144 for (size_t i = 0; i < cloud.points.size (); ++i) 00145 { 00146 Eigen::Vector4f pt = cloud.points[i].getVector4fMap () - centroid; 00147 00148 covariance_matrix (1, 1) += pt.y () * pt.y (); 00149 covariance_matrix (1, 2) += pt.y () * pt.z (); 00150 00151 covariance_matrix (2, 2) += pt.z () * pt.z (); 00152 00153 pt *= pt.x (); 00154 covariance_matrix (0, 0) += pt.x (); 00155 covariance_matrix (0, 1) += pt.y (); 00156 covariance_matrix (0, 2) += pt.z (); 00157 } 00158 } 00159 // NaN or Inf values could exist => check for them 00160 else 00161 { 00162 // For each point in the cloud 00163 for (size_t i = 0; i < cloud.points.size (); ++i) 00164 { 00165 // Check if the point is invalid 00166 if (!pcl_isfinite (cloud.points[i].x) || 00167 !pcl_isfinite (cloud.points[i].y) || 00168 !pcl_isfinite (cloud.points[i].z)) 00169 continue; 00170 00171 Eigen::Vector4f pt = cloud.points[i].getVector4fMap () - centroid; 00172 00173 covariance_matrix (1, 1) += pt.y () * pt.y (); 00174 covariance_matrix (1, 2) += pt.y () * pt.z (); 00175 00176 covariance_matrix (2, 2) += pt.z () * pt.z (); 00177 00178 pt *= pt.x (); 00179 covariance_matrix (0, 0) += pt.x (); 00180 covariance_matrix (0, 1) += pt.y (); 00181 covariance_matrix (0, 2) += pt.z (); 00182 } 00183 } 00184 covariance_matrix (1, 0) = covariance_matrix (0, 1); 00185 covariance_matrix (2, 0) = covariance_matrix (0, 2); 00186 covariance_matrix (2, 1) = covariance_matrix (1, 2); 00187 } 00188 00190 template <typename PointT> inline void 00191 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00192 const Eigen::Vector4f ¢roid, 00193 Eigen::Matrix3f &covariance_matrix) 00194 { 00195 pcl::computeCovarianceMatrix (cloud, centroid, covariance_matrix); 00196 covariance_matrix /= cloud.points.size (); 00197 } 00198 00200 template <typename PointT> inline void 00201 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00202 const std::vector<int> &indices, 00203 const Eigen::Vector4f ¢roid, 00204 Eigen::Matrix3f &covariance_matrix) 00205 { 00206 // Initialize to 0 00207 covariance_matrix.setZero (); 00208 00209 if (indices.empty ()) 00210 return; 00211 // If the data is dense, we don't need to check for NaN 00212 if (cloud.is_dense) 00213 { 00214 // For each point in the cloud 00215 for (size_t i = 0; i < indices.size (); ++i) 00216 { 00217 Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid; 00218 00219 covariance_matrix (1, 1) += pt.y () * pt.y (); 00220 covariance_matrix (1, 2) += pt.y () * pt.z (); 00221 00222 covariance_matrix (2, 2) += pt.z () * pt.z (); 00223 00224 pt *= pt.x (); 00225 covariance_matrix (0, 0) += pt.x (); 00226 covariance_matrix (0, 1) += pt.y (); 00227 covariance_matrix (0, 2) += pt.z (); 00228 } 00229 } 00230 // NaN or Inf values could exist => check for them 00231 else 00232 { 00233 // For each point in the cloud 00234 for (size_t i = 0; i < indices.size (); ++i) 00235 { 00236 // Check if the point is invalid 00237 if (!pcl_isfinite (cloud.points[indices[i]].x) || 00238 !pcl_isfinite (cloud.points[indices[i]].y) || 00239 !pcl_isfinite (cloud.points[indices[i]].z)) 00240 continue; 00241 00242 Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid; 00243 00244 covariance_matrix (1, 1) += pt.y () * pt.y (); 00245 covariance_matrix (1, 2) += pt.y () * pt.z (); 00246 00247 covariance_matrix (2, 2) += pt.z () * pt.z (); 00248 00249 pt *= pt.x (); 00250 covariance_matrix (0, 0) += pt.x (); 00251 covariance_matrix (0, 1) += pt.y (); 00252 covariance_matrix (0, 2) += pt.z (); 00253 } 00254 } 00255 covariance_matrix (1, 0) = covariance_matrix (0, 1); 00256 covariance_matrix (2, 0) = covariance_matrix (0, 2); 00257 covariance_matrix (2, 1) = covariance_matrix (1, 2); 00258 } 00259 00261 template <typename PointT> inline void 00262 pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 00263 const pcl::PointIndices &indices, 00264 const Eigen::Vector4f ¢roid, 00265 Eigen::Matrix3f &covariance_matrix) 00266 { 00267 return (pcl::computeCovarianceMatrix<PointT> (cloud, indices.indices, centroid)); 00268 } 00269 00271 template <typename PointT> inline void 00272 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00273 const std::vector<int> &indices, 00274 const Eigen::Vector4f ¢roid, 00275 Eigen::Matrix3f &covariance_matrix) 00276 { 00277 pcl::computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix); 00278 covariance_matrix /= indices.size (); 00279 } 00280 00282 template <typename PointT> inline void 00283 pcl::computeCovarianceMatrixNormalized (const pcl::PointCloud<PointT> &cloud, 00284 const pcl::PointIndices &indices, 00285 const Eigen::Vector4f ¢roid, 00286 Eigen::Matrix3f &covariance_matrix) 00287 { 00288 pcl::computeCovarianceMatrix (cloud, indices.indices, centroid, covariance_matrix); 00289 covariance_matrix /= indices.indices.size (); 00290 } 00291 00293 template <typename PointT> void 00294 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00295 const Eigen::Vector4f ¢roid, 00296 pcl::PointCloud<PointT> &cloud_out) 00297 { 00298 cloud_out = cloud_in; 00299 00300 // Subtract the centroid from cloud_in 00301 for (size_t i = 0; i < cloud_in.points.size (); ++i) 00302 cloud_out.points[i].getVector4fMap () -= centroid; 00303 } 00304 00306 template <typename PointT> void 00307 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00308 const std::vector<int> &indices, 00309 const Eigen::Vector4f ¢roid, 00310 pcl::PointCloud<PointT> &cloud_out) 00311 { 00312 cloud_out.header = cloud_in.header; 00313 cloud_out.is_dense = cloud_in.is_dense; 00314 if (indices.size () == cloud_in.points.size ()) 00315 { 00316 cloud_out.width = cloud_in.width; 00317 cloud_out.height = cloud_in.height; 00318 } 00319 else 00320 { 00321 cloud_out.width = indices.size (); 00322 cloud_out.height = 1; 00323 } 00324 cloud_out.points.resize (indices.size ()); 00325 00326 // Subtract the centroid from cloud_in 00327 for (size_t i = 0; i < indices.size (); ++i) 00328 cloud_out.points[i].getVector4fMap () = cloud_in.points[indices[i]].getVector4fMap () - 00329 centroid; 00330 } 00331 00333 template <typename PointT> void 00334 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00335 const Eigen::Vector4f ¢roid, 00336 Eigen::MatrixXf &cloud_out) 00337 { 00338 size_t npts = cloud_in.points.size (); 00339 00340 cloud_out = Eigen::MatrixXf::Zero (4, npts); // keep the data aligned 00341 00342 for (size_t i = 0; i < npts; ++i) 00343 // One column at a time 00344 cloud_out.block<4, 1> (0, i) = cloud_in.points[i].getVector4fMap () - centroid; 00345 00346 // Make sure we zero the 4th dimension out (1 row, N columns) 00347 cloud_out.block (3, 0, 1, npts).setZero (); 00348 } 00349 00351 template <typename PointT> void 00352 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00353 const std::vector<int> &indices, 00354 const Eigen::Vector4f ¢roid, 00355 Eigen::MatrixXf &cloud_out) 00356 { 00357 size_t npts = indices.size (); 00358 00359 cloud_out = Eigen::MatrixXf::Zero (4, npts); // keep the data aligned 00360 00361 for (size_t i = 0; i < npts; ++i) 00362 // One column at a time 00363 cloud_out.block<4, 1> (0, i) = cloud_in.points[indices[i]].getVector4fMap () - centroid; 00364 00365 // Make sure we zero the 4th dimension out (1 row, N columns) 00366 cloud_out.block (3, 0, 1, npts).setZero (); 00367 } 00368 00370 template <typename PointT> void 00371 pcl::demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in, 00372 const pcl::PointIndices &indices, 00373 const Eigen::Vector4f ¢roid, 00374 Eigen::MatrixXf &cloud_out) 00375 { 00376 return (pcl::demeanPointCloud (cloud_in, indices.indices, centroid, cloud_out)); 00377 } 00378 00380 template <typename PointT> inline void 00381 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::VectorXf ¢roid) 00382 { 00383 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00384 00385 // Get the size of the fields 00386 centroid.setZero (boost::mpl::size<FieldList>::value); 00387 00388 if (cloud.points.empty ()) 00389 return; 00390 // Iterate over each point 00391 int size = cloud.points.size (); 00392 for (int i = 0; i < size; ++i) 00393 { 00394 // Iterate over each dimension 00395 pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[i], centroid)); 00396 } 00397 centroid /= size; 00398 } 00399 00401 template <typename PointT> inline void 00402 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 00403 Eigen::VectorXf ¢roid) 00404 { 00405 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00406 00407 // Get the size of the fields 00408 centroid.setZero (boost::mpl::size<FieldList>::value); 00409 00410 if (indices.empty ()) 00411 return; 00412 // Iterate over each point 00413 int nr_points = indices.size (); 00414 for (int i = 0; i < nr_points; ++i) 00415 { 00416 // Iterate over each dimension 00417 pcl::for_each_type <FieldList> (NdCentroidFunctor <PointT> (cloud.points[indices[i]], centroid)); 00418 } 00419 centroid /= nr_points; 00420 } 00421 00423 template <typename PointT> inline void 00424 pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 00425 const pcl::PointIndices &indices, Eigen::VectorXf ¢roid) 00426 { 00427 return (pcl::computeNDCentroid<PointT> (cloud, indices.indices, centroid)); 00428 } 00429 00430 #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_ 00431