iSAM
Pose2d.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <cmath>
00031 #include <ostream>
00032 #include <Eigen/Dense>
00033 
00034 #include "util.h"
00035 #include "Point2d.h"
00036 
00037 namespace Eigen {
00038  typedef Matrix<bool, Dynamic, 1> VectorXb;
00039 }
00040 
00041 namespace isam {
00042 
00043 class Pose2d {
00044   friend std::ostream& operator<<(std::ostream& out, const Pose2d& p) {
00045     p.write(out);
00046     return out;
00047  }
00048 
00049   double _x;
00050   double _y;
00051   double _t;
00052 
00053 public:
00054   // assignment operator and copy constructor implicitly created, which is ok
00055   static const int dim = 3;
00056   static const int size = 3;
00057   static const char* name() {
00058     return "Pose2d";
00059   }
00060   Pose2d() : _x(0.), _y(0.), _t(0.) {}
00061   Pose2d(double x, double y, double t) : _x(x), _y(y), _t(t) {}
00062   Pose2d(const Eigen::Vector3d& vec) : _x(vec(0)), _y(vec(1)), _t(vec(2)) {}
00063 
00064   double x() const {return _x;}
00065   double y() const {return _y;}
00066   double t() const {return _t;}
00067 
00068   void set_x(double x) {_x = x;}
00069   void set_y(double y) {_y = y;}
00070   void set_t(double t) {_t = t;}
00071 
00072   Pose2d exmap(const Eigen::Vector3d& delta) const {
00073     Pose2d res = *this;
00074     res._x += delta(0);
00075     res._y += delta(1);
00076     res._t = standardRad(res._t + delta(2));
00077     return res;
00078   }
00079 
00080   Eigen::Vector3d vector() const {
00081     Eigen::Vector3d v(_x, _y, _t);
00082     return v;
00083   }
00084   void set(double x, double y, double t) {
00085     _x = x;
00086     _y = y;
00087     _t = t;
00088   }
00089   void set(const Eigen::Vector3d& v) {
00090     _x = v(0);
00091     _y = v(1);
00092     _t = standardRad(v(2));
00093   }
00094   void write(std::ostream &out) const {
00095     out << "(" << _x << ", " << _y << ", " << _t << ")";
00096   }
00097   
00098   Eigen::VectorXb is_angle() const {
00099     Eigen::VectorXb isang (dim);
00100     isang << false, false, true;
00101     return isang;
00102   }
00103 
00111   Pose2d oplus(const Pose2d& d) const {
00112     double c = cos(t());
00113     double s = sin(t());
00114     double px = x() + c*d.x() - s*d.y();
00115     double py = y() + s*d.x() + c*d.y();
00116     double pt = t() + d.t();
00117     return Pose2d(px,py,pt);
00118   }
00119 
00127   Pose2d ominus(const Pose2d& b) const {
00128     double c = cos(b.t());
00129     double s = sin(b.t());
00130     double dx = x() - b.x();
00131     double dy = y() - b.y();
00132     double ox =  c*dx + s*dy;
00133     double oy = -s*dx + c*dy;
00134     double ot = t() - b.t();
00135     return Pose2d(ox,oy,ot);
00136   }
00137 
00143   Point2d transform_to(const Point2d& p) const {
00144     double c = cos(t());
00145     double s = sin(t());
00146     double dx = p.x() - x();
00147     double dy = p.y() - y();
00148     double x =  c*dx + s*dy;
00149     double y = -s*dx + c*dy;
00150     return Point2d(x,y);
00151   }
00152 
00153   Point2d transform_from(const Point2d& p) const {
00154     double c = cos(t());
00155     double s = sin(t());
00156     double px = x() + c*p.x() - s*p.y();
00157     double py = y() + s*p.x() + c*p.y();
00158     return Point2d(px,py);
00159   }
00160 
00161 };
00162 
00163 }
 All Classes Files Functions Variables