gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
Rot2.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
20#pragma once
21
23#include <gtsam/base/Lie.h>
24#include <boost/optional.hpp>
25
26#include <random>
27
28namespace gtsam {
29
36 class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
37
39 double c_, s_;
40
42 Rot2& normalize();
43
45 inline Rot2(double c, double s) : c_(c), s_(s) {}
46
47 public:
48
51
53 Rot2() : c_(1.0), s_(0.0) {}
54
56 Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
57
59 Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
60
62 static Rot2 fromAngle(double theta) {
63 return Rot2(theta);
64 }
65
67 static Rot2 fromDegrees(double theta) {
68 static const double degree = M_PI / 180;
69 return fromAngle(theta * degree);
70 }
71
73 static Rot2 fromCosSin(double c, double s);
74
82 static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
83 boost::none);
84
86 static Rot2 atan2(double y, double x);
87
94 static Rot2 Random(std::mt19937 & rng);
95
99
101 void print(const std::string& s = "theta") const;
102
104 bool equals(const Rot2& R, double tol = 1e-9) const;
105
109
111 inline static Rot2 Identity() { return Rot2(); }
112
114 Rot2 inverse() const { return Rot2(c_, -s_);}
115
117 Rot2 operator*(const Rot2& R) const {
118 return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
119 }
120
124
126 static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
127
129 static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
130
132 Matrix1 AdjointMap() const { return I_1x1; }
133
135 static Matrix ExpmapDerivative(const Vector& /*v*/) {
136 return I_1x1;
137 }
138
140 static Matrix LogmapDerivative(const Vector& /*v*/) {
141 return I_1x1;
142 }
143
144 // Chart at origin simply uses exponential map and its inverse
146 static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
147 return Expmap(v, H);
148 }
149 static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
150 return Logmap(r, H);
151 }
152 };
153
154 using LieGroup<Rot2, 1>::inverse; // version with derivative
155
159
163 Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
164 OptionalJacobian<2, 2> H2 = boost::none) const;
165
167 inline Point2 operator*(const Point2& p) const {
168 return rotate(p);
169 }
170
174 Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
175 OptionalJacobian<2, 2> H2 = boost::none) const;
176
180
182 inline Point2 unit() const {
183 return Point2(c_, s_);
184 }
185
187 double theta() const {
188 return ::atan2(s_, c_);
189 }
190
192 double degrees() const {
193 const double degree = M_PI / 180;
194 return theta() / degree;
195 }
196
198 inline double c() const {
199 return c_;
200 }
201
203 inline double s() const {
204 return s_;
205 }
206
208 Matrix2 matrix() const;
209
211 Matrix2 transpose() const;
212
214 static Rot2 ClosestTo(const Matrix2& M);
215
216 private:
218 friend class boost::serialization::access;
219 template<class ARCHIVE>
220 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
221 ar & BOOST_SERIALIZATION_NVP(c_);
222 ar & BOOST_SERIALIZATION_NVP(s_);
223 }
224
225 };
226
227 template<>
228 struct traits<Rot2> : public internal::LieGroup<Rot2> {};
229
230 template<>
231 struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
232
233} // gtsam
Base class and basic functions for Lie types.
2D Point
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
Definition Rot2.h:145