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