gtsam  4.1.0
gtsam
SOn-inl.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 
12 #pragma once
13 
21 #include <gtsam/base/Matrix.h>
22 
23 #include <iostream>
24 
25 using namespace std;
26 
27 namespace gtsam {
28 
29 // Implementation for N>=5 just uses dynamic version
30 template <int N>
31 typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
32  return SOn::Hat(xi);
33 }
34 
35 // Implementation for N>=5 just uses dynamic version
36 template <int N>
37 typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
38  return SOn::Vee(X);
39 }
40 
41 template <int N>
42 SO<N> SO<N>::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) {
43  if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented.");
44  const Matrix X = Hat(xi / 2.0);
45  size_t n = AmbientDim(xi.size());
46  const auto I = Eigen::MatrixXd::Identity(n, n);
47  // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
48  return SO((I + X) * (I - X).inverse());
49 }
50 
51 template <int N>
52 typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
53  ChartJacobian H) {
54  if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented.");
55  const size_t n = R.rows();
56  const auto I = Eigen::MatrixXd::Identity(n, n);
57  const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
58  return -2 * Vee(X);
59 }
60 
61 template <int N>
62 typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
63  if (N==2) return I_1x1; // SO(2) case
64  throw std::runtime_error(
65  "SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
66 }
67 
68 template <int N>
69 SO<N> SO<N>::Expmap(const TangentVector& omega, ChartJacobian H) {
70  throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
71 }
72 
73 template <int N>
74 typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
75  throw std::runtime_error("SO<N>::ExpmapDerivative only implemented for SO3.");
76 }
77 
78 template <int N>
79 typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
80  throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
81 }
82 
83 template <int N>
84 typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
85  throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
86 }
87 
88 // Default fixed size version (but specialized elsewehere for N=2,3,4)
89 template <int N>
90 typename SO<N>::VectorN2 SO<N>::vec(
91  OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
92  // Vectorize
93  VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
94 
95  // If requested, calculate H as (I \oplus Q) * P,
96  // where Q is the N*N rotation matrix, and P is calculated below.
97  if (H) {
98  // Calculate P matrix of vectorized generators
99  // TODO(duy): Should we refactor this as the jacobian of Hat?
100  Matrix P = SO<N>::VectorizedGenerators();
101  for (size_t i = 0; i < N; i++) {
102  H->block(i * N, 0, N, dimension) =
103  matrix_ * P.block(i * N, 0, N, dimension);
104  }
105  }
106  return X;
107 }
108 
109 template <int N>
110 void SO<N>::print(const std::string& s) const {
111  cout << s << matrix_ << endl;
112 }
113 
114 } // namespace gtsam
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition: SOn.h:50
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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
Matrix.h
typedef and functions to augment Eigen's MatrixXd