Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/common/include/pcl/common/distances.h
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 
00042 /**
00043   * \file pcl/common/distances.h
00044   * Define standard C methods to do distance calculations
00045   * \ingroup common
00046   */
00047 
00048 /*@{*/
00049 namespace pcl
00050 {
00051   /** \brief Get the shortest 3D segment between two 3D lines
00052     * \param line_a the coefficients of the first line (point, direction)
00053     * \param line_b the coefficients of the second line (point, direction)
00054     * \param pt1_seg the first point on the line segment
00055     * \param pt2_seg the second point on the line segment
00056     * \ingroup common
00057     */
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 
00062   /** \brief Get the square distance from a point to a line (represented by a point and a direction)
00063     * \param pt a point
00064     * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
00065     * \param line_dir the line direction
00066     * \ingroup common
00067     */
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 
00076   /** \brief Get the square distance from a point to a line (represented by a point and a direction)
00077     * \note This one is useful if one has to compute many distances to a fixed line, so the vector length can be pre-computed
00078     * \param pt a point
00079     * \param line_pt a point on the line (make sure that line_pt[3] = 0 as there are no internal checks!)
00080     * \param line_dir the line direction
00081     * \param sqr_length the squared norm of the line direction
00082     * \ingroup common
00083     */
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 
00092   /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
00093     * \param[in] cloud the point cloud dataset
00094     * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment)
00095     * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment)
00096     * \return the length of segment length
00097     * \ingroup common
00098     */
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  
00130   /** \brief Obtain the maximum segment in a given set of points, and return the minimum and maximum points.
00131     * \param[in] cloud the point cloud dataset
00132     * \param[in] indices a set of point indices to use from \a cloud
00133     * \param[out] pmin the coordinates of the "minimum" point in \a cloud (one end of the segment)
00134     * \param[out] pmax the coordinates of the "maximum" point in \a cloud (the other end of the segment)
00135     * \return the length of segment length
00136     * \ingroup common
00137     */
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 
00169   /** \brief Calculate the squared euclidean distance between the two given points.
00170     * \param[in] p1 the first point
00171     * \param[in] p2 the second point
00172     */
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 
00180   /** \brief Calculate the squared euclidean distance between the two given points.
00181     * \param[in] p1 the first point
00182     * \param[in] p2 the second point
00183     */
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 
00191    /** \brief Calculate the euclidean distance between the two given points.
00192     * \param[in] p1 the first point
00193     * \param[in] p2 the second point
00194     */
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