Rot3.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

// \callgraph

#pragma once

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

// You can override the default coordinate mode using this flag
#ifndef ROT3_DEFAULT_COORDINATES_MODE
  #ifdef GTSAM_USE_QUATERNIONS
    // Exponential map is very cheap for quaternions
    #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
  #else
    // If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building
    #ifndef GTSAM_ROT3_EXPMAP
      // For rotation matrices, the Cayley transform is a fast retract alternative
      #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY
    #else
      #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
    #endif
  #endif
#endif

#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Unit3.h>

namespace gtsam {

  typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;

  class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
  public:
    static const size_t dimension = 3;

  private:
#ifdef GTSAM_USE_QUATERNIONS

    Quaternion quaternion_;
#else
    Matrix3 rot_;
#endif

  public:


    Rot3();

    Rot3(const Point3& col1, const Point3& col2, const Point3& col3);

    Rot3(double R11, double R12, double R13,
        double R21, double R22, double R23,
        double R31, double R32, double R33);

    Rot3(const Matrix3& R);

    Rot3(const Matrix& R);

    Rot3(const Quaternion& q);

    static Rot3 Random(boost::mt19937 & rng);

    virtual ~Rot3() {}

    /* Static member function to generate some well known rotations */

    static Rot3 Rx(double t);

    static Rot3 Ry(double t);

    static Rot3 Rz(double t);

    static Rot3 RzRyRx(double x, double y, double z);

    inline static Rot3 RzRyRx(const Vector& xyz) {
      assert(xyz.size() == 3);
      return RzRyRx(xyz(0), xyz(1), xyz(2));
    }

    static Rot3 yaw  (double t) { return Rz(t); }

    static Rot3 pitch(double t) { return Ry(t); }

    static Rot3 roll (double t) { return Rx(t); }

    static Rot3 ypr  (double y, double p, double r) { return RzRyRx(r,p,y);}

    static Rot3 quaternion(double w, double x, double y, double z) {
      Quaternion q(w, x, y, z);
      return Rot3(q);
    }

    static Rot3 rodriguez(const Vector& w, double theta);

    static Rot3 rodriguez(const Point3& w, double theta);

    static Rot3 rodriguez(const Unit3& w, double theta);

    static Rot3 rodriguez(const Vector& v);

    static Rot3 rodriguez(double wx, double wy, double wz)
      { return rodriguez((Vector(3) << wx, wy, wz));}


    void print(const std::string& s="R") const;

    bool equals(const Rot3& p, double tol = 1e-9) const;


    inline static Rot3 identity() {
      return Rot3();
    }

    Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;

    Rot3 compose(const Rot3& R2,
    boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;

    Rot3 operator*(const Rot3& R2) const;

    Rot3 conjugate(const Rot3& cRb) const {
      // TODO: do more efficiently by using Eigen or quaternion properties
      return cRb * (*this) * cRb.inverse();
    }

    Rot3 between(const Rot3& R2,
        boost::optional<Matrix&> H1=boost::none,
        boost::optional<Matrix&> H2=boost::none) const;


    static size_t Dim() { return dimension; }

    size_t dim() const { return dimension; }

    enum CoordinatesMode {
      EXPMAP, 
#ifndef GTSAM_USE_QUATERNIONS
      CAYLEY, 
      SLOW_CAYLEY 
#endif
      };

#ifndef GTSAM_USE_QUATERNIONS
    Rot3 retractCayley(const Vector& omega) const;
#endif

    Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;

    Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;


    static Rot3 Expmap(const Vector& v)  {
      if(zero(v)) return Rot3();
      else return rodriguez(v);
    }

    static Vector3 Logmap(const Rot3& R);

    static Matrix3 dexpL(const Vector3& v);

    static Matrix3 dexpInvL(const Vector3& v);

    static Matrix3 rightJacobianExpMapSO3(const Vector3& x);

    static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x);


    Point3 rotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
        boost::optional<Matrix&> H2 = boost::none) const;

    Point3 operator*(const Point3& p) const;

    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
        boost::optional<Matrix&> H2 = boost::none) const;


    Unit3 rotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
        boost::optional<Matrix&> Hp = boost::none) const;

    Unit3 unrotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
        boost::optional<Matrix&> Hp = boost::none) const;

    Unit3 operator*(const Unit3& p) const;


    Matrix3 matrix() const;

    Matrix3 transpose() const;

    Point3 column(int index) const;

    Point3 r1() const; 
    Point3 r2() const; 
    Point3 r3() const; 

    Vector3 xyz() const;

    Vector3 ypr() const;

    Vector3 rpy() const;

    inline double roll() const  { return ypr()(2); }

    inline double pitch() const { return ypr()(1); }

    inline double yaw() const   { return ypr()(0); }


    Quaternion toQuaternion() const;

    Vector quaternion() const;

    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);

  private:
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int version)
    {
       ar & boost::serialization::make_nvp("Rot3",
           boost::serialization::base_object<Value>(*this));
#ifndef GTSAM_USE_QUATERNIONS
       ar & boost::serialization::make_nvp("rot11", rot_(0,0));
       ar & boost::serialization::make_nvp("rot12", rot_(0,1));
       ar & boost::serialization::make_nvp("rot13", rot_(0,2));
       ar & boost::serialization::make_nvp("rot21", rot_(1,0));
       ar & boost::serialization::make_nvp("rot22", rot_(1,1));
       ar & boost::serialization::make_nvp("rot23", rot_(1,2));
       ar & boost::serialization::make_nvp("rot31", rot_(2,0));
       ar & boost::serialization::make_nvp("rot32", rot_(2,1));
       ar & boost::serialization::make_nvp("rot33", rot_(2,2));
#else
      ar & boost::serialization::make_nvp("w", quaternion_.w());
      ar & boost::serialization::make_nvp("x", quaternion_.x());
      ar & boost::serialization::make_nvp("y", quaternion_.y());
      ar & boost::serialization::make_nvp("z", quaternion_.z());
#endif
    }
  };


  GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
}