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 
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
H
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
Definition: gnuplot_common_settings.hh:74
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::Rot2::ChartAtOrigin::Retract
static Rot2 Retract(const Vector1 &v, ChartJacobian H={})
Definition: Rot2.h:145
gtsam::Rot2::ChartAtOrigin::Local
static Vector1 Local(const Rot2 &r, ChartJacobian H={})
Definition: Rot2.h:148
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
gtsam::Rot2::unit
Point2 unit() const
Creates a unit vector as a Point2.
Definition: Rot2.h:181
gtsam::Rot2::LogmapDerivative
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition: Rot2.h:139
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition: Rot2.h:66
gtsam::Rot2::theta
double theta() const
Definition: Rot2.h:186
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Rot2::Identity
static Rot2 Identity()
Definition: Rot2.h:110
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Rot2::Rot2
Rot2(double c, double s)
Definition: Rot2.h:44
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Point2.h
2D Point
gtsam::Rot2::operator*
Point2 operator*(const Point2 &p) const
Definition: Rot2.h:166
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
gtsam::Rot2::Rot2
Rot2()
Definition: Rot2.h:52
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Rot2::ChartAtOrigin
Definition: Rot2.h:144
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::normalize
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
gtsam::unrotate
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:109
gtsam::rotate
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:97
gtsam::Rot2::s
double s() const
Definition: Rot2.h:202
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:229
degree
const double degree
Definition: SimpleRotation.cpp:59
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Rot2::Rot2
Rot2(const Rot2 &r)
Definition: Rot2.h:55
y
Scalar * y
Definition: level1_cplx_impl.h:124
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:110
gtsam::Rot2
Definition: Rot2.h:35
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot2::Rot2
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition: Rot2.h:58
gtsam::LieGroup
Definition: Lie.h:37
gtsam::tol
const G double tol
Definition: Group.h:79
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::Rot2::c
double c() const
Definition: Rot2.h:197
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::Rot2::ExpmapDerivative
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition: Rot2.h:134
gtsam::Rot2::inverse
Rot2 inverse() const
Definition: Rot2.h:113
gtsam::Rot2::AdjointMap
Matrix1 AdjointMap() const
Definition: Rot2.h:131
gtsam::LieGroup< Rot2, 1 >::inverse
Rot2 inverse(ChartJacobian H) const
Definition: Lie.h:71
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::Rot2::operator*
Rot2 operator*(const Rot2 &R) const
Definition: Rot2.h:116
gtsam::Rot2::degrees
double degrees() const
Definition: Rot2.h:191
gtsam::Rot2::s_
double s_
Definition: Rot2.h:38
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:01