gtsam  4.1.0
gtsam
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 
19 #pragma once
20 
21 #include <gtsam/geometry/Point2.h>
22 #include <gtsam/base/Lie.h>
23 #include <boost/optional.hpp>
24 
25 #include <random>
26 
27 namespace gtsam {
28 
35  class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
36 
38  double c_, s_;
39 
41  Rot2& normalize();
42 
44  inline Rot2(double c, double s) : c_(c), s_(s) {}
45 
46  public:
47 
50 
52  Rot2() : c_(1.0), s_(0.0) {}
53 
55  Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
56 
58  static Rot2 fromAngle(double theta) {
59  return Rot2(theta);
60  }
61 
63  static Rot2 fromDegrees(double theta) {
64  static const double degree = M_PI / 180;
65  return fromAngle(theta * degree);
66  }
67 
69  static Rot2 fromCosSin(double c, double s);
70 
78  static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
79  boost::none);
80 
82  static Rot2 atan2(double y, double x);
83 
90  static Rot2 Random(std::mt19937 & rng);
91 
95 
97  void print(const std::string& s = "theta") const;
98 
100  bool equals(const Rot2& R, double tol = 1e-9) const;
101 
105 
107  inline static Rot2 identity() { return Rot2(); }
108 
110  Rot2 inverse() const { return Rot2(c_, -s_);}
111 
113  Rot2 operator*(const Rot2& R) const {
114  return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
115  }
116 
120 
122  static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
123 
125  static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
126 
128  Matrix1 AdjointMap() const { return I_1x1; }
129 
131  static Matrix ExpmapDerivative(const Vector& /*v*/) {
132  return I_1x1;
133  }
134 
136  static Matrix LogmapDerivative(const Vector& /*v*/) {
137  return I_1x1;
138  }
139 
140  // Chart at origin simply uses exponential map and its inverse
141  struct ChartAtOrigin {
142  static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
143  return Expmap(v, H);
144  }
145  static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
146  return Logmap(r, H);
147  }
148  };
149 
150  using LieGroup<Rot2, 1>::inverse; // version with derivative
151 
155 
159  Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
160  OptionalJacobian<2, 2> H2 = boost::none) const;
161 
163  inline Point2 operator*(const Point2& p) const {
164  return rotate(p);
165  }
166 
170  Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
171  OptionalJacobian<2, 2> H2 = boost::none) const;
172 
176 
178  inline Point2 unit() const {
179  return Point2(c_, s_);
180  }
181 
183  double theta() const {
184  return ::atan2(s_, c_);
185  }
186 
188  double degrees() const {
189  const double degree = M_PI / 180;
190  return theta() / degree;
191  }
192 
194  inline double c() const {
195  return c_;
196  }
197 
199  inline double s() const {
200  return s_;
201  }
202 
204  Matrix2 matrix() const;
205 
207  Matrix2 transpose() const;
208 
209  private:
211  friend class boost::serialization::access;
212  template<class ARCHIVE>
213  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
214  ar & BOOST_SERIALIZATION_NVP(c_);
215  ar & BOOST_SERIALIZATION_NVP(s_);
216  }
217 
218  };
219 
220  template<>
221  struct traits<Rot2> : public internal::LieGroup<Rot2> {};
222 
223  template<>
224  struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
225 
226 } // gtsam
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Rot2
Definition: Rot2.h:35
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition: Point3.cpp:51
gtsam::Rot2::c
double c() const
return cos
Definition: Rot2.h:194
gtsam::Rot2::ExpmapDerivative
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition: Rot2.h:131
gtsam::Rot2::s
double s() const
return sin
Definition: Rot2.h:199
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::Rot2::LogmapDerivative
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition: Rot2.h:136
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:228
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition: Rot2.h:183
gtsam::Rot2::operator*
Rot2 operator*(const Rot2 &R) const
Compose - make a new rotation by adding angles.
Definition: Rot2.h:113
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:58
gtsam::Rot2::degrees
double degrees() const
return angle (DEGREES)
Definition: Rot2.h:188
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Rot2::AdjointMap
Matrix1 AdjointMap() const
Calculate Adjoint map.
Definition: Rot2.h:128
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Rot2::identity
static Rot2 identity()
identity
Definition: Rot2.h:107
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::Rot2::unit
Point2 unit() const
Creates a unit vector as a Point2.
Definition: Rot2.h:178
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
gtsam::Rot2::operator*
Point2 operator*(const Point2 &p) const
syntactic sugar for rotate
Definition: Rot2.h:163
Point2.h
2D Point
gtsam::Rot2::Rot2
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition: Rot2.h:55
gtsam::Rot2::ChartAtOrigin
Definition: Rot2.h:141
gtsam::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:63
gtsam::Rot2::Rot2
Rot2()
default constructor, zero rotation
Definition: Rot2.h:52
gtsam::Rot2::inverse
Rot2 inverse() const
The inverse rotation - negative angle.
Definition: Rot2.h:110
Lie.h
Base class and basic functions for Lie types.