gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
32namespace gtsam {
33
34using 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
42template <>
43GTSAM_EXPORT
44Matrix4 SO4::Hat(const TangentVector &xi);
45
46template <>
47GTSAM_EXPORT
48Vector6 SO4::Vee(const Matrix4 &X);
49
50template <>
51GTSAM_EXPORT
52SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
53
54template <>
55GTSAM_EXPORT
56Matrix6 SO4::AdjointMap() const;
57
58template <>
59GTSAM_EXPORT
60SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
61
62template <>
63GTSAM_EXPORT
64SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
65
66template <>
67GTSAM_EXPORT
68Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
69
73GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
74
79GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
80
82template <class Archive>
83void 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
110template <>
111struct traits<SO4> : public internal::LieGroup<SO4> {};
112
113template <>
114struct traits<const SO4> : public internal::LieGroup<SO4> {};
115
116} // end namespace gtsam
typedef and functions to augment Eigen's MatrixXd
Base class and basic functions for Manifold types.
Concept check class for variable types with Group properties.
Base class and basic functions for Lie types.
N*N matrix representation of SO(N).
Global functions in a separate testing namespace.
Definition chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
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) -> .
Definition SO4.cpp:220
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
Project to top-left 3*3 matrix.
Definition SO4.cpp:206
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
Both LieGroupTraits and Testable.
Definition Lie.h:229
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
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:29