common.hpp
Go to the documentation of this file.
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:22:49