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/base/concepts.h>
#include <gtsam/config.h>  // Get GTSAM_USE_QUATERNIONS macro
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/Unit3.h>
 
#include <random>
#include <stdexcept>
#include <utility>
 
// 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
 
namespace gtsam {
 
namespace internal {
template <typename G>
struct TangentLieGroupJacobian;
}  // namespace internal

class GTSAM_EXPORT Rot3 : public MatrixLieGroup<Rot3, 3, 3> {
 public:
  static constexpr size_t MatrixM = 3;
 private:
 
#ifdef GTSAM_USE_QUATERNIONS
    gtsam::Quaternion quaternion_;
#else
    SO3 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);

    template <typename Derived>
#ifdef GTSAM_USE_QUATERNIONS
    explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
      quaternion_ = Matrix3(R);
    }
#else
    explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
    }
#endif

#ifdef GTSAM_USE_QUATERNIONS
    explicit Rot3(const Matrix3& R) : quaternion_(R) {}
#else
    explicit Rot3(const Matrix3& R) : rot_(R) {}
#endif

#ifdef GTSAM_USE_QUATERNIONS
    explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
#else
    explicit Rot3(const SO3& R) : rot_(R) {}
#endif

    Rot3(const Quaternion& q);
    Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}

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

    virtual ~Rot3() {}

    static bool IsValid(const Matrix3& R, double tol = 1e-9);
 
    /* 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,
                       OptionalJacobian<3, 1> Hx = {},
                       OptionalJacobian<3, 1> Hy = {},
                       OptionalJacobian<3, 1> Hz = {});

    inline static Rot3 RzRyRx(const Vector& xyz,
                              OptionalJacobian<3, 3> H = {}) {
#ifndef NDEBUG
      if (xyz.size() != 3) {
        throw;
      }
#endif
      Rot3 out;
      if (H) {
        Vector3 Hx, Hy, Hz;
        out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
        (*H) << Hx, Hy, Hz;
      } else
        out = RzRyRx(xyz(0), xyz(1), xyz(2));
      return out;
    }

    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,
                    OptionalJacobian<3, 1> Hy = {},
                    OptionalJacobian<3, 1> Hp = {},
                    OptionalJacobian<3, 1> Hr = {}) {
      return RzRyRx(r, p, y, Hr, Hp, Hy);
    }

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

    static Rot3 AxisAngle(const Point3& axis, double angle) {
      // Convert to unit vector.
      Vector3 unitAxis = Unit3(axis).unitVector();
#ifdef GTSAM_USE_QUATERNIONS
      return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
#else
      return Rot3(SO3::AxisAngle(unitAxis,angle));
#endif
    }

    static Rot3 AxisAngle(const Unit3& axis, double angle) {
      return AxisAngle(axis.unitVector(),angle);
    }

    static Rot3 Rodrigues(const Vector3& w) {
      return Rot3::Expmap(w);
    }

    static Rot3 Rodrigues(double wx, double wy, double wz) {
      return Rodrigues(Vector3(wx, wy, wz));
    }

    static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);

    static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                              const Unit3& a_q, const Unit3& b_q);

    static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }

    Rot3 normalized() const;


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

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


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

    Rot3 operator*(const Rot3& R2) const;

    Rot3 inverse() const {
#ifdef GTSAM_USE_QUATERNIONS
      return Rot3(quaternion_.inverse());
#else
      return Rot3(rot_.matrix().transpose());
#endif
    }

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


    enum CoordinatesMode {
      EXPMAP, 
#ifndef GTSAM_USE_QUATERNIONS
      CAYLEY, 
#endif
      };
 
#ifndef GTSAM_USE_QUATERNIONS
 
    // Cayley chart around origin
    struct GTSAM_EXPORT CayleyChart {
    static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = {});
    static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = {});
    };

    Rot3 retractCayley(const Vector& omega) const {
      return compose(CayleyChart::Retract(omega));
    }

    Vector3 localCayley(const Rot3& other) const {
      return CayleyChart::Local(between(other));
    }
 
#endif

 
    using LieAlgebra = Matrix3;

    static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {});

    static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {});

    static Matrix3 ExpmapDerivative(const Vector3& x);

    static Matrix3 LogmapDerivative(const Vector3& x);

    Matrix3 AdjointMap() const { return matrix(); }

    static Matrix3 adjointMap(const Vector3& xi) { return Hat(xi); }
 
    // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
    struct GTSAM_EXPORT ChartAtOrigin {
      static Rot3 Retract(const Vector3& v, ChartJacobian H = {});
      static Vector3 Local(const Rot3& r, ChartJacobian H = {});
    };
 
    using LieGroup<Rot3, 3>::inverse; // version with derivative

    static inline Matrix3 Hat(const Vector3& xi) { return SO3::Hat(xi); }

    static inline Vector3 Vee(const Matrix3& X) { return SO3::Vee(X); }


    Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
        OptionalJacobian<3,3> H2 = {}) const;

    Point3 operator*(const Point3& p) const;

    Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = {},
        OptionalJacobian<3,3> H2={}) const;


    Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
        OptionalJacobian<2,2> Hp = {}) const;

    Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = {},
        OptionalJacobian<2,2> Hp = {}) const;

    Unit3 operator*(const Unit3& p) const;


    Matrix3 matrix() const;

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

    Vector3 xyz(OptionalJacobian<3, 3> H = {}) const;

    Vector3 ypr(OptionalJacobian<3, 3> H = {}) const;

    Vector3 rpy(OptionalJacobian<3, 3> H = {}) const;

    double roll(OptionalJacobian<1, 3> H = {}) const;

    double pitch(OptionalJacobian<1, 3> H = {}) const;

    double yaw(OptionalJacobian<1, 3> H = {}) const;


    std::pair<Unit3, double> axisAngle() const;

    gtsam::Quaternion toQuaternion() const;

    Rot3 slerp(double t, const Rot3& other) const;

    inline Vector9 vec(OptionalJacobian<9, 3> H = {}) const { return SO3(matrix()).vec(H); }

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

 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
    Point3 column(int index) const;
#endif

 
   private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template <class ARCHIVE>
    void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
#ifndef GTSAM_USE_QUATERNIONS
      Matrix3& M = rot_.matrix_;
      ar& boost::serialization::make_nvp("rot11", M(0, 0));
      ar& boost::serialization::make_nvp("rot12", M(0, 1));
      ar& boost::serialization::make_nvp("rot13", M(0, 2));
      ar& boost::serialization::make_nvp("rot21", M(1, 0));
      ar& boost::serialization::make_nvp("rot22", M(1, 1));
      ar& boost::serialization::make_nvp("rot23", M(1, 2));
      ar& boost::serialization::make_nvp("rot31", M(2, 0));
      ar& boost::serialization::make_nvp("rot32", M(2, 1));
      ar& boost::serialization::make_nvp("rot33", M(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
    }
#endif
  };

  using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3>>;

  GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
      const Matrix3& A, OptionalJacobian<3, 9> H = {});
 
template <>
struct traits<Rot3> : public internal::MatrixLieGroup<Rot3, 3> {
  inline constexpr static int QcqpVectorDim = 10;

  template <int D = 1>
  static Matrix QcqpValue(const Rot3& value) {
    if constexpr (D == 1) {
      const Matrix3 R = value.matrix();
      Vector10 X;
      X(0, 0) = 1.0;  // Homogenization entry.
      X.bottomRows(9) = Eigen::Map<const Matrix>(R.data(), 9, 1);
      return X;
    } else if constexpr (D >= 3) {
      Matrix X = Matrix::Zero(3, D);
      X.template leftCols<3>() = value.matrix().transpose();
      return X;
    } else {
      throw std::invalid_argument(
          "traits<Rot3>::QcqpValue only supports D=1 and D>=3.");
    }
  }

  template <int D = 1>
  static std::vector<std::pair<Matrix, double>> QcqpConstraints() {
    if constexpr (D == 1) {
      // The homogenized Rot3 lifted vector is
      // x = [1, r00, r10, r20, r01, r11, r21, r02, r12, r22].
      std::vector<std::pair<Matrix, double>> constraints;
      constraints.reserve(10);
 
      Matrix A = Matrix::Zero(10, 10);
 
      // The quadratic lift fixes x(0)^2 = 1.
      A(0, 0) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      // cross(R.col(1), R.col(2)) = x(0) * R.col(0).
      A.setZero();
      A(5, 9) = 0.5;
      A(9, 5) = 0.5;
      A(6, 8) = -0.5;
      A(8, 6) = -0.5;
      A(0, 1) = -0.5;
      A(1, 0) = -0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(6, 7) = 0.5;
      A(7, 6) = 0.5;
      A(4, 9) = -0.5;
      A(9, 4) = -0.5;
      A(0, 2) = -0.5;
      A(2, 0) = -0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(4, 8) = 0.5;
      A(8, 4) = 0.5;
      A(5, 7) = -0.5;
      A(7, 5) = -0.5;
      A(0, 3) = -0.5;
      A(3, 0) = -0.5;
      constraints.emplace_back(A, 0.0);
 
      // RR^T = I supplies the six row-orthonormality constraints.
      A.setZero();
      A(1, 1) = 1.0;
      A(4, 4) = 1.0;
      A(7, 7) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(1, 2) = 0.5;
      A(2, 1) = 0.5;
      A(4, 5) = 0.5;
      A(5, 4) = 0.5;
      A(7, 8) = 0.5;
      A(8, 7) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(1, 3) = 0.5;
      A(3, 1) = 0.5;
      A(4, 6) = 0.5;
      A(6, 4) = 0.5;
      A(7, 9) = 0.5;
      A(9, 7) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(2, 2) = 1.0;
      A(5, 5) = 1.0;
      A(8, 8) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(2, 3) = 0.5;
      A(3, 2) = 0.5;
      A(5, 6) = 0.5;
      A(6, 5) = 0.5;
      A(8, 9) = 0.5;
      A(9, 8) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(3, 3) = 1.0;
      A(6, 6) = 1.0;
      A(9, 9) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      return constraints;
    } else if constexpr (D >= 3) {
      std::vector<std::pair<Matrix, double>> constraints;
      constraints.reserve(6);
 
      // 3 row-unit-norm: ||row r||^2 = 1.
      for (int r = 0; r < 3; ++r) {
        Matrix A = Matrix::Zero(3, 3);
        A(r, r) = 1.0;
        constraints.emplace_back(A, 1.0);
      }
 
      // 3 row-orthogonality: row r1 . row r2 = 0.
      for (int r1 = 0; r1 < 3; ++r1) {
        for (int r2 = r1 + 1; r2 < 3; ++r2) {
          Matrix A = Matrix::Zero(3, 3);
          A(r1, r2) = 0.5;
          A(r2, r1) = 0.5;
          constraints.emplace_back(A, 0.0);
        }
      }
      return constraints;
    } else {
      throw std::invalid_argument(
          "traits<Rot3>::QcqpConstraints only supports D=1 and D>=3.");
    }
  }

  template <int D>
  static Rot3 FromQcqpValue(const Matrix& X) {
    if constexpr (D == 1) {
      if (X.rows() != QcqpVectorDim || X.cols() != 1 ||
          std::abs(X(0, 0)) < 1e-9) {
        throw std::invalid_argument(
            "traits<Rot3>::FromQcqpValue requires a 10-by-1 vector with a "
            "nonzero homogenization entry.");
      }
      const Vector x = X.col(0) / X(0, 0);
      Matrix3 R;
      R.col(0) = x.segment<3>(1);
      R.col(1) = x.segment<3>(4);
      R.col(2) = x.segment<3>(7);
      return Rot3::ClosestTo(R);
    } else {
      static_assert(D >= 3,
                    "traits<Rot3>::FromQcqpValue requires D >= 3.");
      if (X.rows() != 3 || X.cols() != D) {
        throw std::invalid_argument(
            "traits<Rot3>::FromQcqpValue requires a 3-by-D matrix.");
      }
      return Rot3::ClosestTo(X.template leftCols<3>().transpose());
    }
  }
};
 
template <>
struct traits<const Rot3> : public traits<Rot3> {};
 
namespace internal {

template <>
struct TangentLieGroupJacobian<Rot3> {
  static constexpr bool available = true;
  static constexpr bool expmapAvailable = true;

  static std::pair<Rot3, Vector3> expmap(
      const Vector3& omega, const Vector3& v,
      OptionalJacobian<6, 6> derivative = {}) {
    const so3::DexpFunctor local(omega);
#ifdef GTSAM_USE_QUATERNIONS
    const Rot3 rotation = traits<gtsam::Quaternion>::Expmap(omega);
#else
    const Rot3 rotation(local.expmap());
#endif
    const Vector3 transported =
        local.tangentExpmap(v, rotation.matrix(), derivative);
    return {rotation, transported};
  }
 
  static Matrix6 rightJacobian(const Vector3& omega, const Vector3& v) {
    Matrix6 derivative;
    expmap(omega, v, derivative);
    return derivative;
  }
};
 
}  // namespace internal
 
}  // namespace gtsam