gtsam  4.1.0
gtsam
SO4.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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/SOn.h>
23 
24 #include <gtsam/base/Group.h>
25 #include <gtsam/base/Lie.h>
26 #include <gtsam/base/Manifold.h>
27 #include <gtsam/base/Matrix.h>
28 #include <gtsam/dllexport.h>
29 
30 #include <string>
31 
32 namespace gtsam {
33 
34 using SO4 = SO<4>;
35 
36 // /// Random SO(4) element (no big claims about uniformity)
37 // static SO4 Random(std::mt19937 &rng);
38 
39 // Below are all declarations of SO<4> specializations.
40 // They are *defined* in SO4.cpp.
41 
42 template <>
43 GTSAM_EXPORT
44 Matrix4 SO4::Hat(const TangentVector &xi);
45 
46 template <>
47 GTSAM_EXPORT
48 Vector6 SO4::Vee(const Matrix4 &X);
49 
50 template <>
51 GTSAM_EXPORT
52 SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
53 
54 template <>
55 GTSAM_EXPORT
56 Matrix6 SO4::AdjointMap() const;
57 
58 template <>
59 GTSAM_EXPORT
60 SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
61 
62 template <>
63 GTSAM_EXPORT
64 SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
65 
66 template <>
67 GTSAM_EXPORT
68 Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
69 
73 GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
74 
79 GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
80 
82 template <class Archive>
83 void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
84  Matrix4 &M = Q.matrix_;
85  ar &boost::serialization::make_nvp("Q11", M(0, 0));
86  ar &boost::serialization::make_nvp("Q12", M(0, 1));
87  ar &boost::serialization::make_nvp("Q13", M(0, 2));
88  ar &boost::serialization::make_nvp("Q14", M(0, 3));
89 
90  ar &boost::serialization::make_nvp("Q21", M(1, 0));
91  ar &boost::serialization::make_nvp("Q22", M(1, 1));
92  ar &boost::serialization::make_nvp("Q23", M(1, 2));
93  ar &boost::serialization::make_nvp("Q24", M(1, 3));
94 
95  ar &boost::serialization::make_nvp("Q31", M(2, 0));
96  ar &boost::serialization::make_nvp("Q32", M(2, 1));
97  ar &boost::serialization::make_nvp("Q33", M(2, 2));
98  ar &boost::serialization::make_nvp("Q34", M(2, 3));
99 
100  ar &boost::serialization::make_nvp("Q41", M(3, 0));
101  ar &boost::serialization::make_nvp("Q42", M(3, 1));
102  ar &boost::serialization::make_nvp("Q43", M(3, 2));
103  ar &boost::serialization::make_nvp("Q44", M(3, 3));
104 }
105 
106 /*
107  * Define the traits. internal::LieGroup provides both Lie group and Testable
108  */
109 
110 template <>
111 struct traits<SO4> : public internal::LieGroup<SO4> {};
112 
113 template <>
114 struct traits<const SO4> : public internal::LieGroup<SO4> {};
115 
116 } // end namespace gtsam
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition: SOn-inl.h:52
SOn.h
N*N matrix representation of SO(N).
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition: SOn.h:50
gtsam::SO::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::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::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::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition: SOn-inl.h:42
gtsam::topLeft
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
Project to top-left 3*3 matrix.
Definition: SO4.cpp:206
gtsam::SO::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::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
gtsam::SO::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::stiefel
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H)
Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> S \in St(3,...
Definition: SO4.cpp:220
Matrix.h
typedef and functions to augment Eigen's MatrixXd
Group.h
Concept check class for variable types with Group properties.
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:37
Manifold.h
Base class and basic functions for Manifold types.
Lie.h
Base class and basic functions for Lie types.