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 <boost/optional.hpp>

#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>

namespace gtsam {

  class GTSAM_EXPORT Rot2 : public DerivedValue<Rot2> {

  public:
    static const size_t dimension = 1;

  private:

    double c_, s_;


    Rot2& normalize();

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

  public:


    Rot2() :
        c_(1.0), s_(0.0) {
    }

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

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

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

    static Rot2 fromCosSin(double c, double s);

    static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
        boost::none);

    static Rot2 atan2(double y, double x);


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

    inline Rot2 compose(const Rot2& R, boost::optional<Matrix&> H1 =
        boost::none, boost::optional<Matrix&> H2 = boost::none) const {
      if (H1) *H1 = eye(1);
      if (H2) *H2 = eye(1);
      return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
    }

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

    inline Rot2 between(const Rot2& R, boost::optional<Matrix&> H1 =
        boost::none, boost::optional<Matrix&> H2 = boost::none) const {
      if (H1) *H1 = -eye(1);
      if (H2) *H2 = eye(1);
      return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
    }


    inline static size_t Dim() {
      return dimension;
    }

    inline size_t dim() const {
      return dimension;
    }

    inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }

    inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }


    static Rot2 Expmap(const Vector& v) {
      if (zero(v))
        return (Rot2());
      else
        return Rot2::fromAngle(v(0));
    }

    static inline Vector Logmap(const Rot2& r) {
      return (Vector(1) << r.theta());
    }


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

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

    Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
        boost::optional<Matrix&> H2 = boost::none) 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_;
    }

    Matrix matrix() const;

    Matrix transpose() const;


  private:
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int version) {
      ar & boost::serialization::make_nvp("Rot2",
          boost::serialization::base_object<Value>(*this));
      ar & BOOST_SERIALIZATION_NVP(c_);
      ar & BOOST_SERIALIZATION_NVP(s_);
    }


  };

} // gtsam