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:
60 
62 
63  protected:
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_>>
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).
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 
276 
280  static TangentVector Logmap(const SO& R, ChartJacobian H = {});
281 
284 
285  // inverse with optional derivative
286  using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
287 
291 
297  VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
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"
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
Definition: OptionalJacobian.h:189
gtsam::LieGroup::between
Class between(const Class &g) const
Definition: Lie.h:52
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::SO::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
gtsam::SO< 3 >::IsDynamic
typename std::enable_if< N_==Eigen::Dynamic, void >::type IsDynamic
Definition: SOn.h:69
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition: SOn.h:195
gtsam::SO::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H={})
Definition: SOn-inl.h:40
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
gtsam::SO::dimension
@ dimension
Definition: SOn.h:56
gtsam::SO::equals
bool equals(const SO &other, double tol) const
Definition: SOn.h:168
gtsam::SO::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
s
RealScalar s
Definition: level1_cplx_impl.h:126
d
static const double d[K][N]
Definition: igam.h:11
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Definition: SOn-inl.h:29
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::SO::MatrixDD
Eigen::Matrix< double, dimension, dimension > MatrixDD
Definition: SOn.h:59
gtsam::SO::Dimension
static size_t Dimension(size_t n)
Definition: SOn.h:209
SOn-inl.h
Template implementations for SO(n)
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:314
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
Eigen::AngleAxis
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:290
gtsam::SO::ChartJacobian
OptionalJacobian< dimension, dimension > ChartJacobian
Definition: SOn.h:202
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
type
Definition: pytypes.h:1525
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::SO::SO
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition: SOn.h:113
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::SO::Identity
static SO Identity()
SO<N> identity for N >= 2.
Definition: SOn.h:184
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H={})
Definition: SOn-inl.h:67
gtsam::SO< 3 >::IsSO3
typename std::enable_if< N_==3, void >::type IsSO3
Definition: SOn.h:73
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::SO::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:82
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition: SOn.h:64
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition: SOn.h:302
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:111
gtsam::internal::DimensionSO
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition: SOn.h:41
benchmark.n2
n2
Definition: benchmark.py:85
gtsam::SO::Identity
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:190
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:229
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::SO::MatrixNN
Eigen::Matrix< double, N, N > MatrixNN
Definition: SOn.h:57
Eigen::Dynamic
const int Dynamic
Definition: Constants.h:22
Eigen::PlainObjectBase::rows
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:143
gtsam::SO::SO
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition: SOn.h:117
gtsam::SO::TangentVector
Eigen::Matrix< double, dimension, 1 > TangentVector
Definition: SOn.h:201
gtsam::SO::AmbientDim
static size_t AmbientDim(size_t d)
Definition: SOn.h:212
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition: SOn.h:103
Manifold.h
Base class and basic functions for Manifold types.
make_shared.h
make_shared trampoline function to ensure proper alignment
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition: SOn.h:81
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
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:133
gtsam::SO::operator*
SO operator*(const SO &other) const
Multiplication.
Definition: SOn.h:177
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
gtsam::SO::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:77
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::internal::NSquaredSO
constexpr int NSquaredSO(int N)
Definition: SOn.h:46
gtsam::SO< 3 >::IsFixed
typename std::enable_if< N_ >=2, void >::type IsFixed
Definition: SOn.h:71
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
Definition: Matrix.cpp:166
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
gtsam::SO::print
void print(const std::string &s=std::string()) const
Definition: SOn-inl.h:108
Lie.h
Base class and basic functions for Lie types.
Eigen::Ref
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
gtsam
traits
Definition: SFMdata.h:40
Eigen::PlainObjectBase::cols
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
Definition: PlainObjectBase.h:145
gtsam::SO::cols
size_t cols() const
Definition: SOn.h:160
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::SO::SO
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition: SOn.h:93
std
Definition: BFloat16.h:88
gtsam::SO::dim
size_t dim() const
Definition: SOn.h:216
p
float * p
Definition: Tutorial_Map_using.cpp:9
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
gtsam::SO::SO
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition: SOn.h:85
gtsam::SO::FromMatrix
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition: SOn.h:97
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:50
gtsam::LieGroup
Definition: Lie.h:37
gtsam::SO
Definition: SOn.h:54
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:35
gtsam::SO::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Eigen::Matrix< double, N, N >
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
Definition: types.h:288
gtsam::SO::VectorN2
Eigen::Matrix< double, internal::NSquaredSO(N), 1 > VectorN2
Definition: SOn.h:58
N
#define N
Definition: igam.h:9
internal
Definition: BandTriangularSolver.h:13
Eigen::MatrixBase
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::SO::Random
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition: SOn.h:147
gtsam::SO::rows
size_t rows() const
Definition: SOn.h:159
gtsam::SO::Dim
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition: SOn.h:205
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Definition: SOn-inl.h:88
gtsam::SO::IdentityJacobian
static MatrixDD IdentityJacobian(size_t n)
Definition: SOn.h:257
eval
internal::nested_eval< T, 1 >::type eval(const T &xpr)
Definition: sparse_permutations.cpp:38
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::SO::ChartAtOrigin
Definition: SOn.h:242
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:24