SO3.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 
21 #pragma once
22 
23 #include <gtsam/geometry/SOn.h>
24 
25 #include <gtsam/base/Lie.h>
26 #include <gtsam/base/Matrix.h>
27 #include <gtsam/dllexport.h>
28 
29 #include <cmath>
30 #include <vector>
31 
32 namespace gtsam {
33 
34 using SO3 = SO<3>;
35 
36 // Below are all declarations of SO<3> specializations.
37 // They are *defined* in SO3.cpp.
38 
39 template <>
40 GTSAM_EXPORT
41 SO3 SO3::AxisAngle(const Vector3& axis, double theta);
42 
43 template <>
44 GTSAM_EXPORT
45 SO3 SO3::ClosestTo(const Matrix3& M);
46 
47 template <>
48 GTSAM_EXPORT
49 SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
50 
51 template <>
52 GTSAM_EXPORT
53 Matrix3 SO3::Hat(const Vector3& xi);
54 
55 template <>
56 GTSAM_EXPORT
57 Vector3 SO3::Vee(const Matrix3& X);
58 
60 template <>
61 Matrix3 SO3::AdjointMap() const;
62 
67 template <>
68 GTSAM_EXPORT
69 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
70 
72 template <>
73 GTSAM_EXPORT
74 Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
75 
80 template <>
81 GTSAM_EXPORT
82 Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
83 
85 template <>
86 GTSAM_EXPORT
87 Matrix3 SO3::LogmapDerivative(const Vector3& omega);
88 
89 // Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
90 template <>
91 GTSAM_EXPORT
92 SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
93 
94 template <>
95 GTSAM_EXPORT
96 Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
97 
98 template <>
99 GTSAM_EXPORT
100 Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
101 
102 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
103 template <class Archive>
105 void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
106  Matrix3& M = R.matrix_;
107  ar& boost::serialization::make_nvp("R11", M(0, 0));
108  ar& boost::serialization::make_nvp("R12", M(0, 1));
109  ar& boost::serialization::make_nvp("R13", M(0, 2));
110  ar& boost::serialization::make_nvp("R21", M(1, 0));
111  ar& boost::serialization::make_nvp("R22", M(1, 1));
112  ar& boost::serialization::make_nvp("R23", M(1, 2));
113  ar& boost::serialization::make_nvp("R31", M(2, 0));
114  ar& boost::serialization::make_nvp("R32", M(2, 1));
115  ar& boost::serialization::make_nvp("R33", M(2, 2));
116 }
117 #endif
118 
119 namespace so3 {
120 
125 GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
127 
129 GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
130 
131 // Below are two functors that allow for saving computation when exponential map
132 // and its derivatives are needed at the same location in so<3>. The second
133 // functor also implements dedicated methods to apply dexp and/or inv(dexp).
134 
136 class GTSAM_EXPORT ExpmapFunctor {
137  protected:
138  const double theta2;
139  Matrix3 W, K, KK;
140  bool nearZero;
141  double theta, sin_theta, one_minus_cos; // only defined if !nearZero
142 
143  void init(bool nearZeroApprox = false);
144 
145  public:
147  explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
148 
150  ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
151 
153  SO3 expmap() const;
154 };
155 
157 class DexpFunctor : public ExpmapFunctor {
158  const Vector3 omega;
159  double a, b;
160  Matrix3 dexp_;
161 
162  public:
164  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
165 
166  // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
167  // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
168  // Information Theory, and Lie Groups", Volume 2, 2008.
169  // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
170  // This maps a perturbation v in the tangent space to
171  // a perturbation on the manifold Expmap(dexp * v) */
172  const Matrix3& dexp() const { return dexp_; }
173 
175  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
176  OptionalJacobian<3, 3> H2 = {}) const;
177 
179  GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
180  OptionalJacobian<3, 3> H1 = {},
181  OptionalJacobian<3, 3> H2 = {}) const;
182 };
183 } // namespace so3
184 
185 /*
186  * Define the traits. internal::LieGroup provides both Lie group and Testable
187  */
188 
189 template <>
190 struct traits<SO3> : public internal::LieGroup<SO3> {};
191 
192 template <>
193 struct traits<const SO3> : public internal::LieGroup<SO3> {};
194 
195 } // end namespace gtsam
static MatrixNN Hat(const TangentVector &xi)
Definition: SOn-inl.h:29
static SO ChordalMean(const std::vector< SO > &rotations)
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
Eigen::Vector3d Vector3
Definition: Vector.h:43
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
std::string serialize(const T &input)
serializes to a string
static SO Expmap(const TangentVector &omega, ChartJacobian H={})
Definition: SOn-inl.h:67
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:82
Rot2 R(Rot2::fromAngle(0.1))
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
const Vector3 omega
Definition: SO3.h:158
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
Functor implementing Exponential map.
Definition: SO3.h:136
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:64
const Matrix3 & dexp() const
Definition: SO3.h:172
GTSAM_EXPORT Matrix99 Dcompose(const SO3 &Q)
(constant) Jacobian of compose wrpt M
Definition: SO3.cpp:35
Array< int, Dynamic, 1 > v
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
const double theta2
Definition: SO3.h:138
Base class and basic functions for Lie types.
Vector xi
Definition: testPose2.cpp:148
traits
Definition: chartTesting.h:28
static TangentVector Logmap(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:77
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
Both LieGroupTraits and Testable.
Definition: Lie.h:229
Functor that implements Exponential map and its derivatives.
Definition: SO3.h:157
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
#define X
Definition: icosphere.cpp:20
static SO ClosestTo(const MatrixNN &M)
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:35
N*N matrix representation of SO(N). N can be Eigen::Dynamic.
GTSAM_EXPORT Matrix3 compose(const Matrix3 &M, const SO3 &R, OptionalJacobian< 9, 9 > H)
Definition: SO3.cpp:44
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Definition: SOn-inl.h:88
#define KK


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