iSAM
Rot3d.h
Go to the documentation of this file.
00001 
00033 #define USE_QUATERNIONS
00034 
00035 #pragma once
00036 
00037 #include <cmath>
00038 #include <Eigen/Dense>
00039 #include <Eigen/Geometry>
00040 #include <Eigen/LU>
00041 
00042 #include <isam/util.h>
00043 
00044 namespace isam {
00045 
00046 class Rot3d {
00047   friend std::ostream& operator<<(std::ostream& out, const Rot3d& p) {
00048     p.write(out);
00049     return out;
00050   }
00051 
00052 public:
00053   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00054 
00055   static Eigen::Matrix3d euler_to_wRo(double yaw, double pitch, double roll) {
00056     double c__ = cos(yaw);
00057     double _c_ = cos(pitch);
00058     double __c = cos(roll);
00059     double s__ = sin(yaw);
00060     double _s_ = sin(pitch);
00061     double __s = sin(roll);
00062     double cc_ = c__ * _c_;
00063     double cs_ = c__ * _s_;
00064     double sc_ = s__ * _c_;
00065     double ss_ = s__ * _s_;
00066     double c_c = c__ * __c;
00067     double c_s = c__ * __s;
00068     double s_c = s__ * __c;
00069     double s_s = s__ * __s;
00070     double _cc = _c_ * __c;
00071     double _cs = _c_ * __s;
00072     double csc = cs_ * __c;
00073     double css = cs_ * __s;
00074     double ssc = ss_ * __c;
00075     double sss = ss_ * __s;
00076     Eigen::Matrix3d wRo;
00077     wRo <<
00078       cc_  , css-s_c,  csc+s_s,
00079       sc_  , sss+c_c,  ssc-c_s,
00080      -_s_  ,     _cs,      _cc;
00081     return wRo;
00082   }
00083 
00084   static void wRo_to_euler(const Eigen::Matrix3d& wRo, double& yaw, double& pitch, double& roll) {
00085     yaw = standardRad(atan2(wRo(1,0), wRo(0,0)));
00086     double c = cos(yaw);
00087     double s = sin(yaw);
00088     pitch = standardRad(atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s));
00089     roll  = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c));
00090   }
00091 
00092   static Eigen::Quaterniond wRo_to_quat(const Eigen::Matrix3d& wRo) {
00093     return Eigen::Quaterniond(wRo);
00094   }
00095 
00096   static Eigen::Matrix3d quat_to_wRo(const Eigen::Quaterniond& quat) {
00097     return Eigen::Matrix3d(quat);
00098   }
00099 
00100   static Eigen::Quaterniond euler_to_quat(double yaw, double pitch, double roll) {
00101     double sy = sin(yaw*0.5);
00102     double cy = cos(yaw*0.5);
00103     double sp = sin(pitch*0.5);
00104     double cp = cos(pitch*0.5);
00105     double sr = sin(roll*0.5);
00106     double cr = cos(roll*0.5);
00107     double w = cr*cp*cy + sr*sp*sy;
00108     double x = sr*cp*cy - cr*sp*sy;
00109     double y = cr*sp*cy + sr*cp*sy;
00110     double z = cr*cp*sy - sr*sp*cy;
00111     return Eigen::Quaterniond(w,x,y,z);
00112   }
00113 
00114   static void quat_to_euler(Eigen::Quaterniond q, double& yaw, double& pitch, double& roll) {
00115     const double q0 = q.w();
00116     const double q1 = q.x();
00117     const double q2 = q.y();
00118     const double q3 = q.z();
00119 //    roll = atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
00120     roll = atan2(2.0*(q0*q1+q2*q3), q0*q0-q1*q1-q2*q2+q3*q3); // numerically more stable (thanks to Dehann for pointing this out)
00121     pitch = asin(2.0*(q0*q2-q3*q1));
00122 //    yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
00123     yaw = atan2(2.0*(q0*q3+q1*q2), q0*q0+q1*q1-q2*q2-q3*q3);
00124   }
00125 
00126   static Eigen::Quaterniond delta3_to_quat(const Eigen::Vector3d& delta) {
00127     double theta = delta.norm();
00128     double S;
00129     if (theta < 0.0001) { // sqrt4(machine precession)
00130       S = 0.5 + theta*theta/48.;
00131     } else {
00132       S = sin(0.5*theta)/theta;
00133     }
00134     double C = cos(0.5*theta);
00135     return Eigen::Quaterniond(C, S*delta(0), S*delta(1), S*delta(2));
00136   }
00137 
00138   static const int dim = 3;
00139   static const char* name() {
00140     return "Rot3d";
00141   }
00142 
00143 #ifdef USE_QUATERNIONS
00144 
00145   // new quaternion (and rotation matrix) based Rot3d
00146 
00147   // optimization is based on quaternions, but rotations matrices and
00148   // Euler angles can also be set or requested; efficiency is achieved
00149   // by only converting once needed in combination with caching
00150 
00151 private:
00152 
00153   // quat always exists, while rotation matrix and Euler angles are cached
00154   Eigen::Quaterniond _quat;
00155 
00156   // the following variables can be changed by const methods: it is
00157   // essential that the class still logically behaves as const !!!!
00158   mutable bool _wRo_cached;
00159   mutable Eigen::Matrix3d _wRo;
00160   mutable bool _ypr_cached;
00161   mutable double _yaw;
00162   mutable double _pitch;
00163   mutable double _roll;
00164 
00165   // calculate ypr if it doesn't exist
00166   void ensure_ypr() const {
00167     if (!_ypr_cached) {
00168       quat_to_euler(_quat, _yaw, _pitch, _roll);
00169       _ypr_cached = true;
00170     }
00171   }
00172 
00173 public:
00174 
00175   static const int size = 4;
00176 
00177   Rot3d() : _quat(Eigen::Quaterniond(1,0,0,0)),
00178     _wRo_cached(true), _wRo(Eigen::Matrix<double, 3, 3>::Identity()),
00179     _ypr_cached(true), _yaw(0.), _pitch(0.), _roll(0.) {}
00180 
00181   Rot3d(const Eigen::Quaterniond& quat) : _quat(quat), _wRo_cached(false), _ypr_cached(false) {}
00182 
00183   Rot3d(double yaw, double pitch, double roll) {
00184     set(yaw, pitch, roll);
00185   }
00186 
00187   Rot3d(const Eigen::Matrix3d& wRo) {
00188     _wRo = wRo;
00189     _wRo_cached = true;
00190     _ypr_cached = false;
00191     _quat = wRo_to_quat(wRo);
00192   }
00193 
00194   void write(std::ostream &out) const {
00195     out << yaw() << " " << pitch() << " " << roll() <<
00196         " (quat: " << _quat.w() << " " << _quat.x() << " " << _quat.y() << " " << _quat.z() << ")";
00197   }
00198 
00199   double x() const {return _quat.x();}
00200   double y() const {return _quat.y();}
00201   double z() const {return _quat.z();}
00202   double w() const {return _quat.w();}
00203 
00204   double yaw()   const {ensure_ypr(); return _yaw;}
00205   double pitch() const {ensure_ypr(); return _pitch;}
00206   double roll()  const {ensure_ypr(); return _roll;}
00207 
00208   void ypr(double& yaw, double& pitch, double& roll) const {
00209     ensure_ypr();
00210     yaw   = _yaw;
00211     pitch = _pitch;
00212     roll  = _roll;
00213   }
00214 
00215   void set_yaw  (double yaw)   {ensure_ypr(); set(yaw, _pitch, _roll);}
00216   void set_pitch(double pitch) {ensure_ypr(); set(_yaw, pitch, _roll);}
00217   void set_roll (double roll)  {ensure_ypr(); set(_yaw, _pitch, roll);}
00218 
00219   void set(double yaw, double pitch, double roll) {
00220     _yaw   = yaw;
00221     _pitch = pitch;
00222     _roll  = roll;
00223     _ypr_cached = true;
00224     _wRo_cached = false;
00225     _quat = euler_to_quat(yaw, pitch, roll);
00226   }
00227 
00228 
00229   Rot3d exmap(const Eigen::VectorXd& delta) const {
00230 #if 1
00231     // direct solution by mapping to quaternion (following Grassia98jgt)
00232     Rot3d rot(_quat * delta3_to_quat(delta));
00233     return rot;
00234 #else
00235     // cumbersome and slower solution
00236     double theta = delta.norm();
00237     double a, b, c;
00238     if (theta>1e-10) {
00239       a = delta(0)/theta;
00240       b = delta(1)/theta;
00241       c = delta(2)/theta;
00242     } else {
00243       a = b = c = 0.;
00244     }
00245     double S = sin(theta);
00246     double C = 1-cos(theta);
00247     Eigen::Matrix3d m;
00248     m <<
00249       1+(-a*a-b*b)*C, a*S-b*c*C, b*S+a*c*C,
00250       -a*S-b*c*C, 1+(-a*a-c*c)*C, c*S-a*b*C,
00251       -b*S+a*c*C, -c*S-a*b*C, 1+(-b*b-c*c)*C;
00252     return Rot3d(wRo() * m);
00253 #endif
00254   }
00255 
00256   Eigen::Quaterniond quaternion() const {
00257     return _quat;
00258   }
00259 
00264   const Eigen::Matrix3d& wRo() const {
00265     if (!_wRo_cached) {
00266       _wRo = quat_to_wRo(_quat);
00267       _wRo_cached = true;
00268     }
00269     return _wRo;
00270   }
00271 
00276   Eigen::Matrix3d oRw() const {
00277     return wRo().transpose();
00278   }
00279 
00280 #else
00281 
00282   // old Euler-angle based Rot3d
00283 
00284 private:
00285 
00286   double _yaw;
00287   double _pitch;
00288   double _roll;
00289 
00290 public:
00291 
00292   static const int size = 3;
00293 
00294   Rot3d() : _yaw(0.), _pitch(0.), _roll(0.) {}
00295 
00296   Rot3d(double yaw, double pitch, double roll) : _yaw(yaw), _pitch(pitch), _roll(roll) {}
00297 
00301   Rot3d(const Eigen::MatrixXd& wRo) {
00302     // note that getting the sign right requires recovering both sin and cos
00303     // for each angle; some equations exploit the fact that sin^2+cos^2=1
00304     _yaw = standardRad(atan2(wRo(1,0), wRo(0,0)));
00305     double c = cos(_yaw);
00306     double s = sin(_yaw);
00307     _pitch = standardRad(atan2(-wRo(2,0), wRo(0,0)*c + wRo(1,0)*s));
00308     _roll  = standardRad(atan2(wRo(0,2)*s - wRo(1,2)*c, -wRo(0,1)*s + wRo(1,1)*c));
00309   }
00310 
00311   void write(std::ostream &out) const {
00312     out << yaw() << " " << pitch() << " " << roll();
00313   }
00314 
00315   double yaw()   const {return _yaw;}
00316   double pitch() const {return _pitch;}
00317   double roll()  const {return _roll;}
00318 
00319   void ypr(double& yaw, double& pitch, double& roll) const {
00320     yaw = _yaw; pitch = _pitch; roll = _roll;
00321   }
00322 
00323   void set_yaw  (double yaw)   {_yaw = yaw;}
00324   void set_pitch(double pitch) {_pitch = pitch;}
00325   void set_roll (double roll)  {_roll = roll;}
00326 
00327   void set(double yaw, double pitch, double roll) {
00328     _yaw = yaw; _pitch = pitch; _roll = roll;
00329   }
00330 
00331   Rot3d exmap(const Eigen::VectorXd& delta) const {
00332     Rot3d res = *this;
00333     res._yaw   = standardRad(res._yaw   + delta(0));
00334     res._pitch = standardRad(res._pitch + delta(1));
00335     res._roll  = standardRad(res._roll  + delta(2));
00336     return res;
00337   }
00338 
00343   Eigen::MatrixXd wRo() const {
00344     Eigen::MatrixXd ret = euler_to_wRo(_yaw, _pitch, _roll);
00345     return ret;
00346   }
00347 
00348 #endif
00349 
00350 
00351 };
00352 
00353 }
00354 
 All Classes Files Functions Variables