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 
22 #include <gtsam/geometry/Point2.h>
23 #include <gtsam/base/Lie.h>
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(const Rot2& r) : Rot2(r.c_, r.s_) {}
56 
58  Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
59 
61  static Rot2 fromAngle(double theta) {
62  return Rot2(theta);
63  }
64 
66  static Rot2 fromDegrees(double theta) {
67  static const double degree = M_PI / 180;
68  return fromAngle(theta * degree);
69  }
70 
72  static Rot2 fromCosSin(double c, double s);
73 
81  static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
82  {});
83 
85  static Rot2 atan2(double y, double x);
86 
93  static Rot2 Random(std::mt19937 & rng);
94 
98 
100  void print(const std::string& s = "theta") const;
101 
103  bool equals(const Rot2& R, double tol = 1e-9) const;
104 
108 
110  inline static Rot2 Identity() { return Rot2(); }
111 
113  Rot2 inverse() const { return Rot2(c_, -s_);}
114 
116  Rot2 operator*(const Rot2& R) const {
117  return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
118  }
119 
123 
125  static Rot2 Expmap(const Vector1& v, ChartJacobian H = {});
126 
128  static Vector1 Logmap(const Rot2& r, ChartJacobian H = {});
129 
131  Matrix1 AdjointMap() const { return I_1x1; }
132 
134  static Matrix ExpmapDerivative(const Vector& /*v*/) {
135  return I_1x1;
136  }
137 
139  static Matrix LogmapDerivative(const Vector& /*v*/) {
140  return I_1x1;
141  }
142 
143  // Chart at origin simply uses exponential map and its inverse
144  struct ChartAtOrigin {
145  static Rot2 Retract(const Vector1& v, ChartJacobian H = {}) {
146  return Expmap(v, H);
147  }
148  static Vector1 Local(const Rot2& r, ChartJacobian H = {}) {
149  return Logmap(r, H);
150  }
151  };
152 
153  using LieGroup<Rot2, 1>::inverse; // version with derivative
154 
158 
162  Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
163  OptionalJacobian<2, 2> H2 = {}) const;
164 
166  inline Point2 operator*(const Point2& p) const {
167  return rotate(p);
168  }
169 
173  Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
174  OptionalJacobian<2, 2> H2 = {}) const;
175 
179 
181  inline Point2 unit() const {
182  return Point2(c_, s_);
183  }
184 
186  double theta() const {
187  return ::atan2(s_, c_);
188  }
189 
191  double degrees() const {
192  const double degree = M_PI / 180;
193  return theta() / degree;
194  }
195 
197  inline double c() const {
198  return c_;
199  }
200 
202  inline double s() const {
203  return s_;
204  }
205 
207  Matrix2 matrix() const;
208 
210  Matrix2 transpose() const;
211 
213  static Rot2 ClosestTo(const Matrix2& M);
214 
215  private:
216 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
217 
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 #endif
225 
226  };
227 
228  template<>
229  struct traits<Rot2> : public internal::LieGroup<Rot2> {};
230 
231  template<>
232  struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
233 
234 } // gtsam
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
double degrees() const
Definition: Rot2.h:191
double c() const
Definition: Rot2.h:197
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition: Rot2.h:134
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Scalar * y
std::string serialize(const T &input)
serializes to a string
double s_
Definition: Rot2.h:38
Vector2 Point2
Definition: Point2.h:32
static std::mt19937 rng
Point2 unit() const
Creates a unit vector as a Point2.
Definition: Rot2.h:181
Rot2 operator*(const Rot2 &R) const
Definition: Rot2.h:116
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:106
Rot2 R(Rot2::fromAngle(0.1))
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Pose2_ Expmap(const Vector3_ &xi)
Rot2(const Rot2 &r)
Definition: Rot2.h:55
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition: Rot2.h:139
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:66
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
const double degree
double s() const
Definition: Rot2.h:202
Eigen::VectorXd Vector
Definition: Vector.h:38
Rot2(double c, double s)
Definition: Rot2.h:44
Point2 operator*(const Point2 &p) const
Definition: Rot2.h:166
Array< int, Dynamic, 1 > v
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
static Rot2 Identity()
Definition: Rot2.h:110
static Rot2 Retract(const Vector1 &v, ChartJacobian H={})
Definition: Rot2.h:145
Base class and basic functions for Lie types.
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
traits
Definition: chartTesting.h:28
Rot2()
Definition: Rot2.h:52
Both LieGroupTraits and Testable.
Definition: Lie.h:229
static Vector1 Local(const Rot2 &r, ChartJacobian H={})
Definition: Rot2.h:148
Rot2 inverse() const
Definition: Rot2.h:113
float * p
Matrix1 AdjointMap() const
Definition: Rot2.h:131
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
const G double tol
Definition: Group.h:86
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition: Rot2.h:58
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
The matrix class, also used for vectors and row-vectors.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
2D Point
double c_
Definition: Rot2.h:38
double theta() const
Definition: Rot2.h:186


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:34