iSAM
Pose3d.h
Go to the documentation of this file.
00001 
00052 #pragma once
00053 
00054 #include <cmath>
00055 #include <Eigen/Dense>
00056 #include <Eigen/Geometry>
00057 
00058 #include "util.h"
00059 #include "Rot3d.h"
00060 #include "Pose2d.h"
00061 #include "Point3d.h"
00062 #include "Point3dh.h"
00063 #include "Point2d.h"
00064 
00065 namespace isam {
00066 
00067 typedef Eigen::Matrix< double, 6, 1> Vector6d;
00068 
00069 class Pose3d {
00070   friend std::ostream& operator<<(std::ostream& out, const Pose3d& p) {
00071     p.write(out);
00072     return out;
00073   }
00074 
00075   Point3d _t;
00076   Rot3d _rot;
00077 public:
00078   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00079 
00080   static const int dim = 6;
00081   static const char* name() {
00082     return "Pose3d";
00083   }
00084 
00085   Pose3d() : _t(0,0,0), _rot(0,0,0) {}
00086 
00087   Pose3d(double x, double y, double z, double yaw, double pitch, double roll) : _t(x, y, z), _rot(yaw, pitch, roll) {}
00088 
00089   Pose3d(const Eigen::MatrixXd& m) {
00090     if (m.rows()==6 && m.cols()==1) {
00091       _t = Point3d(m(0), m(1), m(2));
00092       _rot = Rot3d(m(3), m(4), m(5));
00093     } else if (m.rows()==4 && m.cols()==4) {
00094       // Convert a homogeneous 4x4 transformation matrix to a Pose3.
00095       Eigen::Matrix4d wTo = m / m(3,3); // enforce T(3,3)=1
00096       Eigen::Vector3d t = wTo.col(3).head(3);
00097       Eigen::Matrix3d wRo = wTo.topLeftCorner(3,3);
00098       _t = Point3d(t(0), t(1), t(2));
00099       _rot = Rot3d(wRo);
00100     } else {
00101       require(false, "Pose3d constructor called with matrix of wrong dimension");
00102     }
00103   }
00104 
00105   explicit Pose3d(const Eigen::Isometry3d & T) {
00106     Eigen::Vector3d t(T.translation());
00107     _t = Point3d(t(0), t(1), t(2));
00108     _rot = Rot3d(T.rotation());
00109   }
00110 
00111   Pose3d(const Point3d& t, const Rot3d& rot) : _t(t), _rot(rot) {}
00112 
00113   double x() const {return _t.x();}
00114   double y() const {return _t.y();}
00115   double z() const {return _t.z();}
00116   double yaw()   const {return _rot.yaw();}
00117   double pitch() const {return _rot.pitch();}
00118   double roll()  const {return _rot.roll();}
00119 
00120   Point3d trans() const {return _t;}
00121   Rot3d rot() const {return _rot;}
00122 
00123   void set_x(double x) {_t.set_x(x);}
00124   void set_y(double y) {_t.set_y(y);}
00125   void set_z(double z) {_t.set_z(z);}
00126   void set_yaw  (double yaw)   {_rot.set_yaw(yaw);}
00127   void set_pitch(double pitch) {_rot.set_pitch(pitch);}
00128   void set_roll (double roll)  {_rot.set_roll(roll);}
00129 
00130   Pose3d exmap(const Vector6d& delta) const {
00131     Pose3d res = *this;
00132     res._t   = res._t.exmap(delta.head(3));
00133     res._rot = res._rot.exmap(delta.tail(3));
00134     return res;
00135   }
00136 
00137   Vector6d vector() const {
00138     double Y, P, R;
00139     // cheaper to recover ypr at once
00140     _rot.ypr(Y, P, R);
00141     Vector6d tmp;
00142     tmp << x(), y(), z(), Y, P, R;
00143     return tmp;
00144   }
00145 
00146   void set(double x, double y, double z, double yaw, double pitch, double roll) {
00147     _t = Point3d(x, y, z);
00148     _rot = Rot3d(yaw, pitch, roll);
00149   }
00150 
00151   void set(const Vector6d& v) {
00152     _t = Point3d(v(0), v(1), v(2));
00153     _rot = Rot3d(standardRad(v(3)), standardRad(v(4)), standardRad(v(5)));
00154   }
00155 
00156   void of_pose2d(const Pose2d& p) {
00157     set(p.x(), p.y(), 0., p.t(), 0., 0.);
00158   }
00159 
00160   void of_point2d(const Point2d& p) {
00161     set(p.x(), p.y(), 0., 0., 0., 0.);
00162   }
00163 
00164   void of_point3d(const Point3d& p) {
00165     set(p.x(), p.y(), p.z(), 0., 0., 0.);
00166   }
00167 
00168   void write(std::ostream &out) const {
00169     out << "(" << x() << ", " << y() << ", " << z() << "; "
00170         << yaw() << ", " << pitch() << ", " << roll() << ")";
00171   }
00172 
00173   Eigen::VectorXb is_angle() const {
00174     Eigen::VectorXb isang (dim);
00175     isang << false, false, false, true, true, true;
00176     return isang;
00177   }
00178   
00187   Eigen::Matrix4d wTo() const {
00188     Eigen::Matrix4d T;
00189     T.topLeftCorner(3,3) = _rot.wRo();
00190     T.col(3).head(3) << x(), y(), z();
00191     T.row(3) << 0., 0., 0., 1.;
00192     return T;
00193   }
00194 
00203   Eigen::Matrix4d oTw() const {
00204     Eigen::Matrix3d oRw = _rot.wRo().transpose();
00205     Eigen::Vector3d t(x(), y(), z());
00206     Eigen::Vector3d C = - oRw * t;
00207     Eigen::Matrix4d T;
00208     T.topLeftCorner(3,3) = oRw;
00209     T.col(3).head(3) = C;
00210     T.row(3) << 0., 0., 0., 1.;
00211     return T;
00212   }
00213 
00221   Pose3d oplus(const Pose3d& d) const {
00222     return Pose3d(wTo() * d.wTo());
00223   }
00224 
00232   Pose3d ominus(const Pose3d& b) const {
00233     return Pose3d(b.oTw() * wTo());
00234   }
00235 
00241   Point3dh transform_to(const Point3dh& p) const {
00242     return Point3dh(oTw() * p.vector());
00243   }
00244 
00245 
00251   Point3d transform_to(const Point3d& p) const {
00252     return transform_to(Point3dh(p)).to_point3d();
00253   }
00254 
00260   Point3dh transform_from(const Point3dh& p) const {
00261     return Point3dh(wTo() * p.vector());
00262   }
00263 
00269   Point3d transform_from(const Point3d& p) const {
00270     return transform_from(Point3dh(p)).to_point3d();
00271   }
00272 
00273 };
00274 
00275 }
 All Classes Files Functions Variables