gtsam/gtsam/geometry/Rot2.h

/* ----------------------------------------------------------------------------

 * 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

 * -------------------------------------------------------------------------- */

/**
 * @file Rot2.h
 * @brief 2D rotation
 * @date Dec 9, 2009
 * @author Frank Dellaert
 */

#pragma once

#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie.h>
#include <boost/optional.hpp>

namespace gtsam {

  /**
   * Rotation matrix
   * NOTE: the angle theta is in radians unless explicitly stated
   * @addtogroup geometry
   * \nosubgrouping
   */
  class GTSAM_EXPORT Rot2 {

  public:
    /** get the dimension by the type */
    static const size_t dimension = 1;

  private:

    /** we store cos(theta) and sin(theta) */
    double c_, s_;


    /** normalize to make sure cos and sin form unit vector */
    Rot2& normalize();

    /** private constructor from cos/sin */
    inline Rot2(double c, double s) :
        c_(c), s_(s) {
    }

  public:

    /// @name Constructors and named constructors
    /// @{

    /** default constructor, zero rotation */
    Rot2() :
        c_(1.0), s_(0.0) {
    }

    /// Constructor from angle in radians == exponential map at identity
    Rot2(double theta) :
        c_(cos(theta)), s_(sin(theta)) {
    }

    /// Named constructor from angle in radians
    static Rot2 fromAngle(double theta) {
      return Rot2(theta);
    }

    /// Named constructor from angle in degrees
    static Rot2 fromDegrees(double theta) {
      const double degree = M_PI / 180;
      return fromAngle(theta * degree);
    }

    /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize!
    static Rot2 fromCosSin(double c, double s);

    /**
     * Named constructor with derivative
     * Calculate relative bearing to a landmark in local coordinate frame
     * @param d 2D location of landmark
     * @param H optional reference for Jacobian
     * @return 2D rotation \f$ \in SO(2) \f$
     */
    static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
        boost::none);

    /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
    static Rot2 atan2(double y, double x);

    /// @}
    /// @name Testable
    /// @{

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

    /** equals with an tolerance */
    bool equals(const Rot2& R, double tol = 1e-9) const;

    /// @}
    /// @name Group
    /// @{

    /** identity */
    inline static Rot2 identity() {  return Rot2(); }

    /** The inverse rotation - negative angle */
    Rot2 inverse(OptionalJacobian<1,1> H = boost::none) const {
      if (H) *H = -I_1x1;
      return Rot2(c_, -s_);
    }

    /** Compose - make a new rotation by adding angles */
    Rot2 operator*(const Rot2& R) const {
      return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
    }

    /** Compose - make a new rotation by adding angles */
    Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
        boost::none, OptionalJacobian<1,1> H2 = boost::none) const;

    /** Between using the default implementation */
    Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
        boost::none, OptionalJacobian<1,1> H2 = boost::none) const;

    /// @}
    /// @name Manifold
    /// @{

    /// dimension of the variable - used to autodetect sizes
    inline static size_t Dim() {
      return dimension;
    }

    /// Dimensionality of the tangent space, DOF = 1
    inline size_t dim() const {
      return dimension;
    }

    /// Updates a with tangent space delta
    Rot2 retract(const Vector& v, OptionalJacobian<1, 1> H1 = boost::none,
        OptionalJacobian<1, 1> H2 = boost::none) const;

    /// Returns inverse retraction
    Vector1 localCoordinates(const Rot2& t2, OptionalJacobian<1, 1> H1 =
        boost::none, OptionalJacobian<1, 1> H2 = boost::none) const;

    /// @}
    /// @name Lie Group
    /// @{

    /** Calculate Adjoint map */
    Matrix1 AdjointMap() const { return I_1x1; }

    ///Exponential map at identity - create a rotation from canonical coordinates
    static Rot2 Expmap(const Vector& v, OptionalJacobian<1, 1> H = boost::none);

    ///Log map at identity - return the canonical coordinates of this rotation
    static Vector1 Logmap(const Rot2& r, OptionalJacobian<1, 1> H = boost::none);

    /// Left-trivialized derivative of the exponential map
    static Matrix dexpL(const Vector& v) {
      return ones(1);
    }

    /// Left-trivialized derivative inverse of the exponential map
    static Matrix dexpInvL(const Vector& v) {
      return ones(1);
    }

    /// @}
    /// @name Group Action on Point2
    /// @{

    /**
     * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
     */
    Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
        OptionalJacobian<2, 2> H2 = boost::none) const;

    /** syntactic sugar for rotate */
    inline Point2 operator*(const Point2& p) const {
      return rotate(p);
    }

    /**
     * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
     */
    Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
        OptionalJacobian<2, 2> H2 = boost::none) const;

    /// @}
    /// @name Standard Interface
    /// @{

    /// Creates a unit vector as a Point2
    inline Point2 unit() const {
      return Point2(c_, s_);
    }

    /** return angle (RADIANS) */
    double theta() const {
      return ::atan2(s_, c_);
    }

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

    /** return cos */
    inline double c() const {
      return c_;
    }

    /** return sin */
    inline double s() const {
      return s_;
    }

    /** return 2*2 rotation matrix */
    Matrix2 matrix() const;

    /** return 2*2 transpose (inverse) rotation matrix   */
    Matrix2 transpose() const;

  private:
    /** Serialization function */
    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_);
    }

  };

  template<>
  struct traits_x<Rot2> : public internal::LieGroupTraits<Rot2> {};

} // gtsam