door_handle_detector: distances.h Source File

00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id$
00028  *
00029  */
00030 
00033 #ifndef _CLOUD_GEOMETRY_DISTANCES_H_
00034 #define _CLOUD_GEOMETRY_DISTANCES_H_
00035 
00036 // ROS includes
00037 #include <geometry_msgs/Point32.h>
00038 #include <geometry_msgs/Point.h>
00039 #include <Eigen/Core>
00040 
00041 namespace cloud_geometry
00042 {
00043 
00044   namespace distances
00045   {
00047 
00051     inline double
00052       pointToPointXYDistanceSqr (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00053     {
00054       return ( (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) );
00055     }
00056 
00058 
00062     inline double
00063       pointToPointXYDistance (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00064     {
00065       return ( sqrt (pointToPointXYDistanceSqr (p1, p2) ));
00066     }
00067 
00069 
00073     inline double
00074       pointToPointXZDistanceSqr (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00075     {
00076       return ( (p1.x - p2.x) * (p1.x - p2.x) + (p1.z - p2.z) * (p1.z - p2.z) );
00077     }
00078 
00080 
00084     inline double
00085       pointToPointXZDistance (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00086     {
00087       return ( sqrt (pointToPointXZDistanceSqr (p1, p2) ));
00088     }
00089 
00091 
00095     inline double
00096       pointToPointYZDistanceSqr (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00097     {
00098       return ( (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z) );
00099     }
00100 
00102 
00106     inline double
00107       pointToPointYZDistance (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00108     {
00109       return ( sqrt (pointToPointYZDistanceSqr (p1, p2) ));
00110     }
00111 
00113 
00117     inline double
00118       pointToPointDistanceSqr (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00119     {
00120       return ( (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z) );
00121     }
00122 
00124 
00128     inline double
00129       pointToPointDistance (const geometry_msgs::Point32 &p1, const geometry_msgs::Point32 &p2)
00130     {
00131       return (sqrt (pointToPointDistanceSqr (p1, p2) ));
00132     }
00133 
00135 
00139     inline double
00140       pointToPointDistanceSqr (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
00141     {
00142       return ( (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z) );
00143     }
00144 
00146 
00150     inline double
00151       pointToPointDistance (const geometry_msgs::Point &p1, const geometry_msgs::Point &p2)
00152     {
00153       return (sqrt (pointToPointDistanceSqr (p1, p2) ));
00154     }
00155 
00157 
00161     inline double
00162       pointToPointDistanceSqr (const geometry_msgs::Point32 &p1, const geometry_msgs::Point &p2)
00163     {
00164       return ( (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z) );
00165     }
00166 
00168 
00172     inline double
00173       pointToPointDistance (const geometry_msgs::Point32 &p1, const geometry_msgs::Point &p2)
00174     {
00175       return (sqrt (pointToPointDistanceSqr (p1, p2) ));
00176     }
00177 
00179 
00183     inline double
00184       pointToPointDistanceSqr (const geometry_msgs::Point &p1, const geometry_msgs::Point32 &p2)
00185     {
00186       return ( (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z) );
00187     }
00188 
00190 
00194     inline double
00195       pointToPointDistance (const geometry_msgs::Point &p1, const geometry_msgs::Point32 &p2)
00196     {
00197       return (sqrt (pointToPointDistanceSqr (p1, p2) ));
00198     }
00199 
00200     double pointToLineDistance (const geometry_msgs::Point32 &p, const geometry_msgs::Point32 &q, const geometry_msgs::Point32 &dir);
00201     double pointToLineDistance (const geometry_msgs::Point32 &p, const std::vector<double> &line_coefficients);
00202 
00204 
00208     inline double
00209       pointToPlaneDistanceSigned (const geometry_msgs::Point32 &p, const std::vector<double> &plane_coefficients)
00210     {
00211       return (plane_coefficients[0]*p.x + plane_coefficients[1]*p.y + plane_coefficients[2]*p.z + plane_coefficients[3]);
00212     }
00214 
00221     inline double
00222       pointToPlaneDistanceSigned (const geometry_msgs::Point32 &p, double a, double b, double c, double d)
00223     {
00224       return (a * p.x + b * p.y + c * p.z + d);
00225     }
00227 
00231     inline double
00232       pointToPlaneDistanceSigned (const geometry_msgs::Point32 &p, const Eigen::Vector4d &plane_coefficients)
00233     {
00234       return ( plane_coefficients (0) * p.x + plane_coefficients (1) * p.y + plane_coefficients (2) * p.z + plane_coefficients (3) );
00235     }
00236 
00238 
00242     inline double
00243       pointToPlaneDistance (const geometry_msgs::Point32 &p, const std::vector<double> &plane_coefficients)
00244     {
00245       return (fabs (pointToPlaneDistanceSigned (p, plane_coefficients)) );
00246     }
00248 
00255     inline double
00256       pointToPlaneDistance (const geometry_msgs::Point32 &p, double a, double b, double c, double d)
00257     {
00258       return (fabs (pointToPlaneDistance (p, a, b, c, d)) );
00259     }
00261 
00265     inline double
00266       pointToPlaneDistance (const geometry_msgs::Point32 &p, const Eigen::Vector4d &plane_coefficients)
00267     {
00268       return ( fabs (pointToPlaneDistance (p, plane_coefficients)) );
00269     }
00270 
00272 
00276     inline double
00277       pointToSphereDistance (const geometry_msgs::Point32 &p, const std::vector<double> &sphere_coefficients)
00278     {
00279       return (sqrt (
00280                     (p.x - sphere_coefficients[0]) * (p.x - sphere_coefficients[0]) +
00281                     (p.y - sphere_coefficients[1]) * (p.y - sphere_coefficients[1]) +
00282                     (p.z - sphere_coefficients[2]) * (p.z - sphere_coefficients[2])
00283                    ) - sphere_coefficients[3]);
00284     }
00286 
00293     inline double
00294       pointToSphereDistance (const geometry_msgs::Point32 &p, double x, double y, double z, double r)
00295     {
00296       return (sqrt ( (p.x - x) * (p.x - x) + (p.y - y) * (p.y - y) + (p.z - z) * (p.z - z) ) - r);
00297     }
00298 
00299 
00300     void lineToLineSegment (const std::vector<double> &line_a, const std::vector<double> &line_b, std::vector<double> &segment);
00301 
00303 
00309     inline void
00310       createParallel2DLine (const std::vector<double> &line_a, std::vector<double> &line_b, double distance)
00311     {
00312       line_b.resize (6);
00313       // The direction of the resultant line is equal to the first
00314       line_b[3] = line_a[3];
00315       line_b[4] = line_a[4];
00316       line_b[5] = line_a[5];
00317 
00318       // Create a point on an orthogonal line, at the given distance
00319       double angle = atan2 (line_a[3], -line_a[4]);
00320       if (angle < 0)
00321         angle += 2 * M_PI;
00322 
00323       line_b[0] = line_a[0] + distance * cos (angle);
00324       line_b[1] = line_a[1] + distance * sin (angle);
00325       line_b[2] = line_a[2];
00326     }
00327 
00328 
00330 
00334     inline double
00335       pointToPolygonDistanceSqr (const geometry_msgs::Point32 &p, const geometry_msgs::Polygon &poly)
00336     {
00337       double min_distance = FLT_MAX;
00338       geometry_msgs::Point32 dir, p_t;
00339       // Treat the polygon as a set of 3D lines, and compute the distance from the point to each line
00340       unsigned int i = 0;
00341       for (i = 0; i < poly.points.size () - 1; i++)
00342       {
00343         // Compute the direction
00344         dir.x = poly.points[i + 1].x - poly.points[i].x;
00345         dir.y = poly.points[i + 1].y - poly.points[i].y;
00346         dir.z = poly.points[i + 1].z - poly.points[i].z;
00347 
00348         // Calculate the distance from the point to the line
00349         // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
00350         p_t.x = poly.points[i + 1].x - p.x;
00351         p_t.y = poly.points[i + 1].y - p.y;
00352         p_t.z = poly.points[i + 1].z - p.z;
00353 
00354         geometry_msgs::Point32 c = cross (p_t, dir);
00355         double sqr_distance = (c.x * c.x + c.y * c.y + c.z * c.z) / (dir.x * dir.x + dir.y * dir.y + dir.z * dir.z);
00356         if (sqr_distance < min_distance)
00357           min_distance = sqr_distance;
00358       }
00359 
00360       // ---[ Check the [i-1 . 0] line too
00361       // Compute the direction
00362       dir.x = poly.points[i].x - poly.points[0].x;
00363       dir.y = poly.points[i].y - poly.points[0].y;
00364       dir.z = poly.points[i].z - poly.points[0].z;
00365 
00366       // Calculate the distance from the point to the line
00367       // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
00368       p_t.x = poly.points[i].x - p.x;
00369       p_t.y = poly.points[i].y - p.y;
00370       p_t.z = poly.points[i].z - p.z;
00371 
00372       geometry_msgs::Point32 c = cross (p_t, dir);
00373       double sqr_distance = (c.x * c.x + c.y * c.y + c.z * c.z) / (dir.x * dir.x + dir.y * dir.y + dir.z * dir.z);
00374       if (sqr_distance < min_distance)
00375         min_distance = sqr_distance;
00376 
00377       return (min_distance);
00378     }
00379 
00381 
00385     inline double
00386       pointToPolygonDistance (const geometry_msgs::Point32 &p, const geometry_msgs::Polygon &poly)
00387     {
00388       return (sqrt (pointToPolygonDistanceSqr (p, poly)));
00389     }
00390   }
00391 }
00392 
00393 #endif