gtsam 4.2.0
gtsam
SOn.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
19#pragma once
20
21#include <gtsam/base/Lie.h>
22#include <gtsam/base/Manifold.h>
24#include <gtsam/dllexport.h>
25#include <Eigen/Core>
26
27#include <boost/serialization/nvp.hpp>
28
29#include <iostream> // TODO(frank): how to avoid?
30#include <string>
31#include <type_traits>
32#include <vector>
33#include <random>
34
35namespace gtsam {
36
37namespace internal {
39constexpr int DimensionSO(int N) {
40 return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
41}
42
43// Calculate N^2 at compile time, or return Dynamic if so
44constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
45} // namespace internal
46
51template <int N>
52class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
53 public:
54 enum { dimension = internal::DimensionSO(N) };
55 using MatrixNN = Eigen::Matrix<double, N, N>;
56 using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
57 using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
58
60
61 protected:
62 MatrixNN matrix_;
63
64 // enable_if_t aliases, used to specialize constructors/methods, see
65 // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
66 template <int N_>
67 using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
68 template <int N_>
69 using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
70 template <int N_>
71 using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
72
73 public:
76
78 template <int N_ = N, typename = IsFixed<N_>>
79 SO() : matrix_(MatrixNN::Identity()) {}
80
82 template <int N_ = N, typename = IsDynamic<N_>>
83 explicit SO(size_t n = 0) {
84 // We allow for n=0 as the default constructor, needed for serialization,
85 // wrappers etc.
86 matrix_ = Eigen::MatrixXd::Identity(n, n);
87 }
88
90 template <typename Derived>
91 explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
92
94 template <typename Derived>
95 static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
96 return SO(R);
97 }
98
100 template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
101 static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
102 Matrix Q = Matrix::Identity(n, n);
103 const int p = R.rows();
104 assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
105 Q.topLeftCorner(p, p) = R;
106 return SO(Q);
107 }
108
110 template <int M, int N_ = N, typename = IsDynamic<N_>>
111 explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
112
114 template <int N_ = N, typename = IsSO3<N_>>
115 explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
116
118 static SO AxisAngle(const Vector3& axis, double theta);
119
122 static SO ClosestTo(const MatrixNN& M);
123
127 static SO ChordalMean(const std::vector<SO>& rotations);
128
130 template <int N_ = N, typename = IsDynamic<N_>>
131 static SO Random(std::mt19937& rng, size_t n = 0) {
132 if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
133 // TODO(frank): this might need to be re-thought
134 static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
135 const size_t d = SO::Dimension(n);
136 Vector xi(d);
137 for (size_t j = 0; j < d; j++) {
138 xi(j) = randomAngle(rng);
139 }
140 return SO::Retract(xi);
141 }
142
144 template <int N_ = N, typename = IsFixed<N_>>
145 static SO Random(std::mt19937& rng) {
146 // By default, use dynamic implementation above. Specialized for SO(3).
147 return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
148 }
149
153
155 const MatrixNN& matrix() const { return matrix_; }
156
157 size_t rows() const { return matrix_.rows(); }
158 size_t cols() const { return matrix_.cols(); }
159
163
164 void print(const std::string& s = std::string()) const;
165
166 bool equals(const SO& other, double tol) const {
167 return equal_with_abs_tol(matrix_, other.matrix_, tol);
168 }
169
173
175 SO operator*(const SO& other) const {
176 assert(dim() == other.dim());
177 return SO(matrix_ * other.matrix_);
178 }
179
181 template <int N_ = N, typename = IsFixed<N_>>
182 static SO Identity() {
183 return SO();
184 }
185
187 template <int N_ = N, typename = IsDynamic<N_>>
188 static SO Identity(size_t n = 0) {
189 return SO(n);
190 }
191
193 SO inverse() const { return SO(matrix_.transpose()); }
194
198
199 using TangentVector = Eigen::Matrix<double, dimension, 1>;
200 using ChartJacobian = OptionalJacobian<dimension, dimension>;
201
203 static int Dim() { return dimension; }
204
205 // Calculate manifold dimensionality for SO(n).
206 // Available as dimension or Dim() for fixed N.
207 static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
208
209 // Calculate ambient dimension n from manifold dimensionality d.
210 static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
211
212 // Calculate run-time dimensionality of manifold.
213 // Available as dimension or Dim() for fixed N.
214 size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
215
231 static MatrixNN Hat(const TangentVector& xi);
232
234 static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
235
237 static TangentVector Vee(const MatrixNN& X);
238
239 // Chart at origin
245 static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
246
250 static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
251 };
252
253 // Return dynamic identity DxD Jacobian for given SO(n)
254 template <int N_ = N, typename = IsDynamic<N_>>
255 static MatrixDD IdentityJacobian(size_t n) {
256 const size_t d = Dimension(n);
257 return MatrixDD::Identity(d, d);
258 }
259
263
265 MatrixDD AdjointMap() const;
266
270 static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
271
273 static MatrixDD ExpmapDerivative(const TangentVector& omega);
274
278 static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
279
281 static MatrixDD LogmapDerivative(const TangentVector& omega);
282
283 // inverse with optional derivative
284 using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
285
289
295 VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
296 boost::none) const;
297
299 template <int N_ = N, typename = IsFixed<N_>>
300 static Matrix VectorizedGenerators() {
301 constexpr size_t N2 = static_cast<size_t>(N * N);
302 Eigen::Matrix<double, N2, dimension> G;
303 for (size_t j = 0; j < dimension; j++) {
304 const auto X = Hat(Vector::Unit(dimension, j));
305 G.col(j) = Eigen::Map<const VectorN2>(X.data());
306 }
307 return G;
308 }
309
311 template <int N_ = N, typename = IsDynamic<N_>>
312 static Matrix VectorizedGenerators(size_t n = 0) {
313 const size_t n2 = n * n, dim = Dimension(n);
314 Matrix G(n2, dim);
315 for (size_t j = 0; j < dim; j++) {
316 const auto X = Hat(Vector::Unit(dim, j));
317 G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
318 }
319 return G;
320 }
321
325
326 template <class Archive>
327 friend void save(Archive&, SO&, const unsigned int);
328 template <class Archive>
329 friend void load(Archive&, SO&, const unsigned int);
330 template <class Archive>
331 friend void serialize(Archive&, SO&, const unsigned int);
332 friend class boost::serialization::access;
333 friend class Rot3; // for serialize
334
336};
337
338using SOn = SO<Eigen::Dynamic>;
339
340/*
341 * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
342 * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
343 * and implementation for other fixed N is in SOn-inl.h.
344 */
345
346template <>
347GTSAM_EXPORT
348Matrix SOn::Hat(const Vector& xi);
349
350template <>
351GTSAM_EXPORT
352Vector SOn::Vee(const Matrix& X);
353
354/*
355 * Specialize dynamic compose and between, because the derivative is unknowable
356 * by the LieGroup implementations, who return a fixed-size matrix for H2.
357 */
358
359using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
360
361template <>
362GTSAM_EXPORT
363SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
364 DynamicJacobian H2) const;
365
366template <>
367GTSAM_EXPORT
368SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
369 DynamicJacobian H2) const;
370
371/*
372 * Specialize dynamic vec.
373 */
374template <>
375GTSAM_EXPORT
376typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
377
379template<class Archive>
381 Archive& ar, SOn& Q,
382 const unsigned int file_version
383) {
384 Matrix& M = Q.matrix_;
385 ar& BOOST_SERIALIZATION_NVP(M);
386}
387
388/*
389 * Define the traits. internal::LieGroup provides both Lie group and Testable
390 */
391
392template <int N>
393struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
394
395template <int N>
396struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
397
398} // namespace gtsam
399
400#include "SOn-inl.h"
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition: types.h:317
make_shared trampoline function to ensure proper alignment
Base class and basic functions for Manifold types.
Base class and basic functions for Lie types.
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition: SOn.h:39
Template implementations for 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
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:81
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:37
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
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Manifold of special orthogonal rotation matrices SO<N>.
Definition: SOn.h:52
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition: SOn.h:95
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
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition: SOn.h:300
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:193
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition: SOn-inl.h:88
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
SO operator*(const SO &other) const
Multiplication.
Definition: SOn.h:175
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:35
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:62
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:77
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
In-place version of Hat (see details there), implements recursion.
static SO Identity()
SO<N> identity for N >= 2.
Definition: SOn.h:182
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition: SOn.h:203
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
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:82
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition: SOn.h:111
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:83
SO()
Construct SO<N> identity for N >= 2.
Definition: SOn.h:79
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:101
static SO Random(std::mt19937 &rng, size_t n=0)
Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3....
Definition: SOn.h:131
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:188
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:155
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition: SOn.h:145
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition: SOn.h:115
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
Definition: SOn.h:312
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition: SOn.h:91
Definition: SOn.h:240
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition: SOn-inl.h:50
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition: SOn-inl.h:40