gtsam  4.1.0
gtsam
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 
103 template <class Archive>
104 void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
105  Matrix3& M = R.matrix_;
106  ar& boost::serialization::make_nvp("R11", M(0, 0));
107  ar& boost::serialization::make_nvp("R12", M(0, 1));
108  ar& boost::serialization::make_nvp("R13", M(0, 2));
109  ar& boost::serialization::make_nvp("R21", M(1, 0));
110  ar& boost::serialization::make_nvp("R22", M(1, 1));
111  ar& boost::serialization::make_nvp("R23", M(1, 2));
112  ar& boost::serialization::make_nvp("R31", M(2, 0));
113  ar& boost::serialization::make_nvp("R32", M(2, 1));
114  ar& boost::serialization::make_nvp("R33", M(2, 2));
115 }
116 
117 namespace so3 {
118 
123 GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
124  OptionalJacobian<9, 9> H = boost::none);
125 
127 GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
128 
129 // Below are two functors that allow for saving computation when exponential map
130 // and its derivatives are needed at the same location in so<3>. The second
131 // functor also implements dedicated methods to apply dexp and/or inv(dexp).
132 
134 class GTSAM_EXPORT ExpmapFunctor {
135  protected:
136  const double theta2;
137  Matrix3 W, K, KK;
138  bool nearZero;
139  double theta, sin_theta, one_minus_cos; // only defined if !nearZero
140 
141  void init(bool nearZeroApprox = false);
142 
143  public:
145  explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
146 
148  ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
149 
151  SO3 expmap() const;
152 };
153 
155 class DexpFunctor : public ExpmapFunctor {
156  const Vector3 omega;
157  double a, b;
158  Matrix3 dexp_;
159 
160  public:
162  GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
163 
164  // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
165  // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
166  // Information Theory, and Lie Groups", Volume 2, 2008.
167  // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
168  // This maps a perturbation v in the tangent space to
169  // a perturbation on the manifold Expmap(dexp * v) */
170  const Matrix3& dexp() const { return dexp_; }
171 
173  GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
174  OptionalJacobian<3, 3> H2 = boost::none) const;
175 
177  GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
178  OptionalJacobian<3, 3> H1 = boost::none,
179  OptionalJacobian<3, 3> H2 = boost::none) const;
180 };
181 } // namespace so3
182 
183 /*
184  * Define the traits. internal::LieGroup provides both Lie group and Testable
185  */
186 
187 template <>
188 struct traits<SO3> : public internal::LieGroup<SO3> {};
189 
190 template <>
191 struct traits<const SO3> : public internal::LieGroup<SO3> {};
192 
193 } // end namespace gtsam
SOn.h
N*N matrix representation of SO(N).
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:84
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition: SOn.h:50
gtsam::SO< 3 >::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F), currently only defined for ...
gtsam::SO< 3 >::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition: SOn-inl.h:69
gtsam::SO< 3 >::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition: SOn-inl.h:79
gtsam::so3::DexpFunctor
Functor that implements Exponential map and its derivatives.
Definition: SO3.h:155
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::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::so3::DexpFunctor::DexpFunctor
GTSAM_EXPORT DexpFunctor(const Vector3 &omega, bool nearZeroApprox=false)
Constructor with element of Lie algebra so(3)
Definition: SO3.cpp:90
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::SO< 3 >::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition: SOn-inl.h:31
gtsam::SO< 3 >::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
gtsam::SO< 3 >::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition: SOn-inl.h:90
gtsam::so3::ExpmapFunctor
Functor implementing Exponential map.
Definition: SO3.h:134
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:74
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::SO< 3 >::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:37
gtsam::so3::DexpFunctor::applyInvDexp
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp().inverse(), with optional derivatives.
Definition: SO3.cpp:120
Lie.h
Base class and basic functions for Lie types.
gtsam::so3::DexpFunctor::applyDexp
GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp(), with optional derivatives.
Definition: SO3.cpp:101