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 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
28 #include <boost/serialization/nvp.hpp>
29 #endif
30 
31 #include <iostream> // TODO(frank): how to avoid?
32 #include <string>
33 #include <type_traits>
34 #include <vector>
35 #include <random>
36 
37 namespace gtsam {
38 
39 namespace internal {
41 constexpr int DimensionSO(int N) {
42  return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
43 }
44 
45 // Calculate N^2 at compile time, or return Dynamic if so
46 constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
47 } // namespace internal
48 
53 template <int N>
54 class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
55  public:
56  enum { dimension = internal::DimensionSO(N) };
60 
62 
63  protected:
64  MatrixNN matrix_;
65 
66  // enable_if_t aliases, used to specialize constructors/methods, see
67  // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
68  template <int N_>
69  using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
70  template <int N_>
71  using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
72  template <int N_>
73  using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
74 
75  public:
78 
80  template <int N_ = N, typename = IsFixed<N_>>
81  SO() : matrix_(MatrixNN::Identity()) {}
82 
84  template <int N_ = N, typename = IsDynamic<N_>>
85  explicit SO(size_t n = 0) {
86  // We allow for n=0 as the default constructor, needed for serialization,
87  // wrappers etc.
88  matrix_ = Eigen::MatrixXd::Identity(n, n);
89  }
90 
92  template <typename Derived>
93  explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
94 
96  template <typename Derived>
98  return SO(R);
99  }
100 
102  template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
103  static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
104  Matrix Q = Matrix::Identity(n, n);
105  const int p = R.rows();
106  assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
107  Q.topLeftCorner(p, p) = R;
108  return SO(Q);
109  }
110 
112  template <int M, int N_ = N, typename = IsDynamic<N_>>
113  explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
114 
116  template <int N_ = N, typename = IsSO3<N_>>
117  explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
118 
120  static SO AxisAngle(const Vector3& axis, double theta);
121 
124  static SO ClosestTo(const MatrixNN& M);
125 
129  static SO ChordalMean(const std::vector<SO>& rotations);
130 
132  template <int N_ = N, typename = IsDynamic<N_>>
133  static SO Random(std::mt19937& rng, size_t n = 0) {
134  if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
135  // TODO(frank): this might need to be re-thought
136  static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
137  const size_t d = SO::Dimension(n);
138  Vector xi(d);
139  for (size_t j = 0; j < d; j++) {
140  xi(j) = randomAngle(rng);
141  }
142  return SO::Retract(xi);
143  }
144 
146  template <int N_ = N, typename = IsFixed<N_>>
147  static SO Random(std::mt19937& rng) {
148  // By default, use dynamic implementation above. Specialized for SO(3).
149  return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
150  }
151 
155 
157  const MatrixNN& matrix() const { return matrix_; }
158 
159  size_t rows() const { return matrix_.rows(); }
160  size_t cols() const { return matrix_.cols(); }
161 
165 
166  void print(const std::string& s = std::string()) const;
167 
168  bool equals(const SO& other, double tol) const {
169  return equal_with_abs_tol(matrix_, other.matrix_, tol);
170  }
171 
175 
177  SO operator*(const SO& other) const {
178  assert(dim() == other.dim());
179  return SO(matrix_ * other.matrix_);
180  }
181 
183  template <int N_ = N, typename = IsFixed<N_>>
184  static SO Identity() {
185  return SO();
186  }
187 
189  template <int N_ = N, typename = IsDynamic<N_>>
190  static SO Identity(size_t n = 0) {
191  return SO(n);
192  }
193 
195  SO inverse() const { return SO(matrix_.transpose()); }
196 
200 
203 
205  static int Dim() { return dimension; }
206 
207  // Calculate manifold dimensionality for SO(n).
208  // Available as dimension or Dim() for fixed N.
209  static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
210 
211  // Calculate ambient dimension n from manifold dimensionality d.
212  static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
213 
214  // Calculate run-time dimensionality of manifold.
215  // Available as dimension or Dim() for fixed N.
216  size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
217 
233  static MatrixNN Hat(const TangentVector& xi);
234 
236  static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
237 
239  static TangentVector Vee(const MatrixNN& X);
240 
241  // Chart at origin
242  struct ChartAtOrigin {
247  static SO Retract(const TangentVector& xi, ChartJacobian H = {});
248 
252  static TangentVector Local(const SO& R, ChartJacobian H = {});
253  };
254 
255  // Return dynamic identity DxD Jacobian for given SO(n)
256  template <int N_ = N, typename = IsDynamic<N_>>
257  static MatrixDD IdentityJacobian(size_t n) {
258  const size_t d = Dimension(n);
259  return MatrixDD::Identity(d, d);
260  }
261 
265 
267  MatrixDD AdjointMap() const;
268 
272  static SO Expmap(const TangentVector& omega, ChartJacobian H = {});
273 
275  static MatrixDD ExpmapDerivative(const TangentVector& omega);
276 
280  static TangentVector Logmap(const SO& R, ChartJacobian H = {});
281 
283  static MatrixDD LogmapDerivative(const TangentVector& omega);
284 
285  // inverse with optional derivative
287 
291 
298  {}) const;
299 
301  template <int N_ = N, typename = IsFixed<N_>>
303  constexpr size_t N2 = static_cast<size_t>(N * N);
305  for (size_t j = 0; j < dimension; j++) {
306  const auto X = Hat(Vector::Unit(dimension, j));
307  G.col(j) = Eigen::Map<const VectorN2>(X.data());
308  }
309  return G;
310  }
311 
313  template <int N_ = N, typename = IsDynamic<N_>>
314  static Matrix VectorizedGenerators(size_t n = 0) {
315  const size_t n2 = n * n, dim = Dimension(n);
316  Matrix G(n2, dim);
317  for (size_t j = 0; j < dim; j++) {
318  const auto X = Hat(Vector::Unit(dim, j));
319  G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
320  }
321  return G;
322  }
323 
327 
328 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
329  template <class Archive>
330  friend void save(Archive&, SO&, const unsigned int);
331  template <class Archive>
332  friend void load(Archive&, SO&, const unsigned int);
333  template <class Archive>
334  friend void serialize(Archive&, SO&, const unsigned int);
335  friend class boost::serialization::access;
336  friend class Rot3; // for serialize
337 #endif
338 
340 };
341 
343 
344 /*
345  * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
346  * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
347  * and implementation for other fixed N is in SOn-inl.h.
348  */
349 
350 template <>
351 GTSAM_EXPORT
352 Matrix SOn::Hat(const Vector& xi);
353 
354 template <>
355 GTSAM_EXPORT
356 Vector SOn::Vee(const Matrix& X);
357 
358 /*
359  * Specialize dynamic compose and between, because the derivative is unknowable
360  * by the LieGroup implementations, who return a fixed-size matrix for H2.
361  */
362 
364 
365 template <>
366 GTSAM_EXPORT
368  DynamicJacobian H2) const;
369 
370 template <>
371 GTSAM_EXPORT
373  DynamicJacobian H2) const;
374 
375 /*
376  * Specialize dynamic vec.
377  */
378 template <>
379 GTSAM_EXPORT
380 typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
381 
382 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
383 
384 template<class Archive>
385 void serialize(
386  Archive& ar, SOn& Q,
387  const unsigned int file_version
388 ) {
389  Matrix& M = Q.matrix_;
390  ar& BOOST_SERIALIZATION_NVP(M);
391 }
392 #endif
393 
394 /*
395  * Define the traits. internal::LieGroup provides both Lie group and Testable
396  */
397 
398 template <int N>
399 struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
400 
401 template <int N>
402 struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
403 
404 } // namespace gtsam
405 
406 #include "SOn-inl.h"
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static MatrixNN Hat(const TangentVector &xi)
Definition: SOn-inl.h:29
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition: SOn.h:205
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
void save(const Matrix &A, const string &s, const string &filename)
Definition: Matrix.cpp:166
Eigen::Vector3d Vector3
Definition: Vector.h:43
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:195
make_shared trampoline function to ensure proper alignment
std::string serialize(const T &input)
serializes to a string
JacobiRotation< float > G
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Template implementations for SO(n)
typename std::enable_if< N_==3, void >::type IsSO3
Definition: SOn.h:73
static SO Identity()
SO<N> identity for N >= 2.
Definition: SOn.h:184
static std::mt19937 rng
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
Rot2 R(Rot2::fromAngle(0.1))
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition: SOn.h:302
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Pose2_ Expmap(const Vector3_ &xi)
Definition: BFloat16.h:88
static size_t Dimension(size_t n)
Definition: SOn.h:209
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
#define N
Definition: gksort.c:12
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition: SOn.h:113
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition: SOn.h:41
EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedBlockXpr< internal::get_fixed_value< NRowsType >::value, internal::get_fixed_value< NColsType >::value >::Type topLeftCorner(NRowsType cRows, NColsType cCols)
Definition: DenseBase.h:250
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Definition: SOn.h:54
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
Definition: SOn.h:314
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
void g(const string &key, int i)
Definition: testBTree.cpp:41
constexpr int NSquaredSO(int N)
Definition: SOn.h:46
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
Definition: types.h:293
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:64
static size_t AmbientDim(size_t d)
Definition: SOn.h:212
Eigen::VectorXd Vector
Definition: Vector.h:38
Base class and basic functions for Manifold types.
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
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.cpp.
Definition: SOn.h:133
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:190
RealScalar s
typename std::enable_if< N_ >=2, void >::type IsFixed
Definition: SOn.h:71
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition: SOn.h:93
Class compose(const Class &g) const
Definition: Lie.h:48
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition: SOn.h:117
size_t dim() const
Definition: SOn.h:216
Base class and basic functions for Lie types.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
Vector xi
Definition: testPose2.cpp:148
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition: SOn.h:97
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
traits
Definition: chartTesting.h:28
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:103
SO operator*(const SO &other) const
Multiplication.
Definition: SOn.h:177
Both LieGroupTraits and Testable.
Definition: Lie.h:229
The quaternion class used to represent 3D orientations and rotations.
Class between(const Class &g) const
Definition: Lie.h:52
size_t cols() const
Definition: SOn.h:160
float * p
size_t rows() const
Definition: SOn.h:159
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition: Lie.h:111
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition: SOn.h:147
internal::nested_eval< T, 1 >::type eval(const T &xpr)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
static MatrixDD IdentityJacobian(size_t n)
Definition: SOn.h:257
const G double tol
Definition: Group.h:86
const int Dynamic
Definition: Constants.h:22
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:85
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
#define X
Definition: icosphere.cpp:20
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:35
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
typename std::enable_if< N_==Eigen::Dynamic, void >::type IsDynamic
Definition: SOn.h:69
std::ptrdiff_t j
bool equals(const SO &other, double tol) const
Definition: SOn.h:168
Definition: pytypes.h:1370
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Definition: SOn-inl.h:88


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:53