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 namespace gtsam {
26 
27 // Implementation for N>=5 just uses dynamic version
28 template <int N>
30  return SOn::Hat(xi);
31 }
32 
33 // Implementation for N>=5 just uses dynamic version
34 template <int N>
36  return SOn::Vee(X);
37 }
38 
39 template <int N>
41  if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented.");
42  const Matrix X = Hat(xi / 2.0);
43  size_t n = AmbientDim(xi.size());
44  const auto I = Eigen::MatrixXd::Identity(n, n);
45  // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
46  return SO((I + X) * (I - X).inverse());
47 }
48 
49 template <int N>
51  ChartJacobian H) {
52  if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented.");
53  const size_t n = R.rows();
54  const auto I = Eigen::MatrixXd::Identity(n, n);
55  const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
56  return -2 * Vee(X);
57 }
58 
59 template <int N>
60 typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
61  if (N==2) return I_1x1; // SO(2) case
62  throw std::runtime_error(
63  "SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
64 }
65 
66 template <int N>
68  throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
69 }
70 
71 template <int N>
73  throw std::runtime_error("SO<N>::ExpmapDerivative only implemented for SO3.");
74 }
75 
76 template <int N>
78  throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
79 }
80 
81 template <int N>
83  throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
84 }
85 
86 // Default fixed size version (but specialized elsewehere for N=2,3,4)
87 template <int N>
90  // Vectorize
92 
93  // If requested, calculate H as (I \oplus Q) * P,
94  // where Q is the N*N rotation matrix, and P is calculated below.
95  if (H) {
96  // Calculate P matrix of vectorized generators
97  // TODO(duy): Should we refactor this as the jacobian of Hat?
99  for (size_t i = 0; i < N; i++) {
100  H->block(i * N, 0, N, dimension) =
101  matrix_ * P.block(i * N, 0, N, dimension);
102  }
103  }
104  return X;
105 }
106 
107 template <int N>
108 void SO<N>::print(const std::string& s) const {
109  std::cout << s << matrix_ << std::endl;
110 }
111 
112 } // namespace gtsam
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::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
gtsam::SO::dimension
@ dimension
Definition: SOn.h:56
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Definition: SOn-inl.h:29
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
Matrix.h
typedef and functions to augment Eigen's MatrixXd
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H={})
Definition: SOn-inl.h:67
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
I
#define I
Definition: main.h:112
gtsam::SO::AmbientDim
static size_t AmbientDim(size_t d)
Definition: SOn.h:212
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
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
gtsam::internal::NSquaredSO
constexpr int NSquaredSO(int N)
Definition: SOn.h:46
gtsam::SO::print
void print(const std::string &s=std::string()) const
Definition: SOn-inl.h:108
gtsam
traits
Definition: SFMdata.h:40
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:50
P
static double P[]
Definition: ellpe.c:68
Eigen::PlainObjectBase::data
EIGEN_DEVICE_FUNC const EIGEN_STRONG_INLINE Scalar * data() const
Definition: PlainObjectBase.h:247
gtsam::SO
Definition: SOn.h:54
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
Eigen::Matrix< double, N, N >
N
#define N
Definition: igam.h:9
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Definition: SOn-inl.h:88
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))


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