distances.h
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 #ifndef PCL_DISTANCES_H_
00038 #define PCL_DISTANCES_H_
00039 
00040 #include <pcl/common/common.h>
00041 
00049 namespace pcl
00050 {
00058   PCL_EXPORTS void
00059   lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b, 
00060                      Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg);
00061 
00068   double inline
00069   sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
00070   {
00071     // Calculate the distance from the point to the line
00072     // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
00073     return (line_dir.cross3 (line_pt - pt)).squaredNorm () / line_dir.squaredNorm ();
00074   }
00075 
00084   double inline
00085   sqrPointToLineDistance (const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, const double sqr_length)
00086   {
00087     // Calculate the distance from the point to the line
00088     // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p1-p0)) / norm(p2-p1)
00089     return (line_dir.cross3 (line_pt - pt)).squaredNorm () / sqr_length;
00090   }
00091 
00099   template <typename PointT> double inline
00100   getMaxSegment (const pcl::PointCloud<PointT> &cloud, 
00101                  PointT &pmin, PointT &pmax)
00102   {
00103     double max_dist = std::numeric_limits<double>::min ();
00104     int i_min = -1, i_max = -1;
00105 
00106     for (size_t i = 0; i < cloud.points.size (); ++i)
00107     {
00108       for (size_t j = i; j < cloud.points.size (); ++j)
00109       {
00110         // Compute the distance 
00111         double dist = (cloud.points[i].getVector4fMap () - 
00112                        cloud.points[j].getVector4fMap ()).squaredNorm ();
00113         if (dist <= max_dist)
00114           continue;
00115 
00116         max_dist = dist;
00117         i_min = i;
00118         i_max = j;
00119       }
00120     }
00121 
00122     if (i_min == -1 || i_max == -1)
00123       return (max_dist = std::numeric_limits<double>::min ());
00124 
00125     pmin = cloud.points[i_min];
00126     pmax = cloud.points[i_max];
00127     return (std::sqrt (max_dist));
00128   }
00129  
00138   template <typename PointT> double inline
00139   getMaxSegment (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
00140                  PointT &pmin, PointT &pmax)
00141   {
00142     double max_dist = std::numeric_limits<double>::min ();
00143     int i_min = -1, i_max = -1;
00144 
00145     for (size_t i = 0; i < indices.size (); ++i)
00146     {
00147       for (size_t j = i; j < indices.size (); ++j)
00148       {
00149         // Compute the distance 
00150         double dist = (cloud.points[indices[i]].getVector4fMap () - 
00151                        cloud.points[indices[j]].getVector4fMap ()).squaredNorm ();
00152         if (dist <= max_dist)
00153           continue;
00154 
00155         max_dist = dist;
00156         i_min = i;
00157         i_max = j;
00158       }
00159     }
00160 
00161     if (i_min == -1 || i_max == -1)
00162       return (max_dist = std::numeric_limits<double>::min ());
00163 
00164     pmin = cloud.points[indices[i_min]];
00165     pmax = cloud.points[indices[i_max]];
00166     return (std::sqrt (max_dist));
00167   }
00168 
00173   template<typename PointType1, typename PointType2> inline float
00174   squaredEuclideanDistance (const PointType1& p1, const PointType2& p2)
00175   {
00176     float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y, diff_z = p2.z - p1.z;
00177     return (diff_x*diff_x + diff_y*diff_y + diff_z*diff_z);
00178   }
00179 
00184   template<> inline float
00185   squaredEuclideanDistance (const PointXY& p1, const PointXY& p2)
00186   {
00187     float diff_x = p2.x - p1.x, diff_y = p2.y - p1.y;
00188     return (diff_x*diff_x + diff_y*diff_y);
00189   }
00190 
00195   template<typename PointType1, typename PointType2> inline float
00196   euclideanDistance (const PointType1& p1, const PointType2& p2)
00197   {
00198     return (sqrtf (squaredEuclideanDistance (p1, p2)));
00199   }
00200 }
00201 /*@*/
00202 #endif  //#ifndef PCL_DISTANCES_H_
00203 


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