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
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 }