gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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"
Base class and basic functions for Lie types.
Base class and basic functions for Manifold types.
make_shared trampoline function to ensure proper alignment
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
Template implementations for SO(n)
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition SOn.h:39
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