Loading...
Searching...
No Matches
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 <boost/optional.hpp>
#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 =
boost::none);
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 = boost::none);
static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
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 = boost::none) {
return Expmap(v, H);
}
static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
return Logmap(r, H);
}
};
using LieGroup<Rot2, 1>::inverse; // version with derivative
Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
inline Point2 operator*(const Point2& p) const {
return rotate(p);
}
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> 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_;
}
Matrix2 matrix() const;
Matrix2 transpose() const;
static Rot2 ClosestTo(const Matrix2& M);
private:
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<Rot2> : public internal::LieGroup<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
} // gtsam
Lie.h
Base class and basic functions for Lie types.
Point2.h
2D Point
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::ChartJacobian
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
gtsam::Rot2::operator*
Rot2 operator*(const Rot2 &R) const
Compose - make a new rotation by adding angles.
Definition Rot2.h:117
gtsam::Rot2::c
double c() const
return cos
Definition Rot2.h:198
gtsam::Rot2::ExpmapDerivative
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition Rot2.h:135
gtsam::Rot2::Expmap
static Rot2 Expmap(const Vector1 &v, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition Rot2.cpp:67
gtsam::Rot2::rotate
Point2 rotate(const Point2 &p, OptionalJacobian< 2, 1 > H1=boost::none, OptionalJacobian< 2, 2 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot2.cpp:100
gtsam::Rot2::Logmap
static Vector1 Logmap(const Rot2 &r, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition Rot2.cpp:77
gtsam::Rot2::unit
Point2 unit() const
Creates a unit vector as a Point2.
Definition Rot2.h:182
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition Rot2.h:187
gtsam::Rot2::inverse
Rot2 inverse() const
The inverse rotation - negative angle.
Definition Rot2.h:114
gtsam::Rot2::operator*
Point2 operator*(const Point2 &p) const
syntactic sugar for rotate
Definition Rot2.h:167
gtsam::Rot2::fromCosSin
static Rot2 fromCosSin(double c, double s)
Named constructor from cos(theta),sin(theta) pair, will not normalize!
Definition Rot2.cpp:27
gtsam::Rot2::s
double s() const
return sin
Definition Rot2.h:203
gtsam::Rot2::Identity
static Rot2 Identity()
Identity.
Definition Rot2.h:111
gtsam::Rot2::degrees
double degrees() const
return angle (DEGREES)
Definition Rot2.h:192
gtsam::Rot2::LogmapDerivative
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition Rot2.h:140
gtsam::Rot2::AdjointMap
Matrix1 AdjointMap() const
Calculate Adjoint map.
Definition Rot2.h:132
gtsam::Rot2::Rot2
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition Rot2.h:59
gtsam::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition Rot2.h:67
gtsam::Rot2::Rot2
Rot2()
default constructor, zero rotation
Definition Rot2.h:53
gtsam::Rot2::Rot2
Rot2(const Rot2 &r)
copy constructor
Definition Rot2.h:56
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition Rot2.h:62
gtsam::Rot2::ChartAtOrigin
Definition Rot2.h:145