gtsam  4.1.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>
23 #include <gtsam/base/make_shared.h>
24 #include <gtsam/dllexport.h>
25 #include <Eigen/Core>
26 
27 #include <iostream> // TODO(frank): how to avoid?
28 #include <string>
29 #include <type_traits>
30 #include <vector>
31 #include <random>
32 
33 namespace gtsam {
34 
35 namespace internal {
37 constexpr int DimensionSO(int N) {
38  return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
39 }
40 
41 // Calculate N^2 at compile time, or return Dynamic if so
42 constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
43 } // namespace internal
44 
49 template <int N>
50 class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
51  public:
52  enum { dimension = internal::DimensionSO(N) };
53  using MatrixNN = Eigen::Matrix<double, N, N>;
54  using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
55  using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
56 
58 
59  protected:
60  MatrixNN matrix_;
61 
62  // enable_if_t aliases, used to specialize constructors/methods, see
63  // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
64  template <int N_>
65  using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
66  template <int N_>
67  using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
68  template <int N_>
69  using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
70 
71  public:
74 
76  template <int N_ = N, typename = IsFixed<N_>>
77  SO() : matrix_(MatrixNN::Identity()) {}
78 
80  template <int N_ = N, typename = IsDynamic<N_>>
81  explicit SO(size_t n = 0) {
82  // We allow for n=0 as the default constructor, needed for serialization,
83  // wrappers etc.
84  matrix_ = Eigen::MatrixXd::Identity(n, n);
85  }
86 
88  template <typename Derived>
89  explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
90 
92  template <typename Derived>
93  static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
94  return SO(R);
95  }
96 
98  template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
99  static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
100  Matrix Q = Matrix::Identity(n, n);
101  const int p = R.rows();
102  assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
103  Q.topLeftCorner(p, p) = R;
104  return SO(Q);
105  }
106 
108  template <int M, int N_ = N, typename = IsDynamic<N_>>
109  explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
110 
112  template <int N_ = N, typename = IsSO3<N_>>
113  explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
114 
116  static SO AxisAngle(const Vector3& axis, double theta);
117 
120  static SO ClosestTo(const MatrixNN& M);
121 
124  static SO ChordalMean(const std::vector<SO>& rotations);
125 
127  template <int N_ = N, typename = IsDynamic<N_>>
128  static SO Random(std::mt19937& rng, size_t n = 0) {
129  if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
130  // TODO(frank): this might need to be re-thought
131  static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
132  const size_t d = SO::Dimension(n);
133  Vector xi(d);
134  for (size_t j = 0; j < d; j++) {
135  xi(j) = randomAngle(rng);
136  }
137  return SO::Retract(xi);
138  }
139 
141  template <int N_ = N, typename = IsFixed<N_>>
142  static SO Random(std::mt19937& rng) {
143  // By default, use dynamic implementation above. Specialized for SO(3).
144  return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
145  }
146 
150 
152  const MatrixNN& matrix() const { return matrix_; }
153 
154  size_t rows() const { return matrix_.rows(); }
155  size_t cols() const { return matrix_.cols(); }
156 
160 
161  void print(const std::string& s = std::string()) const;
162 
163  bool equals(const SO& other, double tol) const {
164  return equal_with_abs_tol(matrix_, other.matrix_, tol);
165  }
166 
170 
172  SO operator*(const SO& other) const {
173  assert(dim() == other.dim());
174  return SO(matrix_ * other.matrix_);
175  }
176 
178  template <int N_ = N, typename = IsFixed<N_>>
179  static SO identity() {
180  return SO();
181  }
182 
184  template <int N_ = N, typename = IsDynamic<N_>>
185  static SO identity(size_t n = 0) {
186  return SO(n);
187  }
188 
190  SO inverse() const { return SO(matrix_.transpose()); }
191 
195 
196  using TangentVector = Eigen::Matrix<double, dimension, 1>;
197  using ChartJacobian = OptionalJacobian<dimension, dimension>;
198 
200  static int Dim() { return dimension; }
201 
202  // Calculate manifold dimensionality for SO(n).
203  // Available as dimension or Dim() for fixed N.
204  static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
205 
206  // Calculate ambient dimension n from manifold dimensionality d.
207  static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
208 
209  // Calculate run-time dimensionality of manifold.
210  // Available as dimension or Dim() for fixed N.
211  size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
212 
228  static MatrixNN Hat(const TangentVector& xi);
229 
231  static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
232 
234  static TangentVector Vee(const MatrixNN& X);
235 
236  // Chart at origin
237  struct ChartAtOrigin {
242  static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
243 
247  static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
248  };
249 
250  // Return dynamic identity DxD Jacobian for given SO(n)
251  template <int N_ = N, typename = IsDynamic<N_>>
252  static MatrixDD IdentityJacobian(size_t n) {
253  const size_t d = Dimension(n);
254  return MatrixDD::Identity(d, d);
255  }
256 
260 
262  MatrixDD AdjointMap() const;
263 
267  static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
268 
270  static MatrixDD ExpmapDerivative(const TangentVector& omega);
271 
275  static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
276 
278  static MatrixDD LogmapDerivative(const TangentVector& omega);
279 
280  // inverse with optional derivative
281  using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
282 
286 
292  VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
293  boost::none) const;
294 
296  template <int N_ = N, typename = IsFixed<N_>>
297  static Matrix VectorizedGenerators() {
298  constexpr size_t N2 = static_cast<size_t>(N * N);
299  Eigen::Matrix<double, N2, dimension> G;
300  for (size_t j = 0; j < dimension; j++) {
301  const auto X = Hat(Vector::Unit(dimension, j));
302  G.col(j) = Eigen::Map<const VectorN2>(X.data());
303  }
304  return G;
305  }
306 
308  template <int N_ = N, typename = IsDynamic<N_>>
309  static Matrix VectorizedGenerators(size_t n = 0) {
310  const size_t n2 = n * n, dim = Dimension(n);
311  Matrix G(n2, dim);
312  for (size_t j = 0; j < dim; j++) {
313  const auto X = Hat(Vector::Unit(dim, j));
314  G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
315  }
316  return G;
317  }
318 
322 
323  template <class Archive>
324  friend void save(Archive&, SO&, const unsigned int);
325  template <class Archive>
326  friend void load(Archive&, SO&, const unsigned int);
327  template <class Archive>
328  friend void serialize(Archive&, SO&, const unsigned int);
329  friend class boost::serialization::access;
330  friend class Rot3; // for serialize
331 
333 };
334 
335 using SOn = SO<Eigen::Dynamic>;
336 
337 /*
338  * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
339  * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
340  * and implementation for other fixed N is in SOn-inl.h.
341  */
342 
343 template <>
344 GTSAM_EXPORT
345 Matrix SOn::Hat(const Vector& xi);
346 
347 template <>
348 GTSAM_EXPORT
349 Vector SOn::Vee(const Matrix& X);
350 
351 /*
352  * Specialize dynamic compose and between, because the derivative is unknowable
353  * by the LieGroup implementations, who return a fixed-size matrix for H2.
354  */
355 
356 using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
357 
358 template <>
359 SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
360  DynamicJacobian H2) const;
361 
362 template <>
363 SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
364  DynamicJacobian H2) const;
365 
366 /*
367  * Specialize dynamic vec.
368  */
369 template <> typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
370 
372 template<class Archive>
374  Archive& ar, SOn& Q,
375  const unsigned int file_version
376 ) {
377  Matrix& M = Q.matrix_;
378  ar& BOOST_SERIALIZATION_NVP(M);
379 }
380 
381 /*
382  * Define the traits. internal::LieGroup provides both Lie group and Testable
383  */
384 
385 template <int N>
386 struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
387 
388 template <int N>
389 struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
390 
391 } // namespace gtsam
392 
393 #include "SOn-inl.h"
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition: SOn-inl.h:52
gtsam::SO::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::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
gtsam::SO::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::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::equal_with_abs_tol
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:84
gtsam::SO::Random
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:128
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::SO::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::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
SOn-inl.h
Template implementations for SO(n)
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:110
gtsam::SO::SO
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition: SOn.h:89
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::SO::Random
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition: SOn.h:142
gtsam::SO::SO
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition: SOn.h:109
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::SO::SO
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition: SOn.h:113
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::inverse
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:190
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:152
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
gtsam::SO::operator*
SO operator*(const SO &other) const
Multiplication.
Definition: SOn.h:172
gtsam::SO::ChartAtOrigin
Definition: SOn.h:237
gtsam::SO::identity
static SO identity()
SO<N> identity for N >= 2.
Definition: SOn.h:179
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::SO::Dim
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition: SOn.h:200
make_shared.h
make_shared trampoline function to ensure proper alignment
gtsam::Rot3
Definition: Rot3.h:59
gtsam::SO::SO
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:81
gtsam::internal::DimensionSO
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition: SOn.h:37
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:74
gtsam::SO::identity
static SO identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:185
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition: types.h:278
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition: SOn.h:297
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:60
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.
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition: SOn.h:77
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:99
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
Definition: SOn.h:309
Lie.h
Base class and basic functions for Lie types.
gtsam::SO::Hat
static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
In-place version of Hat (see details there), implements recursion.
gtsam::SO::FromMatrix
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition: SOn.h:93