Rot2.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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/MatrixLieGroup.h>
#include <gtsam/geometry/Point2.h>
 
#include <random>
#include <stdexcept>
#include <utility>
#include <vector>
 
namespace gtsam {

  class GTSAM_EXPORT Rot2 : public MatrixLieGroup<Rot2, 1, 2> {
    double c_, s_;

    inline Rot2(double c, double s) : c_(c), s_(s) {}
 
  public:


    Rot2() : c_(1.0), s_(0.0) {}
    
    Rot2(const Rot2& r) = default;
 
    Rot2& operator=(const Rot2& other) = default;

    Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
 
    // Rot2& operator=(const gtsam::Rot2& other) = default;

    static Rot2 fromAngle(double theta) {
      return Rot2(theta);
    }

    static Rot2 fromDegrees(double theta) {
      static const double degree = M_PI / 180;
      return fromAngle(theta * degree);
    }

    static Rot2 fromCosSin(double c, double s);

    static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
        {});

    static Rot2 atan2(double y, double x);

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


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

    bool equals(const Rot2& R, double tol = 1e-9) const;


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

    Rot2 inverse() const { return Rot2(c_, -s_);}

    Rot2 operator*(const Rot2& R) const {
      return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
    }

 
    using LieAlgebra = Matrix2;

    static Rot2 Expmap(const Vector1& v, ChartJacobian H = {});

    static Vector1 Logmap(const Rot2& r, ChartJacobian H = {});

    Matrix1 AdjointMap() const { return I_1x1; }

    static Matrix1 adjointMap(const Vector1&);

    static Vector1 adjoint(const Vector1&, const Vector1&,
                           OptionalJacobian<1, 1> Hxi = {},
                           OptionalJacobian<1, 1> Hy = {});

    static Matrix ExpmapDerivative(const Vector& /*v*/) {
      return I_1x1;
    }

    static Matrix LogmapDerivative(const Vector& /*v*/) {
      return I_1x1;
    }
 
    // Chart at origin simply uses exponential map and its inverse
    struct ChartAtOrigin {
      static Rot2 Retract(const Vector1& v, ChartJacobian H = {}) {
        return Expmap(v, H);
      }
      static Vector1 Local(const Rot2& r, ChartJacobian H = {}) {
        return Logmap(r, H);
      }
    };
 
    using LieGroup<Rot2, 1>::inverse; // version with derivative

    static Matrix2 Hat(const Vector1& xi);

    static Vector1 Vee(const Matrix2& X);


    Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
        OptionalJacobian<2, 2> H2 = {}) const;

    inline Point2 operator*(const Point2& p) const {
      return rotate(p);
    }

    Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
        OptionalJacobian<2, 2> H2 = {}) const;


    inline Point2 unit() const {
      return Point2(c_, s_);
    }

    double theta() const {
      return ::atan2(s_, c_);
    }

    double degrees() const {
      const double degree = M_PI / 180;
      return theta() / degree;
    }

    inline double c() const {
      return c_;
    }

    inline double s() const {
      return s_;
    }

    Matrix2 matrix() const;

    Matrix2 transpose() const;

    static Rot2 ClosestTo(const Matrix2& M);

    Vector4 vec(OptionalJacobian<4, 1> H = {}) const;
 
    private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_NVP(c_);
      ar & BOOST_SERIALIZATION_NVP(s_);
    }
#endif
 
  };
 
template <>
struct traits<Rot2> : public internal::MatrixLieGroup<Rot2, 2> {
  inline constexpr static int QcqpVectorDim = 3;

  template <int D = 1>
  static Matrix QcqpValue(const Rot2& value) {
    if constexpr (D == 1) {
      return Vector3(1.0, value.c(), value.s());
    } else if constexpr (D >= 2) {
      Matrix X = Matrix::Zero(2, D);
      X.leftCols<2>() = value.matrix().transpose();
      return X;
    } else {
      throw std::invalid_argument(
          "traits<Rot2>::QcqpValue requires D>=1.");
    }
  }

  template <int D = 1>
  static std::vector<std::pair<Matrix, double>> QcqpConstraints() {
    if constexpr (D == 1) {
      // The minimal homogenized Rot2 lifted vector is x = [h, c, s].
      std::vector<std::pair<Matrix, double>> constraints;
      constraints.reserve(2);
 
      Matrix A = Matrix::Zero(QcqpVectorDim, QcqpVectorDim);
 
      // The quadratic lift fixes x(0)^2 = 1; a hard prior pins its sign.
      A(0, 0) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      // c^2 + s^2 = 1 simultaneously enforces orthonormality and det(R)=1.
      A.setZero();
      A(1, 1) = 1.0;
      A(2, 2) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      return constraints;
    } else if constexpr (D >= 2) {
      std::vector<std::pair<Matrix, double>> constraints;
      constraints.reserve(3);
 
      Matrix A = Matrix::Zero(2, 2);
      A(0, 0) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(1, 1) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(0, 1) = 0.5;
      A(1, 0) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      return constraints;
    } else {
      throw std::invalid_argument(
          "traits<Rot2>::QcqpConstraints only supports D=1 and D>=2.");
    }
  }

  template <int D>
  static Rot2 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<Rot2>::FromQcqpValue requires a 3-by-1 vector with a "
            "nonzero homogenization entry.");
      }
      const Vector x = X.col(0) / X(0, 0);
      return Rot2::atan2(x(2), x(1));
    } else {
      static_assert(D >= 2,
                    "traits<Rot2>::FromQcqpValue requires D >= 2.");
      if (X.rows() != 2 || X.cols() != D) {
        throw std::invalid_argument(
            "traits<Rot2>::FromQcqpValue requires a 2-by-D matrix.");
      }
      return Rot2::ClosestTo(X.template leftCols<2>().transpose());
    }
  }
};
 
template <>
struct traits<const Rot2> : public traits<Rot2> {};
 
}  // namespace gtsam