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) = default;
56 
57  Rot2& operator=(const Rot2& other) = default;
58 
60  Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
61 
62  // Rot2& operator=(const gtsam::Rot2& other) = default;
63 
65  static Rot2 fromAngle(double theta) {
66  return Rot2(theta);
67  }
68 
70  static Rot2 fromDegrees(double theta) {
71  static const double degree = M_PI / 180;
72  return fromAngle(theta * degree);
73  }
74 
76  static Rot2 fromCosSin(double c, double s);
77 
85  static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
86  {});
87 
89  static Rot2 atan2(double y, double x);
90 
97  static Rot2 Random(std::mt19937 & rng);
98 
102 
104  void print(const std::string& s = "theta") const;
105 
107  bool equals(const Rot2& R, double tol = 1e-9) const;
108 
112 
114  inline static Rot2 Identity() { return Rot2(); }
115 
117  Rot2 inverse() const { return Rot2(c_, -s_);}
118 
120  Rot2 operator*(const Rot2& R) const {
121  return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
122  }
123 
127 
129  static Rot2 Expmap(const Vector1& v, ChartJacobian H = {});
130 
132  static Vector1 Logmap(const Rot2& r, ChartJacobian H = {});
133 
135  Matrix1 AdjointMap() const { return I_1x1; }
136 
138  static Matrix ExpmapDerivative(const Vector& /*v*/) {
139  return I_1x1;
140  }
141 
143  static Matrix LogmapDerivative(const Vector& /*v*/) {
144  return I_1x1;
145  }
146 
147  // Chart at origin simply uses exponential map and its inverse
148  struct ChartAtOrigin {
149  static Rot2 Retract(const Vector1& v, ChartJacobian H = {}) {
150  return Expmap(v, H);
151  }
152  static Vector1 Local(const Rot2& r, ChartJacobian H = {}) {
153  return Logmap(r, H);
154  }
155  };
156 
157  using LieGroup<Rot2, 1>::inverse; // version with derivative
158 
162 
166  Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = {},
167  OptionalJacobian<2, 2> H2 = {}) const;
168 
170  inline Point2 operator*(const Point2& p) const {
171  return rotate(p);
172  }
173 
178  OptionalJacobian<2, 2> H2 = {}) const;
179 
183 
185  inline Point2 unit() const {
186  return Point2(c_, s_);
187  }
188 
190  double theta() const {
191  return ::atan2(s_, c_);
192  }
193 
195  double degrees() const {
196  const double degree = M_PI / 180;
197  return theta() / degree;
198  }
199 
201  inline double c() const {
202  return c_;
203  }
204 
206  inline double s() const {
207  return s_;
208  }
209 
211  Matrix2 matrix() const;
212 
214  Matrix2 transpose() const;
215 
217  static Rot2 ClosestTo(const Matrix2& M);
218 
219  private:
220 #if GTSAM_ENABLE_BOOST_SERIALIZATION
221 
222  friend class boost::serialization::access;
223  template<class ARCHIVE>
224  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
225  ar & BOOST_SERIALIZATION_NVP(c_);
226  ar & BOOST_SERIALIZATION_NVP(s_);
227  }
228 #endif
229 
230  };
231 
232  template<>
233  struct traits<Rot2> : public internal::LieGroup<Rot2> {};
234 
235  template<>
236  struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
237 
238 } // 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:149
gtsam::Rot2::ChartAtOrigin::Local
static Vector1 Local(const Rot2 &r, ChartJacobian H={})
Definition: Rot2.h:152
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:185
gtsam::Rot2::LogmapDerivative
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition: Rot2.h:143
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:70
gtsam::Rot2::theta
double theta() const
Definition: Rot2.h:190
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Rot2::Identity
static Rot2 Identity()
Definition: Rot2.h:114
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
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:170
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:65
gtsam::Rot2::Rot2
Rot2()
Definition: Rot2.h:52
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::Rot2::ChartAtOrigin
Definition: Rot2.h:148
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:145
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:206
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
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:60
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:201
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:138
gtsam::Rot2::inverse
Rot2 inverse() const
Definition: Rot2.h:117
gtsam::Rot2::AdjointMap
Matrix1 AdjointMap() const
Definition: Rot2.h:135
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
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:120
gtsam::Rot2::degrees
double degrees() const
Definition: Rot2.h:195
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 Sun Feb 16 2025 04:03:01