MapReferenceICP: include/geometry2D.h Source File

- Main Page
- Modules
- Classes
- Files

# include/geometry2D.h

    00001 /* Simple 2D geometric operations with points, poses, and lines.
    00002  *
    00003  * These algorithms were worked out by me from first principles several years
    00004  * ago. They work ok, but are not particularly good implementations. I recommend
    00005  * the geometric source-code and resources by David Eberly on the "Magic Software"
    00006  * website: www.magic-software.com.
    00007  *
    00008  * Tim Bailey 2004.
    00009  */
    00010 #ifndef GEOMETRY_2D_H_
    00011 #define GEOMETRY_2D_H_
    00012
    00013 #include <vector>
    00014
    00015 namespace Geom2D {
    00016
    00017 //
    00018 // Basic Structures
    00019 //
    00020
    00021 struct Point
    00022 {
    00023         double x;
    00024         double y;
    00025         short int laser_index;
    00026 };
    00027
    00028 struct Pose
    00029 {
    00030         Point p;
    00031         double phi;
    00032 };
    00033
    00034 struct Line {
    00035         Point first;
    00036         Point second;
    00037 };
    00038
    00039 //
    00040 // Utility functions
    00041 //
    00042
    00043 const double PI = 3.14159265358979;
    00044
    00045 inline
    00046 double sqr(double x) { return x*x; }
    00047
    00048 inline
    00049 double abs(double x) { return (x<0.) ? -x : x; }
    00050
    00051 inline
    00052 double round(double x) {
    00053         return (x<0.) ? -static_cast<int>(0.5-x) : static_cast<int>(0.5+x);
    00054 }
    00055 /*
    00056 template<class T>
    00057 inline
    00058 void swap(T& a, T& b)
    00059 {
    00060         T tmp(a);
    00061         a = b;
    00062         b = tmp;
    00063 }
    00064 */
    00065 inline
    00066 double pi_to_pi(double angle) { // normalise an angle to within +/- PI
    00067         while (angle < -PI)
    00068                 angle += 2.*PI;
    00069         while (angle > PI)
    00070                 angle -= 2.*PI;
    00071         return angle;
    00072 }
    00073
    00074 //
    00075 // Point and Pose algorithms
    00076 //
    00077
    00078 inline
    00079 double dist_sqr(const Point& p, const Point& q) { // squared distance between two Points
    00080         return (sqr(p.x-q.x) + sqr(p.y-q.y));
    00081 }
    00082
    00083 double dist(const Point& p, const Point& q);
    00084 Pose compute_relative_pose(const std::vector<Point>& a, const std::vector<Point>& b);
    00085
    00086 //
    00087 // Line algorithms
    00088 //
    00089
    00090 bool intersection_line_line (Point& p, const Line& l, const Line& m);
    00091 double distance_line_point (const Line& lne, const Point& p);
    00092 void intersection_line_point(Point& p, const Line& l, const Point& q);
    00093
    00094 //
    00095 // Basic transformations on 2-D Points (x,y) and Poses (x,y,phi).
    00096 //
    00097
    00098 class Transform2D {
    00099 public:
    00100         Transform2D(const Pose& ref);
    00101
    00102         void transform_to_relative(Point &p);
    00103         void transform_to_relative(Pose &p);
    00104         void transform_to_global(Point &p);
    00105         void transform_to_global(Pose &p);
    00106
    00107 private:
    00108         const Pose base;
    00109         double c;
    00110         double s;
    00111 };
    00112
    00113 inline
    00114 void Transform2D::transform_to_relative(Point &p) {
    00115         p.x -= base.p.x;
    00116         p.y -= base.p.y;
    00117         double t(p.x);
    00118         p.x = p.x*c + p.y*s;
    00119         p.y = p.y*c -   t*s;
    00120 }
    00121
    00122 inline
    00123 void Transform2D::transform_to_global(Point &p) {
    00124         double t(p.x);
    00125         p.x = base.p.x + c*p.x - s*p.y;
    00126         p.y = base.p.y + s*t   + c*p.y;
    00127 }
    00128
    00129 inline
    00130 void Transform2D::transform_to_relative(Pose &p) {
    00131         transform_to_relative(p.p);
    00132         p.phi= pi_to_pi(p.phi-base.phi);
    00133 }
    00134
    00135 inline
    00136 void Transform2D::transform_to_global(Pose &p) {
    00137         transform_to_global(p.p);
    00138         p.phi= pi_to_pi(p.phi+base.phi);
    00139 }
    00140
    00141 } // namespace Geom2D
    00142
    00143 #endif