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