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) : Rot2(r.c_, r.s_) {}

    Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}

    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_);
    }


    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


    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);

  private:
#ifdef 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::LieGroup<Rot2> {};

  template<>
  struct traits<const Rot2> : public internal::LieGroup<Rot2> {};

} // gtsam