Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, 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 the copyright holder(s) 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$ 00035 * 00036 */ 00037 00038 #ifndef PCL_COMMON_IMPL_H_ 00039 #define PCL_COMMON_IMPL_H_ 00040 00041 #include <pcl/point_types.h> 00042 #include <pcl/common/common.h> 00043 00044 ////////////////////////////////////////////////////////////////////////////////////////////// 00045 inline double 00046 pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2) 00047 { 00048 // Compute the actual angle 00049 double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ()); 00050 if (rad < -1.0) rad = -1.0; 00051 if (rad > 1.0) rad = 1.0; 00052 return (acos (rad)); 00053 } 00054 00055 ////////////////////////////////////////////////////////////////////////////////////////////// 00056 inline void 00057 pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev) 00058 { 00059 double sum = 0, sq_sum = 0; 00060 00061 for (size_t i = 0; i < values.size (); ++i) 00062 { 00063 sum += values[i]; 00064 sq_sum += values[i] * values[i]; 00065 } 00066 mean = sum / static_cast<double>(values.size ()); 00067 double variance = (sq_sum - sum * sum / static_cast<double>(values.size ())) / (static_cast<double>(values.size ()) - 1); 00068 stddev = sqrt (variance); 00069 } 00070 00071 ////////////////////////////////////////////////////////////////////////////////////////////// 00072 template <typename PointT> inline void 00073 pcl::getPointsInBox (const pcl::PointCloud<PointT> &cloud, 00074 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, 00075 std::vector<int> &indices) 00076 { 00077 indices.resize (cloud.points.size ()); 00078 int l = 0; 00079 00080 // If the data is dense, we don't need to check for NaN 00081 if (cloud.is_dense) 00082 { 00083 for (size_t i = 0; i < cloud.points.size (); ++i) 00084 { 00085 // Check if the point is inside bounds 00086 if (cloud.points[i].x < min_pt[0] || cloud.points[i].y < min_pt[1] || cloud.points[i].z < min_pt[2]) 00087 continue; 00088 if (cloud.points[i].x > max_pt[0] || cloud.points[i].y > max_pt[1] || cloud.points[i].z > max_pt[2]) 00089 continue; 00090 indices[l++] = int (i); 00091 } 00092 } 00093 // NaN or Inf values could exist => check for them 00094 else 00095 { 00096 for (size_t i = 0; i < cloud.points.size (); ++i) 00097 { 00098 // Check if the point is invalid 00099 if (!pcl_isfinite (cloud.points[i].x) || 00100 !pcl_isfinite (cloud.points[i].y) || 00101 !pcl_isfinite (cloud.points[i].z)) 00102 continue; 00103 // Check if the point is inside bounds 00104 if (cloud.points[i].x < min_pt[0] || cloud.points[i].y < min_pt[1] || cloud.points[i].z < min_pt[2]) 00105 continue; 00106 if (cloud.points[i].x > max_pt[0] || cloud.points[i].y > max_pt[1] || cloud.points[i].z > max_pt[2]) 00107 continue; 00108 indices[l++] = int (i); 00109 } 00110 } 00111 indices.resize (l); 00112 } 00113 00114 ////////////////////////////////////////////////////////////////////////////////////////////// 00115 template<typename PointT> inline void 00116 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt) 00117 { 00118 float max_dist = -FLT_MAX; 00119 int max_idx = -1; 00120 float dist; 00121 00122 // If the data is dense, we don't need to check for NaN 00123 if (cloud.is_dense) 00124 { 00125 for (size_t i = 0; i < cloud.points.size (); ++i) 00126 { 00127 pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap (); 00128 dist = (pivot_pt - pt).norm (); 00129 if (dist > max_dist) 00130 { 00131 max_idx = int (i); 00132 max_dist = dist; 00133 } 00134 } 00135 } 00136 // NaN or Inf values could exist => check for them 00137 else 00138 { 00139 for (size_t i = 0; i < cloud.points.size (); ++i) 00140 { 00141 // Check if the point is invalid 00142 if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z)) 00143 continue; 00144 pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap (); 00145 dist = (pivot_pt - pt).norm (); 00146 if (dist > max_dist) 00147 { 00148 max_idx = int (i); 00149 max_dist = dist; 00150 } 00151 } 00152 } 00153 00154 if(max_idx != -1) 00155 max_pt = cloud.points[max_idx].getVector4fMap (); 00156 else 00157 max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN()); 00158 } 00159 00160 ////////////////////////////////////////////////////////////////////////////////////////////// 00161 template<typename PointT> inline void 00162 pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 00163 const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt) 00164 { 00165 float max_dist = -FLT_MAX; 00166 int max_idx = -1; 00167 float dist; 00168 00169 // If the data is dense, we don't need to check for NaN 00170 if (cloud.is_dense) 00171 { 00172 for (size_t i = 0; i < indices.size (); ++i) 00173 { 00174 pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap (); 00175 dist = (pivot_pt - pt).norm (); 00176 if (dist > max_dist) 00177 { 00178 max_idx = static_cast<int> (i); 00179 max_dist = dist; 00180 } 00181 } 00182 } 00183 // NaN or Inf values could exist => check for them 00184 else 00185 { 00186 for (size_t i = 0; i < indices.size (); ++i) 00187 { 00188 // Check if the point is invalid 00189 if (!pcl_isfinite (cloud.points[indices[i]].x) || !pcl_isfinite (cloud.points[indices[i]].y) 00190 || 00191 !pcl_isfinite (cloud.points[indices[i]].z)) 00192 continue; 00193 00194 pcl::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap (); 00195 dist = (pivot_pt - pt).norm (); 00196 if (dist > max_dist) 00197 { 00198 max_idx = static_cast<int> (i); 00199 max_dist = dist; 00200 } 00201 } 00202 } 00203 00204 if(max_idx != -1) 00205 max_pt = cloud.points[max_idx].getVector4fMap (); 00206 else 00207 max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN()); 00208 } 00209 00210 ////////////////////////////////////////////////////////////////////////////////////////////// 00211 template <typename PointT> inline void 00212 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt) 00213 { 00214 Eigen::Array4f min_p, max_p; 00215 min_p.setConstant (FLT_MAX); 00216 max_p.setConstant (-FLT_MAX); 00217 00218 // If the data is dense, we don't need to check for NaN 00219 if (cloud.is_dense) 00220 { 00221 for (size_t i = 0; i < cloud.points.size (); ++i) 00222 { 00223 pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap (); 00224 min_p = min_p.min (pt); 00225 max_p = max_p.max (pt); 00226 } 00227 } 00228 // NaN or Inf values could exist => check for them 00229 else 00230 { 00231 for (size_t i = 0; i < cloud.points.size (); ++i) 00232 { 00233 // Check if the point is invalid 00234 if (!pcl_isfinite (cloud.points[i].x) || 00235 !pcl_isfinite (cloud.points[i].y) || 00236 !pcl_isfinite (cloud.points[i].z)) 00237 continue; 00238 pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap (); 00239 min_p = min_p.min (pt); 00240 max_p = max_p.max (pt); 00241 } 00242 } 00243 min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2]; 00244 max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2]; 00245 } 00246 00247 ////////////////////////////////////////////////////////////////////////////////////////////// 00248 template <typename PointT> inline void 00249 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) 00250 { 00251 Eigen::Array4f min_p, max_p; 00252 min_p.setConstant (FLT_MAX); 00253 max_p.setConstant (-FLT_MAX); 00254 00255 // If the data is dense, we don't need to check for NaN 00256 if (cloud.is_dense) 00257 { 00258 for (size_t i = 0; i < cloud.points.size (); ++i) 00259 { 00260 pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap (); 00261 min_p = min_p.min (pt); 00262 max_p = max_p.max (pt); 00263 } 00264 } 00265 // NaN or Inf values could exist => check for them 00266 else 00267 { 00268 for (size_t i = 0; i < cloud.points.size (); ++i) 00269 { 00270 // Check if the point is invalid 00271 if (!pcl_isfinite (cloud.points[i].x) || 00272 !pcl_isfinite (cloud.points[i].y) || 00273 !pcl_isfinite (cloud.points[i].z)) 00274 continue; 00275 pcl::Array4fMapConst pt = cloud.points[i].getArray4fMap (); 00276 min_p = min_p.min (pt); 00277 max_p = max_p.max (pt); 00278 } 00279 } 00280 min_pt = min_p; 00281 max_pt = max_p; 00282 } 00283 00284 00285 ////////////////////////////////////////////////////////////////////////////////////////////// 00286 template <typename PointT> inline void 00287 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices, 00288 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) 00289 { 00290 Eigen::Array4f min_p, max_p; 00291 min_p.setConstant (FLT_MAX); 00292 max_p.setConstant (-FLT_MAX); 00293 00294 // If the data is dense, we don't need to check for NaN 00295 if (cloud.is_dense) 00296 { 00297 for (size_t i = 0; i < indices.indices.size (); ++i) 00298 { 00299 pcl::Array4fMapConst pt = cloud.points[indices.indices[i]].getArray4fMap (); 00300 min_p = min_p.min (pt); 00301 max_p = max_p.max (pt); 00302 } 00303 } 00304 // NaN or Inf values could exist => check for them 00305 else 00306 { 00307 for (size_t i = 0; i < indices.indices.size (); ++i) 00308 { 00309 // Check if the point is invalid 00310 if (!pcl_isfinite (cloud.points[indices.indices[i]].x) || 00311 !pcl_isfinite (cloud.points[indices.indices[i]].y) || 00312 !pcl_isfinite (cloud.points[indices.indices[i]].z)) 00313 continue; 00314 pcl::Array4fMapConst pt = cloud.points[indices.indices[i]].getArray4fMap (); 00315 min_p = min_p.min (pt); 00316 max_p = max_p.max (pt); 00317 } 00318 } 00319 min_pt = min_p; 00320 max_pt = max_p; 00321 } 00322 00323 ////////////////////////////////////////////////////////////////////////////////////////////// 00324 template <typename PointT> inline void 00325 pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, 00326 Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) 00327 { 00328 min_pt.setConstant (FLT_MAX); 00329 max_pt.setConstant (-FLT_MAX); 00330 00331 // If the data is dense, we don't need to check for NaN 00332 if (cloud.is_dense) 00333 { 00334 for (size_t i = 0; i < indices.size (); ++i) 00335 { 00336 pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap (); 00337 min_pt = min_pt.array ().min (pt); 00338 max_pt = max_pt.array ().max (pt); 00339 } 00340 } 00341 // NaN or Inf values could exist => check for them 00342 else 00343 { 00344 for (size_t i = 0; i < indices.size (); ++i) 00345 { 00346 // Check if the point is invalid 00347 if (!pcl_isfinite (cloud.points[indices[i]].x) || 00348 !pcl_isfinite (cloud.points[indices[i]].y) || 00349 !pcl_isfinite (cloud.points[indices[i]].z)) 00350 continue; 00351 pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap (); 00352 min_pt = min_pt.array ().min (pt); 00353 max_pt = max_pt.array ().max (pt); 00354 } 00355 } 00356 } 00357 00358 ////////////////////////////////////////////////////////////////////////////////////////////// 00359 template <typename PointT> inline double 00360 pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc) 00361 { 00362 Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0); 00363 Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0); 00364 Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0); 00365 00366 double p2p1 = (p2 - p1).norm (), p3p2 = (p3 - p2).norm (), p1p3 = (p1 - p3).norm (); 00367 // Calculate the area of the triangle using Heron's formula 00368 // (http://en.wikipedia.org/wiki/Heron's_formula) 00369 double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0; 00370 double area = sqrt (semiperimeter * (semiperimeter - p2p1) * (semiperimeter - p3p2) * (semiperimeter - p1p3)); 00371 // Compute the radius of the circumscribed circle 00372 return ((p2p1 * p3p2 * p1p3) / (4.0 * area)); 00373 } 00374 00375 ////////////////////////////////////////////////////////////////////////////////////////////// 00376 template <typename PointT> inline void 00377 pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p) 00378 { 00379 min_p = FLT_MAX; 00380 max_p = -FLT_MAX; 00381 00382 for (int i = 0; i < len; ++i) 00383 { 00384 min_p = (histogram[i] > min_p) ? min_p : histogram[i]; 00385 max_p = (histogram[i] < max_p) ? max_p : histogram[i]; 00386 } 00387 } 00388 00389 ////////////////////////////////////////////////////////////////////////////////////////////// 00390 template <typename PointT> inline float 00391 pcl::calculatePolygonArea (const pcl::PointCloud<PointT> &polygon) 00392 { 00393 float area = 0.0f; 00394 int num_points = polygon.size (); 00395 int j = 0; 00396 Eigen::Vector3f va,vb,res; 00397 00398 res(0) = res(1) = res(2) = 0.0f; 00399 for (int i = 0; i < num_points; ++i) 00400 { 00401 j = (i + 1) % num_points; 00402 va = polygon[i].getVector3fMap (); 00403 vb = polygon[j].getVector3fMap (); 00404 res += va.cross (vb); 00405 } 00406 area = res.norm (); 00407 return (area*0.5); 00408 } 00409 00410 #endif //#ifndef PCL_COMMON_IMPL_H_ 00411