24#include <boost/optional.hpp>
45 inline Rot2(
double c,
double s) : c_(c), s_(s) {}
53 Rot2() : c_(1.0), s_(0.0) {}
59 Rot2(
double theta) : c_(cos(theta)), s_(sin(theta)) {}
68 static const double degree = M_PI / 180;
69 return fromAngle(theta * degree);
73 static Rot2 fromCosSin(
double c,
double s);
86 static Rot2 atan2(
double y,
double x);
94 static Rot2 Random(std::mt19937 & rng);
101 void print(
const std::string& s =
"theta")
const;
104 bool equals(
const Rot2& R,
double tol = 1e-9)
const;
118 return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
126 static Rot2 Expmap(
const Vector1& v, ChartJacobian H = boost::none);
129 static Vector1 Logmap(
const Rot2& r, ChartJacobian H = boost::none);
146 static Rot2 Retract(
const Vector1& v, ChartJacobian H = boost::none) {
149 static Vector1 Local(
const Rot2& r, ChartJacobian H = boost::none) {
188 return ::atan2(s_, c_);
193 const double degree = M_PI / 180;
194 return theta() / degree;
198 inline double c()
const {
203 inline double s()
const {
208 Matrix2 matrix()
const;
211 Matrix2 transpose()
const;
214 static Rot2 ClosestTo(
const Matrix2& M);
218 friend class boost::serialization::access;
219 template<
class ARCHIVE>
220 void serialize(ARCHIVE & ar,
const unsigned int ) {
221 ar & BOOST_SERIALIZATION_NVP(c_);
222 ar & BOOST_SERIALIZATION_NVP(s_);
Base class and basic functions for Lie types.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
Both LieGroupTraits and Testable.
Definition Lie.h:229
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Template to create a binary predicate.
Definition Testable.h:111
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
Rot2 operator*(const Rot2 &R) const
Compose - make a new rotation by adding angles.
Definition Rot2.h:117
double c() const
return cos
Definition Rot2.h:198
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition Rot2.h:135
Point2 unit() const
Creates a unit vector as a Point2.
Definition Rot2.h:182
double theta() const
return angle (RADIANS)
Definition Rot2.h:187
Rot2 inverse() const
The inverse rotation - negative angle.
Definition Rot2.h:114
Point2 operator*(const Point2 &p) const
syntactic sugar for rotate
Definition Rot2.h:167
double s() const
return sin
Definition Rot2.h:203
static Rot2 Identity()
Identity.
Definition Rot2.h:111
double degrees() const
return angle (DEGREES)
Definition Rot2.h:192
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition Rot2.h:140
Matrix1 AdjointMap() const
Calculate Adjoint map.
Definition Rot2.h:132
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition Rot2.h:59
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition Rot2.h:67
Rot2()
default constructor, zero rotation
Definition Rot2.h:53
Rot2(const Rot2 &r)
copy constructor
Definition Rot2.h:56
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition Rot2.h:62