door_handle_detector: angles.h Source File

00001 /*
00002  * Copyright (c) 2009 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_ANGLES_H_
00034 #define _CLOUD_GEOMETRY_ANGLES_H_
00035 
00036 // ROS includes
00037 #include <geometry_msgs/Point.h>
00038 #include <geometry_msgs/Point32.h>
00039 #include <geometry_msgs/PointStamped.h>
00040 #include <sensor_msgs/PointCloud.h>
00041 #include <geometry_msgs/Polygon.h>
00042 
00043 #include <door_handle_detector/geometry/point.h>
00044 #include <door_handle_detector/geometry/nearest.h>
00045 
00046 #include <Eigen/Core>
00047 
00048 namespace cloud_geometry
00049 {
00050 
00051   namespace angles
00052   {
00054 
00058     inline double
00059       getAngleBetweenPlanes (const std::vector<double> &plane_a, const std::vector<double> &plane_b)
00060     {
00061       return (acos (plane_a[0] * plane_b[0] + plane_a[1] * plane_b[1] + plane_a[2] * plane_b[2]));
00062     }
00063 
00065 
00069     inline double
00070       getAngleBetweenPlanes (const geometry_msgs::Point32 &plane_a, const geometry_msgs::Point32 &plane_b)
00071     {
00072       return (acos (plane_a.x * plane_b.x + plane_a.y * plane_b.y + plane_a.z * plane_b.z));
00073     }
00074 
00076 
00080     inline double
00081       getAngleBetweenPlanes (const geometry_msgs::Point32 &plane_a, const std::vector<double> &plane_b)
00082     {
00083       return (acos (plane_a.x * plane_b[0] + plane_a.y * plane_b[1] + plane_a.z * plane_b[2]));
00084     }
00085 
00087 
00091     inline double
00092       getAngleBetweenPlanes (const std::vector<double> &plane_a, const geometry_msgs::Point32 &plane_b)
00093     {
00094       return (acos (plane_a[0] * plane_b.x + plane_a[1] * plane_b.y + plane_a[2] * plane_b.z));
00095     }
00096 
00098 
00101     inline double
00102       getAngle2D (const double point[2])
00103     {
00104       double rad;
00105       if (point[0] == 0)
00106         rad = (point[1] < 0) ? -M_PI / 2.0 : M_PI / 2.0;
00107       else
00108       {
00109         rad = atan (point[1] / point[0]);
00110         if (point[0] < 0)
00111           rad += M_PI;
00112       }
00113       if (rad < 0)
00114         rad += 2 * M_PI;
00115 
00116       return (rad);
00117     }
00118 
00120 
00124     inline double
00125       getAngle2D (double x, double y)
00126     {
00127       double rad;
00128       if (x == 0)
00129         rad = (y < 0) ? -M_PI / 2.0 : M_PI / 2.0;
00130       else
00131       {
00132         rad = atan (y / x);
00133         if (x < 0)
00134           rad += M_PI;
00135       }
00136       if (rad < 0)
00137         rad += 2 * M_PI;
00138 
00139       return (rad);
00140     }
00141 
00143 
00147     inline double
00148       getAngle3D (const geometry_msgs::Point32 &v1, const geometry_msgs::Point32 &v2)
00149     {
00150       // Compute the vectors norms
00151       double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
00152       double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);
00153 
00154       // Compute the actual angle
00155       double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );
00156 
00157       // Check against NaN
00158       if (std::isnan (rad))
00159         ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
00160       return (rad);
00161     }
00162 
00164 
00168     inline double
00169       getAngle3D (const geometry_msgs::Point &v1, const geometry_msgs::Point &v2)
00170     {
00171       // Compute the vectors norms
00172       double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
00173       double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);
00174 
00175       // Compute the actual angle
00176       double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );
00177 
00178       // Check against NaN
00179       if (std::isnan (rad))
00180         ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
00181       return (rad);
00182     }
00183 
00185 
00189     inline double
00190       getAngle3D (const geometry_msgs::Point32 &v1, const geometry_msgs::Point &v2)
00191     {
00192       // Compute the vectors norms
00193       double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
00194       double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);
00195 
00196       // Compute the actual angle
00197       double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );
00198 
00199       // Check against NaN
00200       if (std::isnan (rad))
00201         ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
00202       return (rad);
00203     }
00204 
00206 
00210     inline double
00211       getAngle3D (const geometry_msgs::Point &v1, const geometry_msgs::Point32 &v2)
00212     {
00213       // Compute the vectors norms
00214       double norm_v1 = (v1.x * v1.x) + (v1.y * v1.y) + (v1.z * v1.z);
00215       double norm_v2 = (v2.x * v2.x) + (v2.y * v2.y) + (v2.z * v2.z);
00216 
00217       // Compute the actual angle
00218       double rad = acos ( cloud_geometry::dot (v1, v2) / sqrt (norm_v1 * norm_v2) );
00219 
00220       // Check against NaN
00221       if (std::isnan (rad))
00222         ROS_ERROR ("[cloud_geometry::angles::getAngle3D] got a NaN angle!");
00223       return (rad);
00224     }
00225 
00226 
00228 
00233     inline void
00234       flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::PointStamped &viewpoint)
00235     {
00236       // See if we need to flip any plane normals
00237       float vp_m[3];
00238       vp_m[0] = viewpoint.point.x - point.x;
00239       vp_m[1] = viewpoint.point.y - point.y;
00240       vp_m[2] = viewpoint.point.z - point.z;
00241 
00242       // Dot product between the (viewpoint - point) and the plane normal
00243       double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));
00244 
00245       // Flip the plane normal
00246       if (cos_theta < 0)
00247       {
00248         for (int d = 0; d < 3; d++)
00249           normal (d) *= -1;
00250         // Hessian form (D = nc . p_plane (centroid here) + p)
00251         normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
00252       }
00253     }
00254 
00256 
00261     inline void
00262       flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::Point &viewpoint)
00263     {
00264       // See if we need to flip any plane normals
00265       float vp_m[3];
00266       vp_m[0] = viewpoint.x - point.x;
00267       vp_m[1] = viewpoint.y - point.y;
00268       vp_m[2] = viewpoint.z - point.z;
00269 
00270       // Dot product between the (viewpoint - point) and the plane normal
00271       double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));
00272 
00273       // Flip the plane normal
00274       if (cos_theta < 0)
00275       {
00276         for (int d = 0; d < 3; d++)
00277           normal (d) *= -1;
00278         // Hessian form (D = nc . p_plane (centroid here) + p)
00279         normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
00280       }
00281     }
00282 
00284 
00289     inline void
00290       flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::Point32 &viewpoint)
00291     {
00292       // See if we need to flip any plane normals
00293       float vp_m[3];
00294       vp_m[0] = viewpoint.x - point.x;
00295       vp_m[1] = viewpoint.y - point.y;
00296       vp_m[2] = viewpoint.z - point.z;
00297 
00298       // Dot product between the (viewpoint - point) and the plane normal
00299       double cos_theta = (vp_m[0] * normal (0) + vp_m[1] * normal (1) + vp_m[2] * normal (2));
00300 
00301       // Flip the plane normal
00302       if (cos_theta < 0)
00303       {
00304         for (int d = 0; d < 3; d++)
00305           normal (d) *= -1;
00306         // Hessian form (D = nc . p_plane (centroid here) + p)
00307         normal (3) = -1 * (normal (0) * point.x + normal (1) * point.y + normal (2) * point.z);
00308       }
00309     }
00310 
00312 
00317     inline void
00318       flipNormalTowardsViewpoint (std::vector<double> &normal, const geometry_msgs::Point32 &point, const geometry_msgs::PointStamped &viewpoint)
00319     {
00320       // See if we need to flip any plane normals
00321       float vp_m[3];
00322       vp_m[0] = viewpoint.point.x - point.x;
00323       vp_m[1] = viewpoint.point.y - point.y;
00324       vp_m[2] = viewpoint.point.z - point.z;
00325 
00326       // Dot product between the (viewpoint - point) and the plane normal
00327       double cos_theta = (vp_m[0] * normal[0] + vp_m[1] * normal[1] + vp_m[2] * normal[2]);
00328 
00329       // Flip the plane normal
00330       if (cos_theta < 0)
00331       {
00332         for (int d = 0; d < 3; d++)
00333           normal[d] *= -1;
00334         // Hessian form (D = nc . p_plane (centroid here) + p)
00335         normal[3] = -1 * (normal[0] * point.x + normal[1] * point.y + normal[2] * point.z);
00336       }
00337     }
00338 
00340 
00345     inline void
00346       flipNormalTowardsViewpoint (std::vector<double> &normal, const geometry_msgs::Point32 &point, const geometry_msgs::Point32 &viewpoint)
00347     {
00348       // See if we need to flip any plane normals
00349       float vp_m[3];
00350       vp_m[0] = viewpoint.x - point.x;
00351       vp_m[1] = viewpoint.y - point.y;
00352       vp_m[2] = viewpoint.z - point.z;
00353 
00354       // Dot product between the (viewpoint - point) and the plane normal
00355       double cos_theta = (vp_m[0] * normal[0] + vp_m[1] * normal[1] + vp_m[2] * normal[2]);
00356 
00357       // Flip the plane normal
00358       if (cos_theta < 0)
00359       {
00360         for (int d = 0; d < 3; d++)
00361           normal[d] *= -1;
00362         // Hessian form (D = nc . p_plane (centroid here) + p)
00363         normal[3] = -1 * (normal[0] * point.x + normal[1] * point.y + normal[2] * point.z);
00364       }
00365     }
00366 
00367   }
00368 }
00369 
00370 #endif