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/geometry/Point2.h>
#include <gtsam/base/Lie.h>
 
#include <random>
 
namespace gtsam {
 
  class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
    double c_, s_;
 
    Rot2& normalize();
 
    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 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> {};
 
  template<>
  struct traits<const Rot2> : public internal::MatrixLieGroup<Rot2> {};
 
} // gtsam