6810 lines
109 KiB
Plaintext
6810 lines
109 KiB
Plaintext
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
|
\lyxformat 413
|
|
\begin_document
|
|
\begin_header
|
|
\textclass article
|
|
\use_default_options false
|
|
\begin_modules
|
|
eqs-within-sections
|
|
figs-within-sections
|
|
theorems-ams-bytype
|
|
\end_modules
|
|
\maintain_unincluded_children false
|
|
\language english
|
|
\language_package default
|
|
\inputencoding auto
|
|
\fontencoding global
|
|
\font_roman times
|
|
\font_sans default
|
|
\font_typewriter default
|
|
\font_default_family rmdefault
|
|
\use_non_tex_fonts false
|
|
\font_sc false
|
|
\font_osf false
|
|
\font_sf_scale 100
|
|
\font_tt_scale 100
|
|
|
|
\graphics default
|
|
\default_output_format default
|
|
\output_sync 0
|
|
\bibtex_command default
|
|
\index_command default
|
|
\paperfontsize 12
|
|
\spacing single
|
|
\use_hyperref false
|
|
\papersize default
|
|
\use_geometry true
|
|
\use_amsmath 1
|
|
\use_esint 0
|
|
\use_mhchem 1
|
|
\use_mathdots 1
|
|
\cite_engine basic
|
|
\use_bibtopic false
|
|
\use_indices false
|
|
\paperorientation portrait
|
|
\suppress_date false
|
|
\use_refstyle 0
|
|
\index Index
|
|
\shortcut idx
|
|
\color #008000
|
|
\end_index
|
|
\leftmargin 1in
|
|
\topmargin 1in
|
|
\rightmargin 1in
|
|
\bottommargin 1in
|
|
\secnumdepth 3
|
|
\tocdepth 3
|
|
\paragraph_separation indent
|
|
\paragraph_indentation default
|
|
\quotes_language english
|
|
\papercolumns 1
|
|
\papersides 1
|
|
\paperpagestyle default
|
|
\tracking_changes false
|
|
\output_changes false
|
|
\html_math_output 0
|
|
\html_css_as_file 0
|
|
\html_be_strict false
|
|
\end_header
|
|
|
|
\begin_body
|
|
|
|
\begin_layout Title
|
|
Derivatives and Differentials
|
|
\end_layout
|
|
|
|
\begin_layout Author
|
|
Frank Dellaert
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Box Frameless
|
|
position "t"
|
|
hor_pos "c"
|
|
has_inner_box 1
|
|
inner_pos "t"
|
|
use_parbox 0
|
|
use_makebox 0
|
|
width "100col%"
|
|
special "none"
|
|
height "1in"
|
|
height_special "totalheight"
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SqrMah}[3]{\Vert{#1}-{#2}\Vert_{#3}^{2}}
|
|
{\Vert{#1}-{#2}\Vert_{#3}^{2}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SqrZMah}[2]{\Vert{#1}\Vert_{#2}^{2}}
|
|
{\Vert{#1}\Vert_{#2}^{2}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rone}{\mathbb{R}}
|
|
{\mathbb{R}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Reals}[1]{\mathbb{R}^{#1}}
|
|
{\mathbb{R}^{#1}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\OneD}[1]{\Reals{#1}\rightarrow\Rone}
|
|
{\Reals{#1}\rightarrow\Rone}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Multi}[2]{\Reals{#1}\rightarrow\Reals{#2}}
|
|
{\Reals{#1}\rightarrow\Reals{#2}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Man}{\mathcal{M}}
|
|
{\mathcal{M}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Note Comment
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
Derivatives
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
|
|
{\frac{\partial#1}{\partial#2}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
|
|
{#1\biggr\rvert_{#2}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
|
|
{\at{\deriv{#1}{#2}}{#3}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Note Comment
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
Lie Groups
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\xhat}{\hat{x}}
|
|
{\hat{x}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\yhat}{\hat{y}}
|
|
{\hat{y}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Ad}[1]{Ad_{#1}}
|
|
{Ad_{#1}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\define}{\stackrel{\Delta}{=}}
|
|
{\stackrel{\Delta}{=}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\gg}{\mathfrak{g}}
|
|
{\mathfrak{g}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rn}{\mathbb{R}^{n}}
|
|
{\mathbb{R}^{n}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Note Comment
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
SO(2)
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rtwo}{\mathbb{R}^{2}}
|
|
{\mathbb{R}^{2}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SOtwo}{SO(2)}
|
|
{SO(2)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\sotwo}{\mathfrak{so(2)}}
|
|
{\mathfrak{so(2)}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\that}{\hat{\theta}}
|
|
{\hat{\theta}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\skew}[1]{[#1]_{+}}
|
|
{[#1]_{+}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Note Comment
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
SE(2)
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SEtwo}{SE(2)}
|
|
{SE(2)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\setwo}{\mathfrak{se(2)}}
|
|
{\mathfrak{se(2)}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Note Comment
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
SO(3)
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rthree}{\mathbb{R}^{3}}
|
|
{\mathbb{R}^{3}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SOthree}{SO(3)}
|
|
{SO(3)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\sothree}{\mathfrak{so(3)}}
|
|
{\mathfrak{so(3)}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\what}{\hat{\omega}}
|
|
{\hat{\omega}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Skew}[1]{[#1]_{\times}}
|
|
{[#1]_{\times}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Note Comment
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
SE(3)
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rsix}{\mathbb{R}^{6}}
|
|
{\mathbb{R}^{6}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SEthree}{SE(3)}
|
|
{SE(3)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\sethree}{\mathfrak{se(3)}}
|
|
{\mathfrak{se(3)}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\xihat}{\hat{\xi}}
|
|
{\hat{\xi}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Part
|
|
Theory
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Optimization
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We will be concerned with minimizing a non-linear least squares objective
|
|
of the form
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
x^{*}=\arg\min_{x}\SqrMah{h(x)}z{\Sigma}\label{eq:objective}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $x\in\Man$
|
|
\end_inset
|
|
|
|
is a point on an
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional manifold (which could be
|
|
\begin_inset Formula $\Reals n$
|
|
\end_inset
|
|
|
|
, an n-dimensional Lie group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
, or a general manifold
|
|
\begin_inset Formula $\Man)$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $z\in\Reals m$
|
|
\end_inset
|
|
|
|
is an observed measurement,
|
|
\begin_inset Formula $h:\Man\rightarrow\Reals m$
|
|
\end_inset
|
|
|
|
is a measurement function that predicts
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
from
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\SqrZMah e{\Sigma}\define e^{T}\Sigma^{-1}e$
|
|
\end_inset
|
|
|
|
is the squared Mahalanobis distance with covariance
|
|
\begin_inset Formula $\Sigma$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
To minimize
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:objective"
|
|
|
|
\end_inset
|
|
|
|
we need a notion of how the non-linear measurement function
|
|
\begin_inset Formula $h(x)$
|
|
\end_inset
|
|
|
|
behaves in the neighborhood of a linearization point
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
Loosely speaking, we would like to define an
|
|
\begin_inset Formula $m\times n$
|
|
\end_inset
|
|
|
|
Jacobian matrix
|
|
\begin_inset Formula $H_{a}$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
h(a\oplus\xi)\approx h(a)+H_{a}\xi\label{eq:LocalBehavior}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\xi\in\Reals n$
|
|
\end_inset
|
|
|
|
, and the operation
|
|
\begin_inset Formula $\oplus$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
increments
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $a\in\Man$
|
|
\end_inset
|
|
|
|
.
|
|
Below we more formally develop this notion, first for functions from
|
|
\begin_inset Formula $\Multi nm$
|
|
\end_inset
|
|
|
|
, then for Lie groups, and finally for manifolds.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Once equipped with the approximation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:LocalBehavior"
|
|
|
|
\end_inset
|
|
|
|
, we can minimize the objective function
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:objective"
|
|
|
|
\end_inset
|
|
|
|
with respect to
|
|
\begin_inset Formula $\delta x$
|
|
\end_inset
|
|
|
|
instead:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\xi^{*}=\arg\min_{\xi}\SqrMah{h(a)+H_{a}\xi}z{\Sigma}\label{eq:ApproximateObjective}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
This can be done by setting the derivative of
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:ApproximateObjective"
|
|
|
|
\end_inset
|
|
|
|
to zero,
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{1}{2}H_{a}^{T}(h(a)+H_{a}\xi-z)=0
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
yielding the
|
|
\series bold
|
|
normal equations
|
|
\series default
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
H_{a}^{T}H_{a}\xi=H_{a}^{T}\left(z-h(a)\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which can be solved using Cholesky factorization.
|
|
Of course, we might have to iterate this multiple times, and use a trust-region
|
|
method to bound
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
when the approximation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:LocalBehavior"
|
|
|
|
\end_inset
|
|
|
|
is not good.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Multivariate Differentiation
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For a vector space
|
|
\begin_inset Formula $\Reals n$
|
|
\end_inset
|
|
|
|
, the notion of an increment is just done by vector addition
|
|
\begin_inset Formula
|
|
\[
|
|
a\oplus\xi\define a+\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and for the approximation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:LocalBehavior"
|
|
|
|
\end_inset
|
|
|
|
we will use a Taylor expansion using multivariate differentiation.
|
|
However, loosely following
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Spivak65book"
|
|
|
|
\end_inset
|
|
|
|
, we use a perhaps unfamiliar way to define derivatives:
|
|
\end_layout
|
|
|
|
\begin_layout Definition
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "def:differentiable"
|
|
|
|
\end_inset
|
|
|
|
We define a function
|
|
\begin_inset Formula $f:\Multi nm$
|
|
\end_inset
|
|
|
|
to be
|
|
\series bold
|
|
differentiable
|
|
\series default
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
if there exists a matrix
|
|
\begin_inset Formula $f'(a)\in\Reals{m\times n}$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula
|
|
\[
|
|
\lim_{\delta x\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f(a+\xi)\right|}{\left|\xi\right|}=0
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\left|e\right|\define\sqrt{e^{T}e}$
|
|
\end_inset
|
|
|
|
is the usual norm.
|
|
If
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
is differentiable, then the matrix
|
|
\begin_inset Formula $f'(a)$
|
|
\end_inset
|
|
|
|
is called the
|
|
\series bold
|
|
Jacobian matrix
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
, and the linear map
|
|
\begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$
|
|
\end_inset
|
|
|
|
is called the
|
|
\series bold
|
|
derivative
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
When no confusion is likely, we use the notation
|
|
\begin_inset Formula $F_{a}\define f'(a)$
|
|
\end_inset
|
|
|
|
to stress that
|
|
\begin_inset Formula $f'(a)$
|
|
\end_inset
|
|
|
|
is a matrix.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The benefit of using this definition is that it generalizes the notion of
|
|
a scalar derivative
|
|
\begin_inset Formula $f'(a):\Rone\rightarrow\Rone$
|
|
\end_inset
|
|
|
|
to multivariate functions from
|
|
\begin_inset Formula $\Multi nm$
|
|
\end_inset
|
|
|
|
.
|
|
In particular, the derivative
|
|
\begin_inset Formula $Df_{a}$
|
|
\end_inset
|
|
|
|
maps vector increments
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
on
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
to increments
|
|
\begin_inset Formula $f'(a)\xi$
|
|
\end_inset
|
|
|
|
on
|
|
\begin_inset Formula $f(a)$
|
|
\end_inset
|
|
|
|
, such that this linear map locally approximates
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
f(a+\xi)\approx f(a)+f'(a)\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "ex:projection"
|
|
|
|
\end_inset
|
|
|
|
The function
|
|
\begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$
|
|
\end_inset
|
|
|
|
projects a 3D point
|
|
\begin_inset Formula $(x,y,z)$
|
|
\end_inset
|
|
|
|
to the image plane, and has the Jacobian matrix
|
|
\begin_inset Formula
|
|
\[
|
|
\pi'(x,y,z)=\frac{1}{z}\left[\begin{array}{ccc}
|
|
1 & 0 & -x/z\\
|
|
0 & 1 & -y/z
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Properties of Derivatives
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
This notion of a multivariate derivative obeys the usual rules:
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
(Chain rule) If
|
|
\begin_inset Formula $f:\Multi np$
|
|
\end_inset
|
|
|
|
is differentiable at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $g:\Multi pm$
|
|
\end_inset
|
|
|
|
is differentiable at
|
|
\begin_inset Formula $f(a)$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
then
|
|
\begin_inset Formula $D(g\circ f)_{a}=Dg_{f(a)}\circ Df_{a}$
|
|
\end_inset
|
|
|
|
and
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
then the Jacobian matrix
|
|
\begin_inset Formula $H_{a}$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $h=g\circ f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $m\times n$
|
|
\end_inset
|
|
|
|
matrix product
|
|
\begin_inset Formula
|
|
\[
|
|
H_{a}=G_{f(a)}F_{a}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $G_{f(a)}$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $m\times p$
|
|
\end_inset
|
|
|
|
Jacobian matrix of
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
evaluated at
|
|
\begin_inset Formula $f(a)$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $F_{a}$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $p\times n$
|
|
\end_inset
|
|
|
|
Jacobian matrix of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
evaluated at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
See
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Spivak65book"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "ex:chain-rule"
|
|
|
|
\end_inset
|
|
|
|
If we follow the projection
|
|
\begin_inset Formula $\pi$
|
|
\end_inset
|
|
|
|
by a calibration step
|
|
\begin_inset Formula $\gamma:(x,y)\mapsto(u_{0}+fx,u_{0}+fy)$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula
|
|
\[
|
|
\gamma'(x,y)=\left[\begin{array}{cc}
|
|
f & 0\\
|
|
0 & f
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
then the combined function
|
|
\begin_inset Formula $\gamma\circ\pi$
|
|
\end_inset
|
|
|
|
has the Jacobian matrix
|
|
\begin_inset Formula
|
|
\[
|
|
(\gamma\circ\pi)'(x,y)=\frac{f}{z}\left[\begin{array}{ccc}
|
|
1 & 0 & -x/z\\
|
|
0 & 1 & -y/z
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
(Inverse) If
|
|
\begin_inset Formula $f:\Multi nn$
|
|
\end_inset
|
|
|
|
is differentiable and has a differentiable inverse
|
|
\begin_inset Formula $g\define f^{-1}$
|
|
\end_inset
|
|
|
|
, then its Jacobian matrix
|
|
\begin_inset Formula $G_{a}$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
is just the inverse of that of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
, evaluated at
|
|
\begin_inset Formula $g(a)$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
G_{a}=\left[F_{g(a)}\right]^{-1}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
See
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Spivak65book"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "ex:inverse"
|
|
|
|
\end_inset
|
|
|
|
The function
|
|
\begin_inset Formula $f:(x,y)\mapsto(x^{2},xy)$
|
|
\end_inset
|
|
|
|
has the Jacobian matrix
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
\begin_inset Formula
|
|
\[
|
|
F_{(x,y)}=\left[\begin{array}{cc}
|
|
2x & 0\\
|
|
y & x
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and, for
|
|
\begin_inset Formula $x\geq0$
|
|
\end_inset
|
|
|
|
, its inverse is the function
|
|
\begin_inset Formula $g:(x,y)\mapsto(x^{1/2},x^{-1/2}y)$
|
|
\end_inset
|
|
|
|
with the Jacobian matrix
|
|
\begin_inset Formula
|
|
\[
|
|
G_{(x,y)}=\frac{1}{2}\left[\begin{array}{cc}
|
|
x^{-1/2} & 0\\
|
|
-x^{-3/2}y & 2x^{-1/2}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
It is easily verified that
|
|
\begin_inset Formula
|
|
\[
|
|
g'(a,b)f'(a^{1/2},a^{-1/2}b)=\frac{1}{2}\left[\begin{array}{cc}
|
|
a^{-1/2} & 0\\
|
|
-a^{-3/2}b & 2a^{-1/2}
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
2a^{1/2} & 0\\
|
|
a^{-1/2}b & a^{1/2}
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
1 & 0\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Problem
|
|
Verify the above for
|
|
\begin_inset Formula $(a,b)=(4,6)$
|
|
\end_inset
|
|
|
|
.
|
|
Sketch the situation graphically to get insight.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Computing Multivariate Derivatives
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Computing derivatives is made easy by defining the concept of a partial
|
|
derivative:
|
|
\end_layout
|
|
|
|
\begin_layout Definition
|
|
For
|
|
\begin_inset Formula $f:\OneD n$
|
|
\end_inset
|
|
|
|
, the
|
|
\series bold
|
|
partial derivative
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
,
|
|
\series bold
|
|
|
|
\series default
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
D_{j}f(a)\define\lim_{h\rightarrow0}\frac{f\left(a^{1},\ldots,a^{j}+h,\ldots,a^{n}\right)-f\left(a^{1},\ldots,a^{n}\right)}{h}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which is the ordinary derivative of the scalar function
|
|
\begin_inset Formula $g(x)\define f\left(a^{1},\ldots,x,\ldots,a^{n}\right)$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Using this definition, one can show that the Jacobian matrix
|
|
\begin_inset Formula $F_{a}$
|
|
\end_inset
|
|
|
|
of a differentiable
|
|
\emph on
|
|
multivariate
|
|
\emph default
|
|
function
|
|
\begin_inset Formula $f:\Multi nm$
|
|
\end_inset
|
|
|
|
consists simply of the
|
|
\begin_inset Formula $m\times n$
|
|
\end_inset
|
|
|
|
partial derivatives
|
|
\begin_inset Formula $D_{j}f^{i}(a)$
|
|
\end_inset
|
|
|
|
, evaluated at
|
|
\begin_inset Formula $a\in\Reals n$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
F_{a}=\left[\begin{array}{ccc}
|
|
D_{1}f^{1}(a) & \cdots & D_{n}f^{1}(a)\\
|
|
\vdots & \ddots & \vdots\\
|
|
D_{1}f^{m}(a) & \ldots & D_{n}f^{m}(a)
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Problem
|
|
Verify the derivatives in Examples
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "ex:projection"
|
|
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "ex:inverse"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Multivariate Functions on Lie Groups
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Lie Groups
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Lie groups are not as easy to treat as the vector space
|
|
\begin_inset Formula $\Reals n$
|
|
\end_inset
|
|
|
|
but nevertheless have a lot of structure.
|
|
To generalize the concept of the total derivative above we just need to
|
|
replace
|
|
\begin_inset Formula $a\oplus\xi$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:ApproximateObjective"
|
|
|
|
\end_inset
|
|
|
|
with a suitable operation in the Lie group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
.
|
|
In particular, the notion of an exponential map allows us to define an
|
|
incremental transformation as tracing out a geodesic curve on the group
|
|
manifold along a certain
|
|
\series bold
|
|
tangent vector
|
|
\series default
|
|
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
a\oplus\xi\define a\exp\left(\hat{\xi}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\xi\in\Reals n$
|
|
\end_inset
|
|
|
|
for an
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional Lie group,
|
|
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
|
|
\end_inset
|
|
|
|
the Lie algebra element corresponding to the vector
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\exp\hat{\xi}$
|
|
\end_inset
|
|
|
|
the exponential map.
|
|
Note that if
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
is equal to
|
|
\begin_inset Formula $\Reals n$
|
|
\end_inset
|
|
|
|
then composing with the exponential map
|
|
\begin_inset Formula $ae^{\xihat}$
|
|
\end_inset
|
|
|
|
is just vector addition
|
|
\begin_inset Formula $a+\xi$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For the Lie group
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
of 3D rotations the vector
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
is denoted as
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
and represents an angular displacement.
|
|
The Lie algebra element
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
is a skew symmetric matrix denoted as
|
|
\begin_inset Formula $\Skew{\omega}\in\sothree$
|
|
\end_inset
|
|
|
|
, and is given by
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega}=\left[\begin{array}{ccc}
|
|
0 & -\omega_{z} & \omega_{y}\\
|
|
\omega_{z} & 0 & -\omega_{x}\\
|
|
-\omega_{y} & \omega_{x} & 0
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Finally, the increment
|
|
\begin_inset Formula $a\oplus\xi=ae^{\xihat}$
|
|
\end_inset
|
|
|
|
corresponds to an incremental rotation
|
|
\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can generalize Definition
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "def:differentiable"
|
|
|
|
\end_inset
|
|
|
|
to map exponential coordinates
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
to increments
|
|
\begin_inset Formula $f'(a)\xi$
|
|
\end_inset
|
|
|
|
on
|
|
\begin_inset Formula $f(a)$
|
|
\end_inset
|
|
|
|
, such that the linear map
|
|
\begin_inset Formula $Df_{a}$
|
|
\end_inset
|
|
|
|
locally approximates a function
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
from
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $\Reals m$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
f(ae^{\xihat})\approx f(a)+f'(a)\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Definition
|
|
We define a function
|
|
\begin_inset Formula $f:G\rightarrow\Reals m$
|
|
\end_inset
|
|
|
|
to be
|
|
\series bold
|
|
differentiable
|
|
\series default
|
|
at
|
|
\begin_inset Formula $a\in G$
|
|
\end_inset
|
|
|
|
if there exists a matrix
|
|
\begin_inset Formula $f'(a)\in\Reals{m\times n}$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula
|
|
\[
|
|
\lim_{\xi\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f(ae^{\hat{\xi}})\right|}{\left|\xi\right|}=0
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
If
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
is differentiable, then the matrix
|
|
\begin_inset Formula $f'(a)$
|
|
\end_inset
|
|
|
|
is called the
|
|
\series bold
|
|
Jacobian matrix
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
, and the linear map
|
|
\begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$
|
|
\end_inset
|
|
|
|
is called the
|
|
\series bold
|
|
derivative
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Note that the vectors
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
can be viewed as lying in the tangent space to
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
, but defining this rigorously would take us on a longer tour of differential
|
|
geometry.
|
|
Informally,
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
is simply the direction, in a local coordinate frame, that is locally tangent
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
to a geodesic curve
|
|
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
|
|
\end_inset
|
|
|
|
traced out by the exponential map, with
|
|
\begin_inset Formula $\gamma(0)=a$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivative of an Action
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "sec:Derivatives-of-Actions"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The (usual) action of an
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional matrix group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
is matrix-vector multiplication on
|
|
\begin_inset Formula $\mathbb{R}^{n}$
|
|
\end_inset
|
|
|
|
, i.e.,
|
|
\begin_inset Formula $f:G\times\Reals n\rightarrow\Reals n$
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula
|
|
\[
|
|
f(T,p)=Tp
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Since this is a function defined on the product
|
|
\begin_inset Formula $G\times\Reals n$
|
|
\end_inset
|
|
|
|
the derivative is a linear transformation
|
|
\begin_inset Formula $Df:\Multi{2n}n$
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula
|
|
\[
|
|
Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p)}\left(\delta p\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "th:Action"
|
|
|
|
\end_inset
|
|
|
|
The Jacobian matrix of the group action
|
|
\begin_inset Formula $f(T,p)=Tp$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $(T,p)$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula
|
|
\[
|
|
F_{(T,p)}=\left[\begin{array}{cc}
|
|
TH(p) & T\end{array}\right]=T\left[\begin{array}{cc}
|
|
H(p) & I_{n}\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$
|
|
\end_inset
|
|
|
|
a linear mapping that depends on
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $I_{n}$
|
|
\end_inset
|
|
|
|
the
|
|
\begin_inset Formula $n\times n$
|
|
\end_inset
|
|
|
|
identity matrix.
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
First, the derivative
|
|
\begin_inset Formula $D_{2}f$
|
|
\end_inset
|
|
|
|
with respect to
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
is easy, as its matrix is simply T:
|
|
\begin_inset Formula
|
|
\[
|
|
f(T,p+\delta p)=T(p+\delta p)=Tp+T\delta p=f(T,p)+D_{2}f(\delta p)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
For the derivative
|
|
\begin_inset Formula $D_{1}f$
|
|
\end_inset
|
|
|
|
with respect to a change in the first argument
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
, we want to find the linear map
|
|
\begin_inset Formula $D_{1}f$
|
|
\end_inset
|
|
|
|
such that
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
Tp+D_{1}f(\xi)\approx f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
Since the matrix exponential is given by the series
|
|
\begin_inset Formula $e^{A}=I+A+\frac{A^{2}}{2!}+\frac{A^{3}}{3!}+\ldots$
|
|
\end_inset
|
|
|
|
we have, to first order
|
|
\begin_inset Formula
|
|
\[
|
|
Te^{\hat{\xi}}p\approx T(I+\hat{\xi})p=Tp+T\hat{\xi}p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $D_{1}f(\xi)=T\hat{\xi}p$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
Note also that
|
|
\begin_inset Formula
|
|
\[
|
|
T\hat{\xi}p=\left(T\hat{\xi}T^{-1}\right)Tp=\left(\Ad T\xihat\right)\left(Tp\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
Hence, to complete the proof, we need to show that
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\xihat p=H(p)\xi\label{eq:Hp}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
an
|
|
\begin_inset Formula $n\times n$
|
|
\end_inset
|
|
|
|
matrix that depends on
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
.
|
|
Expressing the map
|
|
\begin_inset Formula $\xi\rightarrow\hat{\xi}$
|
|
\end_inset
|
|
|
|
in terms of the Lie algebra generators
|
|
\begin_inset Formula $G^{i}$
|
|
\end_inset
|
|
|
|
, using tensors and Einstein summation, we have
|
|
\begin_inset Formula $\hat{\xi}_{j}^{i}=G_{jk}^{i}\xi^{k}$
|
|
\end_inset
|
|
|
|
allowing us to calculate
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $\hat{\xi}p$
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\noun default
|
|
\color inherit
|
|
as
|
|
\begin_inset Formula
|
|
\[
|
|
\left(\hat{\xi}p\right)^{i}=\hat{\xi}_{j}^{i}p^{j}=G_{jk}^{i}\xi^{k}p^{j}=\left(G_{jk}^{i}p^{j}\right)\xi^{k}=H_{k}^{i}(p)\xi^{k}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For 3D rotations
|
|
\begin_inset Formula $R\in\SOthree$
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula $\hat{\omega}=\Skew{\omega}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula
|
|
\[
|
|
G_{k=1}:\left(\begin{array}{ccc}
|
|
0 & 0 & 0\\
|
|
0 & 0 & -1\\
|
|
0 & 1 & 0
|
|
\end{array}\right)\mbox{}G_{k=2}:\left(\begin{array}{ccc}
|
|
0 & 0 & 1\\
|
|
0 & 0 & 0\\
|
|
-1 & 0 & 0
|
|
\end{array}\right)\mbox{ }G_{k=3}:\left(\begin{array}{ccc}
|
|
0 & -1 & 0\\
|
|
1 & 0 & 0\\
|
|
0 & 0 & 0
|
|
\end{array}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
The matrices
|
|
\begin_inset Formula $\left(G_{k}^{i}\right)_{j}$
|
|
\end_inset
|
|
|
|
are obtained by assembling the
|
|
\begin_inset Formula $j^{th}$
|
|
\end_inset
|
|
|
|
columns of the generators above, yielding
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
equal to:
|
|
\begin_inset Formula
|
|
\[
|
|
\left(\begin{array}{ccc}
|
|
0 & 0 & 0\\
|
|
0 & 0 & 1\\
|
|
0 & -1 & 0
|
|
\end{array}\right)p^{1}+\left(\begin{array}{ccc}
|
|
0 & 0 & -1\\
|
|
0 & 0 & 0\\
|
|
1 & 0 & 0
|
|
\end{array}\right)p^{2}+\left(\begin{array}{ccc}
|
|
0 & 1 & 0\\
|
|
-1 & 0 & 0\\
|
|
0 & 0 & 0
|
|
\end{array}\right)p^{3}=\left(\begin{array}{ccc}
|
|
0 & p^{3} & -p^{2}\\
|
|
-p^{3} & 0 & p^{1}\\
|
|
p^{2} & -p^{1} & 0
|
|
\end{array}\right)=\Skew{-p}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\noun default
|
|
\color inherit
|
|
Hence, the Jacobian matrix of
|
|
\begin_inset Formula $f(R,p)=Rp$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula
|
|
\[
|
|
F_{(R,p)}=R\left(\begin{array}{cc}
|
|
\Skew{-p} & I_{3}\end{array}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivative of an Inverse Action
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Applying the action by the inverse of
|
|
\begin_inset Formula $T\in G$
|
|
\end_inset
|
|
|
|
yields a function
|
|
\begin_inset Formula $g:G\times\Reals n\rightarrow\Reals n$
|
|
\end_inset
|
|
|
|
defined by
|
|
\begin_inset Formula
|
|
\[
|
|
g(T,p)=T^{-1}p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "Th:InverseAction"
|
|
|
|
\end_inset
|
|
|
|
The Jacobian matrix of the inverse group action
|
|
\begin_inset Formula $g(T,p)=T^{-1}p$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula
|
|
\[
|
|
G_{(T,p)}=\left[\begin{array}{cc}
|
|
-H(T^{-1}p) & T^{-1}\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$
|
|
\end_inset
|
|
|
|
is the same mapping as before.
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
Again, the derivative
|
|
\begin_inset Formula $D_{2}g$
|
|
\end_inset
|
|
|
|
with respect to in
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
is easy, the matrix of which is simply
|
|
\begin_inset Formula $T^{-1}$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
g(T,p+\delta p)=T^{-1}(p+\delta p)=T^{-1}p+T^{-1}\delta p=g(T,p)+D_{2}g(\delta p)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Conversely, a change in
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
yields
|
|
\begin_inset Formula
|
|
\[
|
|
g(Te^{\xihat},p)=\left(Te^{\xihat}\right)^{-1}p=e^{-\xihat}T^{-1}p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Similar to before, if we expand the matrix exponential we get
|
|
\begin_inset Formula
|
|
\[
|
|
e^{-A}=I-A+\frac{A^{2}}{2!}-\frac{A^{3}}{3!}+\ldots
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
so
|
|
\begin_inset Formula
|
|
\[
|
|
e^{-\xihat}T^{-1}p\approx(I-\xihat)T^{-1}p=g(T,p)-\xihat\left(T^{-1}p\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For 3D rotations
|
|
\begin_inset Formula $R\in\SOthree$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula $R^{-1}=R^{T}$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $H(p)=-\Skew p$
|
|
\end_inset
|
|
|
|
, and hence the Jacobian matrix of
|
|
\begin_inset Formula $g(R,p)=R^{T}p$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula
|
|
\[
|
|
G_{(R,p)}=\left(\begin{array}{cc}
|
|
\Skew{R^{T}p} & R^{T}\end{array}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
My earlier attempt: because the wedge operator is linear, we have
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
f(\xi+x) & = & \exp\widehat{\left(\xi+x\right)}\\
|
|
& = & \exp\left(\xihat+\hat{x}\right)
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
However, except for commutative Lie groups, it is not true that
|
|
\begin_inset Formula $\exp\left(\xihat+\hat{x}\right)=\exp\xihat\exp\hat{x}$
|
|
\end_inset
|
|
|
|
.
|
|
However, if we expand the matrix exponential to second order and assume
|
|
|
|
\begin_inset Formula $x\rightarrow0$
|
|
\end_inset
|
|
|
|
we do have
|
|
\begin_inset Formula
|
|
\[
|
|
\exp\left(\xihat+\hat{x}\right)\approx I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Now, if we ask what
|
|
\begin_inset Formula $\hat{y}$
|
|
\end_inset
|
|
|
|
would effect the same change:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\exp\xihat\exp\yhat & = & I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat\\
|
|
\exp\xihat(I+\yhat) & = & I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat\\
|
|
\left(\exp\xihat\right)\yhat & = & \xhat+\xhat\xihat
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Instantaneous Velocity
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For matrix Lie groups, if we have a matrix
|
|
\begin_inset Formula $T_{b}^{n}(t)$
|
|
\end_inset
|
|
|
|
that depends on a parameter
|
|
\begin_inset Formula $t$
|
|
\end_inset
|
|
|
|
, i.e.,
|
|
\begin_inset Formula $T_{b}^{n}(t)$
|
|
\end_inset
|
|
|
|
follows a curve on the manifold, then it would be of interest to find the
|
|
velocity of a point
|
|
\begin_inset Formula $q^{n}(t)=T_{b}^{n}(t)p^{b}$
|
|
\end_inset
|
|
|
|
acted upon by
|
|
\begin_inset Formula $T_{b}^{n}(t)$
|
|
\end_inset
|
|
|
|
.
|
|
We can express the velocity of
|
|
\begin_inset Formula $q(t)$
|
|
\end_inset
|
|
|
|
in both the n-frame and b-frame:
|
|
\begin_inset Formula
|
|
\[
|
|
\dot{q}^{n}=\dot{T}_{b}^{n}p^{b}=\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}p^{n}\mbox{\,\,\,\,\ and\,\,\,\,}\dot{q}^{b}=\left(T_{b}^{n}\right)^{-1}\dot{q}^{n}=\left(T_{b}^{n}\right)^{-1}\dot{T}_{b}^{n}p^{b}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Both the matrices
|
|
\begin_inset Formula $\xihat_{nb}^{n}\define\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\xihat_{nb}^{b}\define\left(T_{b}^{n}\right)^{-1}\dot{T}_{b}^{n}$
|
|
\end_inset
|
|
|
|
are skew-symmetric Lie algebra elements that describe the
|
|
\series bold
|
|
instantaneous velocity
|
|
\series default
|
|
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "page 51 for rotations, page 419 for SE(3)"
|
|
key "Murray94book"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
We will revisit this for both rotations and rigid 3D transformations.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Differentials: Smooth Mapping between Lie Groups
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Motivation and Definition
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The above shows how to compute the derivative of a function
|
|
\begin_inset Formula $f:G\rightarrow\Reals m$
|
|
\end_inset
|
|
|
|
.
|
|
However, what if the argument to
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
is itself the result of a mapping between Lie groups? In other words,
|
|
\begin_inset Formula $f=g\circ\varphi$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $g:G\rightarrow\Reals m$
|
|
\end_inset
|
|
|
|
and where
|
|
\begin_inset Formula $\varphi:H\rightarrow G$
|
|
\end_inset
|
|
|
|
is a smooth mapping from the
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional Lie group
|
|
\begin_inset Formula $H$
|
|
\end_inset
|
|
|
|
to the
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
-dimensional Lie group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
.
|
|
In this case, one would expect that we can arrive at
|
|
\begin_inset Formula $Df_{a}$
|
|
\end_inset
|
|
|
|
by composing linear maps, as follows:
|
|
\begin_inset Formula
|
|
\[
|
|
f'(a)=(g\circ\varphi)'(a)=G_{\varphi(a)}\varphi'(a)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\varphi'(a)$
|
|
\end_inset
|
|
|
|
is an
|
|
\begin_inset Formula $n\times p$
|
|
\end_inset
|
|
|
|
matrix that is the best linear approximation to the map
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $\varphi:H\rightarrow G$
|
|
\end_inset
|
|
|
|
.
|
|
The corresponding linear map
|
|
\begin_inset Formula $D\varphi_{a}$
|
|
\end_inset
|
|
|
|
is called the
|
|
\family default
|
|
\series bold
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
differential
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
or
|
|
\series bold
|
|
pushforward
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
of
|
|
\begin_inset Formula $ $
|
|
\end_inset
|
|
|
|
the mapping
|
|
\begin_inset Formula $\varphi$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
Because a rigorous definition will lead us too far astray, here we only
|
|
informally define the pushforward of
|
|
\begin_inset Formula $\varphi$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
as the linear map
|
|
\begin_inset Formula $D\varphi_{a}:\Multi np$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula $D\varphi_{a}\left(\xi\right)\define\varphi'(a)\xi$
|
|
\end_inset
|
|
|
|
and
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\varphi\left(ae^{\xihat}\right)\approx\varphi\left(a\right)\exp\left(\widehat{\varphi'(a)\xi}\right)\label{eq:pushforward}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
with equality for
|
|
\begin_inset Formula $\xi\rightarrow0$
|
|
\end_inset
|
|
|
|
.
|
|
We call
|
|
\begin_inset Formula $\varphi'(a)$
|
|
\end_inset
|
|
|
|
the
|
|
\series bold
|
|
Jacobian matrix
|
|
\series default
|
|
of the map
|
|
\begin_inset Formula $\varphi$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
Below we show that even with this informal definition we can deduce the
|
|
pushforward in a number of useful cases.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Left Multiplication with a Constant
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
Suppose
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
is an
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional Lie group, and
|
|
\begin_inset Formula $\varphi:G\rightarrow G$
|
|
\end_inset
|
|
|
|
is defined as
|
|
\begin_inset Formula $\varphi(g)=hg$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $h\in G$
|
|
\end_inset
|
|
|
|
a constant.
|
|
Then
|
|
\begin_inset Formula $D\varphi_{a}$
|
|
\end_inset
|
|
|
|
is the identity mapping and
|
|
\begin_inset Formula
|
|
\[
|
|
\varphi'(a)=I_{n}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
Defining
|
|
\begin_inset Formula $y=D\varphi_{a}x$
|
|
\end_inset
|
|
|
|
as in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:pushforward"
|
|
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\varphi(a)e^{\yhat} & = & \varphi(ae^{\xhat})\\
|
|
hae^{\yhat} & = & hae^{\xhat}\\
|
|
y & = & x
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Pushforward of the Inverse Mapping
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A well known property of Lie groups is the the fact that applying an incremental
|
|
change
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
in a different frame
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
can be applied in a single step by applying the change
|
|
\begin_inset Formula $Ad_{g}\xihat$
|
|
\end_inset
|
|
|
|
in the original frame,
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
ge^{\xihat}g^{-1}=\exp\left(Ad_{g}\xihat\right)\label{eq:Adjoint2}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $Ad_{g}:\mathfrak{g}\rightarrow\mathfrak{g}$
|
|
\end_inset
|
|
|
|
is the
|
|
\series bold
|
|
adjoint representation
|
|
\series default
|
|
.
|
|
This comes in handy in the following:
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
Suppose that
|
|
\begin_inset Formula $\varphi:G\rightarrow G$
|
|
\end_inset
|
|
|
|
is defined as the mapping from an element
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
to its
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
|
|
\begin_inset Formula $g^{-1}$
|
|
\end_inset
|
|
|
|
, i.e.,
|
|
\begin_inset Formula $\varphi(g)=g^{-1}$
|
|
\end_inset
|
|
|
|
, then the pushforward
|
|
\begin_inset Formula $D\varphi_{a}$
|
|
\end_inset
|
|
|
|
satisfies
|
|
\begin_inset Formula
|
|
\begin{align}
|
|
\left(D\varphi_{a}x\right)\hat{} & =-Ad_{a}\xhat\label{eq:Dinverse}
|
|
\end{align}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset ERT
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
|
|
\backslash
|
|
noindent
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
In other words, and this is intuitive in hindsight, approximating the inverse
|
|
is accomplished by negation of
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
, along with an adjoint to make sure it is applied in the right frame.
|
|
|
|
\begin_inset ERT
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
|
|
\backslash
|
|
noindent
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
Note, however, that
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:Dinverse"
|
|
|
|
\end_inset
|
|
|
|
does not immediately yield a useful expression for the Jacobian matrix
|
|
|
|
\begin_inset Formula $\varphi'(a)$
|
|
\end_inset
|
|
|
|
, but in many important cases this will turn out to be easy.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
Defining
|
|
\begin_inset Formula $y=D\varphi_{a}x$
|
|
\end_inset
|
|
|
|
as in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:pushforward"
|
|
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\varphi(a)e^{\yhat} & = & \varphi(ae^{\xhat})\\
|
|
a^{-1}e^{\yhat} & = & \left(ae^{\xhat}\right)^{-1}\\
|
|
e^{\yhat} & = & -ae^{\xhat}a^{-1}\\
|
|
\yhat & = & -\Ad a\xhat
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For 3D rotations
|
|
\begin_inset Formula $R\in\SOthree$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula
|
|
\[
|
|
Ad_{g}(\hat{\omega})=R\hat{\omega}R^{T}=\Skew{R\omega}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and hence the pushforward for the inverse mapping
|
|
\begin_inset Formula $\varphi(R)=R^{T}$
|
|
\end_inset
|
|
|
|
has the matrix
|
|
\begin_inset Formula $\varphi'(R)=-R$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Right Multiplication with a Constant
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
Suppose
|
|
\begin_inset Formula $\varphi:G\rightarrow G$
|
|
\end_inset
|
|
|
|
is defined as
|
|
\begin_inset Formula $\varphi(g)=gh$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $h\in G$
|
|
\end_inset
|
|
|
|
a constant.
|
|
Then
|
|
\begin_inset Formula $D\varphi_{a}$
|
|
\end_inset
|
|
|
|
satisfies
|
|
\begin_inset Formula
|
|
\[
|
|
\left(D\varphi_{a}x\right)\hat{}=\Ad{h^{-1}}\xhat
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
Defining
|
|
\begin_inset Formula $y=D\varphi_{a}x$
|
|
\end_inset
|
|
|
|
as in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:pushforward"
|
|
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula
|
|
\begin{align*}
|
|
\varphi(a)e^{\yhat} & =\varphi(ae^{\xhat})\\
|
|
ahe & =ae^{\xhat}h\\
|
|
e^{\yhat} & =h^{-1}e^{\xhat}h=\exp\left(\Ad{h^{-1}}\xhat\right)\\
|
|
\yhat & =\Ad{h^{-1}}\xhat
|
|
\end{align*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
In the case of 3D rotations, right multiplication with a constant rotation
|
|
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
is done through the mapping
|
|
\begin_inset Formula $\varphi(A)=AR$
|
|
\end_inset
|
|
|
|
, and satisfies
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{D\varphi_{A}x}=\Ad{R^{T}}\Skew x
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
For 3D rotations
|
|
\begin_inset Formula $R\in\SOthree$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula
|
|
\[
|
|
Ad_{R^{T}}(\hat{\omega})=R^{T}\hat{\omega}R=\Skew{R^{T}\omega}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and hence the Jacobian matrix of
|
|
\begin_inset Formula $\varphi$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $A$
|
|
\end_inset
|
|
|
|
is
|
|
\begin_inset Formula $\varphi'(A)=R^{T}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Pushforward of Compose
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
If we define the mapping
|
|
\begin_inset Formula $\varphi:G\times G\rightarrow G$
|
|
\end_inset
|
|
|
|
as the product of two group elements
|
|
\begin_inset Formula $g,h\in G$
|
|
\end_inset
|
|
|
|
, i.e.,
|
|
\begin_inset Formula $\varphi(g,h)=gh$
|
|
\end_inset
|
|
|
|
, then the pushforward will satisfy
|
|
\begin_inset Formula
|
|
\[
|
|
D\varphi_{(a,b)}(x,y)=D_{1}\varphi_{(a,b)}x+D_{2}\varphi_{(a,b)}y
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula
|
|
\[
|
|
\left(D_{1}\varphi_{(a,b)}x\right)\hat{}=\Ad{b^{-1}}\xhat\mbox{\;\ and\;}D_{2}\varphi_{(a,b)}y=y
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
Looking at the first argument, the proof is very similar to right multiplication
|
|
with a constant
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
.
|
|
Indeed, defining
|
|
\begin_inset Formula $y=D\varphi_{a}x$
|
|
\end_inset
|
|
|
|
as in
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:pushforward"
|
|
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula
|
|
\begin{align}
|
|
\varphi(a,b)e^{\yhat} & =\varphi(ae^{\xhat},b)\nonumber \\
|
|
abe^{\yhat} & =ae^{\xhat}b\nonumber \\
|
|
e^{\yhat} & =b^{-1}e^{\xhat}b=\exp\left(\Ad{b^{-1}}\xhat\right)\nonumber \\
|
|
\yhat & =\Ad{b^{-1}}\xhat\label{eq:Dcompose1}
|
|
\end{align}
|
|
|
|
\end_inset
|
|
|
|
In other words, to apply an incremental change
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
we first need to undo
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
, then apply
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
, and then apply
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
again.
|
|
Using
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:Adjoint2"
|
|
|
|
\end_inset
|
|
|
|
this can be done in one step by simply applying
|
|
\begin_inset Formula $\Ad{b^{-1}}\xhat$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
The second argument is quite a bit easier and simply yields the identity
|
|
mapping:
|
|
\begin_inset Formula
|
|
\begin{align}
|
|
\varphi(a,b)e^{\yhat} & =\varphi(a,be^{\xhat})\nonumber \\
|
|
abe^{\yhat} & =abe^{\xhat}\nonumber \\
|
|
y & =x\label{eq:Dcompose2}
|
|
\end{align}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Note Note
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
In summary, the Jacobian matrix of
|
|
\begin_inset Formula $\varphi(g,h)=gh$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $(a,b)\in G\times G$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula
|
|
\[
|
|
\varphi'(a,b)=?
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For 3D rotations
|
|
\begin_inset Formula $A,B\in\SOthree$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula $\varphi(A,B)=AB$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\Ad{B^{T}}\Skew{\omega}=\Skew{B^{T}\omega}$
|
|
\end_inset
|
|
|
|
, hence the Jacobian matrix
|
|
\begin_inset Formula $\varphi'(A,B)$
|
|
\end_inset
|
|
|
|
of composing two rotations is given by
|
|
\begin_inset Formula
|
|
\[
|
|
\varphi'(A,B)=\left[\begin{array}{cc}
|
|
B^{T} & I_{3}\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "sub:Pushforward-of-Between"
|
|
|
|
\end_inset
|
|
|
|
Pushforward of Between
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Finally, let us find the pushforward of
|
|
\series bold
|
|
between
|
|
\series default
|
|
, defined as
|
|
\begin_inset Formula $\varphi(g,h)=g^{-1}h$
|
|
\end_inset
|
|
|
|
.
|
|
For the first argument we reason as:
|
|
\begin_inset Formula
|
|
\begin{align}
|
|
\varphi(g,h)e^{\yhat} & =\varphi(ge^{\xhat},h)\nonumber \\
|
|
g^{-1}he^{\yhat} & =\left(ge^{\xhat}\right)^{-1}h=-e^{\xhat}g^{-1}h\nonumber \\
|
|
e^{\yhat} & =-\left(h^{-1}g\right)e^{\xhat}\left(h^{-1}g\right)^{-1}=-\exp\Ad{\left(h^{-1}g\right)}\xhat\nonumber \\
|
|
\yhat & =-\Ad{\left(h^{-1}g\right)}\xhat=-\Ad{\varphi\left(h,g\right)}\xhat\label{eq:Dbetween1}
|
|
\end{align}
|
|
|
|
\end_inset
|
|
|
|
The second argument yields the identity mapping.
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For 3D rotations
|
|
\begin_inset Formula $A,B\in\SOthree$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula $\varphi(A,B)=A^{T}B$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\Ad{B^{T}A}\Skew{-\omega}=\Skew{-B^{T}A\omega}$
|
|
\end_inset
|
|
|
|
, hence the Jacobian matrix
|
|
\begin_inset Formula $\varphi'(A,B)$
|
|
\end_inset
|
|
|
|
of between is given by
|
|
\begin_inset Formula
|
|
\[
|
|
\varphi'(A,B)=\left[\begin{array}{cc}
|
|
\left(-B^{T}A\right) & I_{3}\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Numerical PushForward
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Let's examine
|
|
\begin_inset Formula
|
|
\[
|
|
f\left(g\right)e^{\yhat}=f\left(ge^{\xhat}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and multiply with
|
|
\begin_inset Formula $f(g)^{-1}$
|
|
\end_inset
|
|
|
|
on both sides:
|
|
\begin_inset Formula
|
|
\[
|
|
e^{\yhat}=f\left(g\right)^{-1}f\left(ge^{\xhat}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We then take the log (which in our case returns
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
, not
|
|
\begin_inset Formula $\yhat$
|
|
\end_inset
|
|
|
|
):
|
|
\begin_inset Formula
|
|
\[
|
|
y(x)=\log\left[f\left(g\right)^{-1}f\left(ge^{\xhat}\right)\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Let us look at
|
|
\begin_inset Formula $x=0$
|
|
\end_inset
|
|
|
|
, and perturb in direction
|
|
\begin_inset Formula $i$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $e_{i}=[0,0,1,0,0]$
|
|
\end_inset
|
|
|
|
.
|
|
Then take derivative,
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{y(d)}d\define\lim_{d\rightarrow0}\frac{y(d)-y(0)}{d}=\lim_{d\rightarrow0}\frac{1}{d}\log\left[f\left(g\right)^{-1}f\left(ge^{\widehat{de_{i}}}\right)\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which is the basis for a numerical derivative scheme.
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
Not understood yet: Let us also look at a chain rule.
|
|
If we know the behavior at the origin
|
|
\begin_inset Formula $I$
|
|
\end_inset
|
|
|
|
, we can extrapolate
|
|
\begin_inset Formula
|
|
\[
|
|
f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivative of the Exponential and Logarithm Map
|
|
\end_layout
|
|
|
|
\begin_layout Theorem
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "D-exp"
|
|
|
|
\end_inset
|
|
|
|
The derivative of the function
|
|
\begin_inset Formula $f:\Reals n\rightarrow G$
|
|
\end_inset
|
|
|
|
that applies the wedge operator followed by the exponential map, i.e.,
|
|
\begin_inset Formula $f(\xi)=\exp\xihat$
|
|
\end_inset
|
|
|
|
, is the identity map for
|
|
\begin_inset Formula $\xi=0$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
For
|
|
\begin_inset Formula $\xi=0$
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
f(\xi)e^{\yhat} & = & f(\xi+x)\\
|
|
f(0)e^{\yhat} & = & f(0+x)\\
|
|
e^{\yhat} & = & e^{\xhat}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Corollary
|
|
The derivative of the inverse
|
|
\begin_inset Formula $f^{-1}$
|
|
\end_inset
|
|
|
|
is the identity as well, i.e., for
|
|
\begin_inset Formula $T=e$
|
|
\end_inset
|
|
|
|
, the identity element in
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For
|
|
\begin_inset Formula $\xi\neq0$
|
|
\end_inset
|
|
|
|
, things are not simple, see .
|
|
|
|
\begin_inset Flex URL
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti
|
|
al-of-the-exponential/
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
General Manifolds
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Retractions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\retract}{\mathcal{R}}
|
|
{\mathcal{R}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
General manifolds that are not Lie groups do not have an exponential map,
|
|
but can still be handled by defining a
|
|
\series bold
|
|
retraction
|
|
\series default
|
|
|
|
\begin_inset Formula $\retract:\Man\times\Reals n\rightarrow\Man$
|
|
\end_inset
|
|
|
|
, such that
|
|
\begin_inset Formula
|
|
\[
|
|
a\oplus\xi\define\retract_{a}\left(\xi\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
A retraction
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Absil07book"
|
|
|
|
\end_inset
|
|
|
|
is required to be tangent to geodesics on the manifold
|
|
\begin_inset Formula $\Man$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
We can define many retractions for a manifold
|
|
\begin_inset Formula $\Man$
|
|
\end_inset
|
|
|
|
, even for those with more structure.
|
|
For the vector space
|
|
\begin_inset Formula $\Reals n$
|
|
\end_inset
|
|
|
|
the retraction is just vector addition, and for Lie groups the obvious
|
|
retraction is simply the exponential map, i.e.,
|
|
\begin_inset Formula $\retract_{a}(\xi)=a\cdot\exp\xihat$
|
|
\end_inset
|
|
|
|
.
|
|
However, one can choose other, possibly computationally attractive retractions,
|
|
as long as around a they agree with the geodesic induced by the exponential
|
|
map, i.e.,
|
|
\begin_inset Formula
|
|
\[
|
|
\lim_{\xi\rightarrow0}\frac{\left|a\cdot\exp\xihat-\retract_{a}\left(\xi\right)\right|}{\left|\xi\right|}=0
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
For
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
, instead of using the true exponential map it is computationally more efficient
|
|
to define the retraction, which uses a first order approximation of the
|
|
translation update
|
|
\begin_inset Formula
|
|
\[
|
|
\retract_{T}\left(\left[\begin{array}{c}
|
|
\omega\\
|
|
v
|
|
\end{array}\right]\right)=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
e^{\Skew{\omega}} & v\\
|
|
0 & 1
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
Re^{\Skew{\omega}} & t+Rv\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Equipped with a retraction, then, we can generalize the notion of a derivative
|
|
for functions
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
from general a manifold
|
|
\begin_inset Formula $\Man$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $\Reals m$
|
|
\end_inset
|
|
|
|
:
|
|
\end_layout
|
|
|
|
\begin_layout Definition
|
|
We define a function
|
|
\begin_inset Formula $f:\Man\rightarrow\Reals m$
|
|
\end_inset
|
|
|
|
to be
|
|
\series bold
|
|
differentiable
|
|
\series default
|
|
at
|
|
\begin_inset Formula $a\in\Man$
|
|
\end_inset
|
|
|
|
if there exists a matrix
|
|
\begin_inset Formula $f'(a)$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula
|
|
\[
|
|
\lim_{\xi\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f\left(\retract_{a}(\xi)\right)\right|}{\left|\xi\right|}=0
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\xi\in\Reals n$
|
|
\end_inset
|
|
|
|
for an
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional manifold, and
|
|
\begin_inset Formula $\retract_{a}:\Reals n\rightarrow\Man$
|
|
\end_inset
|
|
|
|
a retraction
|
|
\begin_inset Formula $\retract$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
If
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
is differentiable, then
|
|
\begin_inset Formula $f'(a)$
|
|
\end_inset
|
|
|
|
is called the
|
|
\series bold
|
|
Jacobian matrix
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
, and the linear transformation
|
|
\begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$
|
|
\end_inset
|
|
|
|
is called the
|
|
\series bold
|
|
derivative
|
|
\series default
|
|
of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Definition
|
|
For manifolds that are also Lie groups, the derivative of any function
|
|
\begin_inset Formula $f:G\rightarrow\Reals m$
|
|
\end_inset
|
|
|
|
will agree no matter what retraction
|
|
\begin_inset Formula $\retract$
|
|
\end_inset
|
|
|
|
is used.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Part
|
|
Practice
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Below we apply the results derived in the theory part to the geometric objects
|
|
we use in GTSAM.
|
|
Above we preferred the modern notation
|
|
\begin_inset Formula $D_{1}f$
|
|
\end_inset
|
|
|
|
for the partial derivative.
|
|
Below (because this was written earlier) we use the more classical notation
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{f(x,y)}x
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
In addition, for Lie groups we will abuse the notation and take
|
|
\begin_inset Formula
|
|
\[
|
|
\at{\deriv{\varphi(g)}{\xi}}a
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
to be the Jacobian matrix
|
|
\begin_inset Formula $\varphi'($
|
|
\end_inset
|
|
|
|
a) of the mapping
|
|
\begin_inset Formula $\varphi$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $a\in G$
|
|
\end_inset
|
|
|
|
, associated with the pushforward
|
|
\begin_inset Formula $D\varphi_{a}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
SLAM Example
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Let us examine a visual SLAM example.
|
|
We have 2D measurements
|
|
\begin_inset Formula $z_{ij}$
|
|
\end_inset
|
|
|
|
, where each measurement is predicted by
|
|
\begin_inset Formula
|
|
\[
|
|
z_{ij}=h(T_{i},p_{j})=\pi(T_{i}^{-1}p_{j})
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $T_{i}$
|
|
\end_inset
|
|
|
|
is the 3D pose of the
|
|
\begin_inset Formula $i^{th}$
|
|
\end_inset
|
|
|
|
camera,
|
|
\begin_inset Formula $p_{j}$
|
|
\end_inset
|
|
|
|
is the location of the
|
|
\begin_inset Formula $j^{th}$
|
|
\end_inset
|
|
|
|
point, and
|
|
\begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$
|
|
\end_inset
|
|
|
|
is the camera projection function from Example
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "ex:projection"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
BetweenFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\series bold
|
|
\emph on
|
|
BetweenFactor
|
|
\series default
|
|
\emph default
|
|
is a factor in GTSAM that is used ubiquitously to process measurements
|
|
indicating the relative pose between two unknown poses
|
|
\begin_inset Formula $T_{1}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $T_{2}$
|
|
\end_inset
|
|
|
|
.
|
|
Let us assume the measured relative pose is
|
|
\begin_inset Formula $Z$
|
|
\end_inset
|
|
|
|
, then the code that calculates the error in
|
|
\series bold
|
|
\emph on
|
|
BetweenFactor
|
|
\series default
|
|
\emph default
|
|
first calculates the predicted relative pose
|
|
\begin_inset Formula $T_{12}$
|
|
\end_inset
|
|
|
|
, and then evaluates the error between the measured and predicted relative
|
|
pose:
|
|
\end_layout
|
|
|
|
\begin_layout LyX-Code
|
|
T12 = between(T1, T2);
|
|
\end_layout
|
|
|
|
\begin_layout LyX-Code
|
|
return localCoordinates(Z, T12);
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
where we recall that the function
|
|
\series bold
|
|
\emph on
|
|
between
|
|
\series default
|
|
\emph default
|
|
is given in group theoretic notation as
|
|
\begin_inset Formula
|
|
\[
|
|
\varphi(g,h)=g^{-1}h
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The function
|
|
\series bold
|
|
\emph on
|
|
localCoordinates
|
|
\series default
|
|
\emph default
|
|
itself also calls
|
|
\series bold
|
|
\emph on
|
|
between
|
|
\series default
|
|
\emph default
|
|
, and converts to canonical coordinates:
|
|
\end_layout
|
|
|
|
\begin_layout LyX-Code
|
|
localCoordinates(Z,T12) = Logmap(between(Z, T12));
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Hence, given two elements
|
|
\begin_inset Formula $T_{1}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $T_{2}$
|
|
\end_inset
|
|
|
|
,
|
|
\series bold
|
|
\emph on
|
|
BetweenFactor
|
|
\series default
|
|
\emph default
|
|
evaluates
|
|
\begin_inset Formula $g:G\times G\rightarrow\Reals n$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
g(T_{1},T_{2};Z)=f^{-1}\left(\varphi(Z,\varphi(T_{1},T_{2})\right)=f^{-1}\left(Z^{-1}\left(T_{1}^{-1}T_{2}\right)\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $f^{-1}$
|
|
\end_inset
|
|
|
|
is the inverse of the map
|
|
\begin_inset Formula $f:\xi\mapsto\exp\xihat$
|
|
\end_inset
|
|
|
|
.
|
|
If we assume that the measurement has only small error, then
|
|
\begin_inset Formula $Z\approx T_{1}^{-1}T_{2}$
|
|
\end_inset
|
|
|
|
, and hence we have
|
|
\begin_inset Formula $Z^{-1}T_{1}^{-1}T_{2}\approx e$
|
|
\end_inset
|
|
|
|
, and we can invoke Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "D-exp"
|
|
|
|
\end_inset
|
|
|
|
, which says that the derivative of the exponential map
|
|
\begin_inset Formula $f:\xi\mapsto\exp\xihat$
|
|
\end_inset
|
|
|
|
is identity at
|
|
\begin_inset Formula $\xi=0$
|
|
\end_inset
|
|
|
|
, as well as its inverse.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Finally, because the derivative of
|
|
\series bold
|
|
\emph on
|
|
between
|
|
\series default
|
|
\emph default
|
|
is identity in its second argument, the derivative of the
|
|
\series bold
|
|
\emph on
|
|
BetweenFactor
|
|
\series default
|
|
\emph default
|
|
error is identical to the derivative of pushforward of
|
|
\begin_inset Formula $\varphi(T_{1},T_{2})$
|
|
\end_inset
|
|
|
|
, derived in Section
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "sub:Pushforward-of-Between"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Point3
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A cross product
|
|
\begin_inset Formula $a\times b$
|
|
\end_inset
|
|
|
|
can be written as a matrix multiplication
|
|
\begin_inset Formula
|
|
\[
|
|
a\times b=\Skew ab
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Skew a$
|
|
\end_inset
|
|
|
|
is a skew-symmetric matrix defined as
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{x,y,z}=\left[\begin{array}{ccc}
|
|
0 & -z & y\\
|
|
z & 0 & -x\\
|
|
-y & x & 0
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We also have
|
|
\begin_inset Formula
|
|
\[
|
|
a^{T}\Skew b=-(\Skew ba)^{T}=-(a\times b)^{T}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The derivative of a cross product
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\frac{\partial(a\times b)}{\partial a}=\Skew{-b}\label{eq:Dcross1}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\frac{\partial(a\times b)}{\partial b}=\Skew a\label{eq:Dcross2}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
2D Rotations
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Rot2 in GTSAM
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A rotation is stored as
|
|
\begin_inset Formula $(\cos\theta,\sin\theta)$
|
|
\end_inset
|
|
|
|
.
|
|
An incremental rotation is applied using the trigonometric sum rule:
|
|
\begin_inset Formula
|
|
\[
|
|
\cos\theta'=\cos\theta\cos\delta-\sin\theta\sin\delta
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\sin\theta'=\sin\theta\cos\delta+\cos\theta\sin\delta
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
is an incremental rotation angle.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In the case of
|
|
\begin_inset Formula $\SOtwo$
|
|
\end_inset
|
|
|
|
the vector space is
|
|
\begin_inset Formula $\Rtwo$
|
|
\end_inset
|
|
|
|
, and the group action
|
|
\begin_inset Formula $f(R,p)$
|
|
\end_inset
|
|
|
|
corresponds to rotating the 2D point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
f(R,p)=Rp
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
According to Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "th:Action"
|
|
|
|
\end_inset
|
|
|
|
, the Jacobian matrix of
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula
|
|
\[
|
|
f'(R,p)=\left[\begin{array}{cc}
|
|
RH(p) & R\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $H:\Reals 2\rightarrow\Reals{2\times2}$
|
|
\end_inset
|
|
|
|
a linear mapping that depends on
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
.
|
|
In the case of
|
|
\begin_inset Formula $\SOtwo$
|
|
\end_inset
|
|
|
|
, we can find
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
by equating (as in Equation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Hp"
|
|
|
|
\end_inset
|
|
|
|
):
|
|
\begin_inset Formula
|
|
\[
|
|
\skew wp=\left[\begin{array}{cc}
|
|
0 & -\omega\\
|
|
\omega & 0
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
x\\
|
|
y
|
|
\end{array}\right]=\left[\begin{array}{c}
|
|
-y\\
|
|
x
|
|
\end{array}\right]\omega=H(p)\omega
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Note that
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
H(p)=\left[\begin{array}{c}
|
|
-y\\
|
|
x
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
0 & -1\\
|
|
1 & 0
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
x\\
|
|
y
|
|
\end{array}\right]=R_{\pi/2}p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and since 2D rotations commute, we also have, with
|
|
\begin_inset Formula $q=Rp$
|
|
\end_inset
|
|
|
|
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
f'(R,p)=\left[\begin{array}{cc}
|
|
R\left(R_{\pi/2}p\right) & R\end{array}\right]=\left[\begin{array}{cc}
|
|
R_{\pi/2}q & R\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Pushforwards of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Since
|
|
\begin_inset Formula $\Ad R\skew{\omega}=\skew{\omega}$
|
|
\end_inset
|
|
|
|
, we have the derivative of
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial R^{T}}{\partial\omega}=-\Ad R=-1\mbox{ }
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
compose,
|
|
\series default
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}}=\Ad{R_{2}^{T}}=1\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=1
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\series bold
|
|
between:
|
|
\series default
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}}=-\Ad{R_{2}^{T}R_{1}}=-1\mbox{ and }\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{2}}=1
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
2D Rigid Transformations
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
The derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The action of
|
|
\begin_inset Formula $\SEtwo$
|
|
\end_inset
|
|
|
|
on 2D points is done by embedding the points in
|
|
\begin_inset Formula $\mathbb{R}^{3}$
|
|
\end_inset
|
|
|
|
by using homogeneous coordinates
|
|
\begin_inset Formula
|
|
\[
|
|
f(T,p)=\hat{q}=\left[\begin{array}{c}
|
|
q\\
|
|
1
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
p\\
|
|
1
|
|
\end{array}\right]=T\hat{p}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
To find the derivative, we write the quantity
|
|
\begin_inset Formula $\xihat\hat{p}$
|
|
\end_inset
|
|
|
|
as the product of the
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
matrix
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\xihat\hat{p}=\left[\begin{array}{cc}
|
|
\skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
p\\
|
|
1
|
|
\end{array}\right]=\left[\begin{array}{c}
|
|
\skew{\omega}p+v\\
|
|
0
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
I_{2} & R_{\pi/2}p\\
|
|
0 & 0
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
v\\
|
|
\omega
|
|
\end{array}\right]=H(p)\xi\label{eq:HpSE2}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
Hence, by Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "th:Action"
|
|
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\deriv{\left(T\hat{p}\right)}{\xi}=TH(p)=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
I_{2} & R_{\pi/2}p\\
|
|
0 & 0
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
R & RR_{\pi/2}p\\
|
|
0 & 0
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
R & R_{\pi/2}q\\
|
|
0 & 0
|
|
\end{array}\right]\label{eq:SE2Action}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
Note that, looking only at the top rows of
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:HpSE2"
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:SE2Action"
|
|
|
|
\end_inset
|
|
|
|
, we can recognize the quantity
|
|
\begin_inset Formula $\skew{\omega}p+v=v+\omega\left(R_{\pi/2}p\right)$
|
|
\end_inset
|
|
|
|
as the velocity of
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $\Rtwo$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $\left[\begin{array}{cc}
|
|
R & R_{\pi/2}q\end{array}\right]$
|
|
\end_inset
|
|
|
|
is the derivative of the action on
|
|
\begin_inset Formula $\Rtwo$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The derivative of the inverse action
|
|
\begin_inset Formula $g(T,p)=T^{-1}\hat{p}$
|
|
\end_inset
|
|
|
|
is given by Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "Th:InverseAction"
|
|
|
|
\end_inset
|
|
|
|
specialized to
|
|
\begin_inset Formula $\SEtwo$
|
|
\end_inset
|
|
|
|
:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(T^{-1}\hat{p}\right)}{\xi}=-H(T^{-1}p)=\left[\begin{array}{cc}
|
|
-I_{2} & -R_{\pi/2}\left(T^{-1}p\right)\\
|
|
0 & 0
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Pushforwards of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can just define all derivatives in terms of the adjoint map, which in
|
|
the case of
|
|
\begin_inset Formula $\SEtwo$
|
|
\end_inset
|
|
|
|
, in twist coordinates, is the linear mapping
|
|
\begin_inset Formula
|
|
\[
|
|
\Ad T\xi=\left[\begin{array}{cc}
|
|
R & -R_{\pi/2}t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
v\\
|
|
\omega
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and we have
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\frac{\partial T^{^{-1}}}{\partial\xi} & = & -\Ad T
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}} & = & \Ad{T_{2}^{^{-1}}}\mbox{ and }\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}}=I_{3}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}^{-1}T_{2}\right)}{\partial\xi_{1}} & = & -\Ad{T_{2}^{^{-1}}T_{1}}=-\Ad{between(T_{2},T_{1})}\mbox{ and }\frac{\partial\left(T_{1}^{-1}T_{2}\right)}{\partial\xi_{2}}=I_{3}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
3D Rotations
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In the case of
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
the vector space is
|
|
\begin_inset Formula $\Rthree$
|
|
\end_inset
|
|
|
|
, and the group action
|
|
\begin_inset Formula $f(R,p)$
|
|
\end_inset
|
|
|
|
corresponds to rotating a point
|
|
\begin_inset Formula
|
|
\[
|
|
q=f(R,p)=Rp
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
To calculate
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
for use in Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "th:Action"
|
|
|
|
\end_inset
|
|
|
|
we make use of
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega}p=\omega\times p=-p\times\omega=\Skew{-p}\omega
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
so
|
|
\begin_inset Formula $H(p)\define\Skew{-p}$
|
|
\end_inset
|
|
|
|
.
|
|
Hence, the final derivative of an action in its first argument is
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p\label{eq:Rot3action}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
Likewise, according to Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "Th:InverseAction"
|
|
|
|
\end_inset
|
|
|
|
, the derivative of the inverse action is given by
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(R^{T}p\right)}{\omega}=-H(R^{T}p)=\Skew{R^{T}p}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "sub:3DAngularVelocities"
|
|
|
|
\end_inset
|
|
|
|
Instantaneous Velocity
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For 3D rotations
|
|
\begin_inset Formula $R_{b}^{n}$
|
|
\end_inset
|
|
|
|
from a body frame
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
to a navigation frame
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
we have the spatial angular velocity
|
|
\begin_inset Formula $\omega_{nb}^{n}$
|
|
\end_inset
|
|
|
|
measured in the navigation frame,
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega_{nb}^{n}}\define\dot{R}_{b}^{n}\left(R_{b}^{n}\right)^{T}=\dot{R}_{b}^{n}R_{n}^{b}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the body angular velocity
|
|
\begin_inset Formula $\omega_{nb}^{b}$
|
|
\end_inset
|
|
|
|
measured in the body frame:
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega_{nb}^{b}}\define\left(R_{b}^{n}\right)^{T}\dot{R}_{b}^{n}=R_{n}^{b}\dot{R}_{b}^{n}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
These quantities can be used to derive the velocity of a point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
, and we choose between spatial or body angular velocity depending on the
|
|
frame in which we choose to represent
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
v^{n}=\Skew{\omega_{nb}^{n}}p^{n}=\omega_{nb}^{n}\times p^{n}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
v^{b}=\Skew{\omega_{nb}^{b}}p^{b}=\omega_{nb}^{b}\times p^{b}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We can transform these skew-symmetric matrices from navigation to body frame
|
|
by conjugating,
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega_{nb}^{b}}=R_{n}^{b}\Skew{\omega_{nb}^{n}}R_{b}^{n}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
but because the adjoint representation satisfies
|
|
\begin_inset Formula
|
|
\[
|
|
Ad_{R}\Skew{\omega}\define R\Skew{\omega}R^{T}=\Skew{R\omega}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
we can even more easily transform between spatial and body angular velocities
|
|
as 3-vectors:
|
|
\begin_inset Formula
|
|
\[
|
|
\omega_{nb}^{b}=R_{n}^{b}\omega_{nb}^{n}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Pushforwards of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula $\Ad R\Skew{\omega}=\Skew{R\omega}$
|
|
\end_inset
|
|
|
|
and, in terms of angular velocities:
|
|
\begin_inset Formula $\Ad R\omega=R\omega$
|
|
\end_inset
|
|
|
|
.
|
|
Hence, the Jacobian matrix of the
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
mapping is (see Equation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Dinverse"
|
|
|
|
\end_inset
|
|
|
|
)
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial R^{T}}{\partial\omega}=-\Ad R=-R
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
for
|
|
\series bold
|
|
compose
|
|
\series default
|
|
we have (Equations
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Dcompose1"
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Dcompose2"
|
|
|
|
\end_inset
|
|
|
|
):
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}}=R_{2}^{T}\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=I_{3}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and
|
|
\series bold
|
|
between
|
|
\series default
|
|
(Equation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Dbetween1"
|
|
|
|
\end_inset
|
|
|
|
):
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}}=-R_{2}^{T}R_{1}=-between(R_{2},R_{1})\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=I_{3}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Retractions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Absil
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "page 58"
|
|
key "Absil07book"
|
|
|
|
\end_inset
|
|
|
|
discusses two possible retractions for
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
based on the QR decomposition or the polar decomposition of the matrix
|
|
|
|
\begin_inset Formula $R\Skew{\omega}$
|
|
\end_inset
|
|
|
|
, but they are expensive.
|
|
Another retraction is based on the Cayley transform
|
|
\begin_inset Formula $\mathcal{C}:\sothree\rightarrow\SOthree$
|
|
\end_inset
|
|
|
|
, a mapping from the skew-symmetric matrices to rotation matrices:
|
|
\begin_inset Formula
|
|
\[
|
|
Q=\mathcal{C}(\Omega)=(I-\Omega)(I+\Omega)^{-1}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Interestingly, the inverse Cayley transform
|
|
\begin_inset Formula $\mathcal{C}^{-1}:\SOthree\rightarrow\sothree$
|
|
\end_inset
|
|
|
|
has the same form:
|
|
\begin_inset Formula
|
|
\[
|
|
\Omega=\mathcal{C}^{-1}(Q)=(I-Q)(I+Q)^{-1}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The retraction needs a factor
|
|
\begin_inset Formula $-\frac{1}{2}$
|
|
\end_inset
|
|
|
|
however, to make it locally align with a geodesic:
|
|
\begin_inset Formula
|
|
\[
|
|
R'=\retract_{R}(\omega)=R\mathcal{C}(-\frac{1}{2}\Skew{\omega})
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Note that given
|
|
\begin_inset Formula $\omega=(x,y,z)$
|
|
\end_inset
|
|
|
|
this has the closed-form expression below
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{1}{4+x^{2}+y^{2}+z^{2}}\left[\begin{array}{ccc}
|
|
4+x^{2}-y^{2}-z^{2} & 2xy-4z & 2xz+4y\\
|
|
2xy+4z & 4-x^{2}+y^{2}-z^{2} & 2yz-4x\\
|
|
2xz-4y & 2yz+4x & 4-x^{2}-y^{2}+z^{2}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
=\frac{1}{4+x^{2}+y^{2}+z^{2}}\left\{ 4(I+\Skew{\omega})+\left[\begin{array}{ccc}
|
|
x^{2}-y^{2}-z^{2} & 2xy & 2xz\\
|
|
2xy & -x^{2}+y^{2}-z^{2} & 2yz\\
|
|
2xz & 2yz & -x^{2}-y^{2}+z^{2}
|
|
\end{array}\right]\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
so it can be seen to be a second-order correction on
|
|
\begin_inset Formula $(I+\Skew{\omega})$
|
|
\end_inset
|
|
|
|
.
|
|
The corresponding approximation to the logarithmic map is:
|
|
\begin_inset Formula
|
|
\[
|
|
\Skew{\omega}=\retract_{R}^{-1}(R')=-2\mathcal{C}^{-1}\left(R^{T}R'\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
3D Rigid Transformations
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
The derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The action of
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
on 3D points is done by embedding the points in
|
|
\begin_inset Formula $\mathbb{R}^{4}$
|
|
\end_inset
|
|
|
|
by using homogeneous coordinates
|
|
\begin_inset Formula
|
|
\[
|
|
\hat{q}=\left[\begin{array}{c}
|
|
q\\
|
|
1
|
|
\end{array}\right]=f(T,p)=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
p\\
|
|
1
|
|
\end{array}\right]=T\hat{p}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The quantity
|
|
\begin_inset Formula $\xihat\hat{p}$
|
|
\end_inset
|
|
|
|
corresponds to a velocity in
|
|
\begin_inset Formula $\mathbb{R}^{4}$
|
|
\end_inset
|
|
|
|
(in the local
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
frame), and equating it to
|
|
\begin_inset Formula $H(p)\xi$
|
|
\end_inset
|
|
|
|
as in Equation
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Hp"
|
|
|
|
\end_inset
|
|
|
|
yields the
|
|
\begin_inset Formula $4\times6$
|
|
\end_inset
|
|
|
|
matrix
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Foot
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
can also be obtained by taking the
|
|
\begin_inset Formula $j^{th}$
|
|
\end_inset
|
|
|
|
column of each of the 6 generators to multiply with components of
|
|
\begin_inset Formula $\hat{p}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
\xihat\hat{p}=\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
p\\
|
|
1
|
|
\end{array}\right]=\left[\begin{array}{c}
|
|
\omega\times p+v\\
|
|
0
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
\Skew{-p} & I_{3}\\
|
|
0 & 0
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
\omega\\
|
|
v
|
|
\end{array}\right]=H(p)\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Note how velocities are analogous to points at infinity in projective geometry:
|
|
they correspond to free vectors indicating a direction and magnitude of
|
|
change.
|
|
According to Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "th:Action"
|
|
|
|
\end_inset
|
|
|
|
, the derivative of the group action is then
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(T\hat{p}\right)}{\xi}=TH(p)=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
\Skew{-p} & I_{3}\\
|
|
0 & 0
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
R\Skew{-p} & R\\
|
|
0 & 0
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(T\hat{p}\right)}{\hat{p}}=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
in homogenous coordinates.
|
|
In
|
|
\begin_inset Formula $\Rthree$
|
|
\end_inset
|
|
|
|
this becomes
|
|
\begin_inset Formula $R\left[\begin{array}{cc}
|
|
-\Skew p & I_{3}\end{array}\right]$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The derivative of the inverse action
|
|
\begin_inset Formula $T^{-1}p$
|
|
\end_inset
|
|
|
|
is given by Theorem
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "Th:InverseAction"
|
|
|
|
\end_inset
|
|
|
|
:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(T^{-1}\hat{p}\right)}{\xi}=-H\left(T^{-1}\hat{p}\right)=\left[\begin{array}{cc}
|
|
\Skew{T^{-1}\hat{p}} & -I_{3}\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{\left(T^{-1}\hat{p}\right)}{\hat{p}}=\left[\begin{array}{cc}
|
|
R^{T} & -R^{T}t\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Example
|
|
Let us examine a visual SLAM example.
|
|
We have 2D measurements
|
|
\begin_inset Formula $z_{ij}$
|
|
\end_inset
|
|
|
|
, where each measurement is predicted by
|
|
\begin_inset Formula
|
|
\[
|
|
z_{ij}=h(T_{i},p_{j})=\pi(T_{i}^{-1}p_{j})=\pi(q)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $T_{i}$
|
|
\end_inset
|
|
|
|
is the 3D pose of the
|
|
\begin_inset Formula $i^{th}$
|
|
\end_inset
|
|
|
|
camera,
|
|
\begin_inset Formula $p_{j}$
|
|
\end_inset
|
|
|
|
is the location of the
|
|
\begin_inset Formula $j^{th}$
|
|
\end_inset
|
|
|
|
point,
|
|
\begin_inset Formula $q=(x',y',z')=T^{-1}p$
|
|
\end_inset
|
|
|
|
is the point in camera coordinates, and
|
|
\begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$
|
|
\end_inset
|
|
|
|
is the camera projection function from Example
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "ex:projection"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
By the chain rule, we then have
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{h(T,p)}{\xi}=\deriv{\pi(q)}q\deriv{(T^{-1}p)}{\xi}=\frac{1}{z'}\left[\begin{array}{ccc}
|
|
1 & 0 & -x'/z'\\
|
|
0 & 1 & -y'/z'
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
\Skew q & -I_{3}\end{array}\right]=\left[\begin{array}{cc}
|
|
\pi'(q)\Skew q & -\pi'(q)\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\deriv{h(T,p)}p=\pi'(q)R^{T}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Instantaneous Velocity
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For rigid 3D transformations
|
|
\begin_inset Formula $T_{b}^{n}$
|
|
\end_inset
|
|
|
|
from a body frame
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
to a navigation frame
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
we have the instantaneous spatial twist
|
|
\begin_inset Formula $\xi_{nb}^{n}$
|
|
\end_inset
|
|
|
|
measured in the navigation frame,
|
|
\begin_inset Formula
|
|
\[
|
|
\hat{\xi}_{nb}^{n}\define\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the instantaneous body twist
|
|
\begin_inset Formula $\xi_{nb}^{b}$
|
|
\end_inset
|
|
|
|
measured in the body frame:
|
|
\begin_inset Formula
|
|
\[
|
|
\hat{\xi}_{nb}^{b}\define\left(T_{b}^{n}\right)^{T}\dot{T}_{b}^{n}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Pushforwards of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
As we can express the Adjoint representation in terms of twist coordinates,
|
|
we have
|
|
\begin_inset Formula
|
|
\[
|
|
\left[\begin{array}{c}
|
|
\omega'\\
|
|
v'
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
R & 0\\
|
|
\Skew tR & R
|
|
\end{array}\right]\left[\begin{array}{c}
|
|
\omega\\
|
|
v
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Hence, as with
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
, we are now in a position to simply posit the derivative of
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial T^{-1}}{\partial\xi}=-\Ad T=-\left[\begin{array}{cc}
|
|
R & 0\\
|
|
\Skew tR & R
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
compose
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}}=\Ad{T_{2}^{-1}}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
in its second argument,
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}}=I_{6}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
between
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}}=-\Ad{T_{2}^{^{-1}}T_{1}}=\left[\begin{array}{cc}
|
|
-R_{2}^{T}R_{1} & 0\\
|
|
R_{2}^{T}\left[t_{2}-t_{1}\right]_{\times}R_{1} & -R_{2}^{T}R_{1}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and in its second argument,
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Retractions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
, instead of using the true exponential map it is computationally more efficient
|
|
to design other retractions.
|
|
A first-order approximation to the exponential map does not quite cut it,
|
|
as it yields a
|
|
\begin_inset Formula $4\times4$
|
|
\end_inset
|
|
|
|
matrix which is not in
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
T\exp\xihat & \approx & T(I+\xihat)\\
|
|
& = & T\left(I_{4}+\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]\right)\\
|
|
& = & \left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
I_{3}+\Skew{\omega} & v\\
|
|
0 & 1
|
|
\end{array}\right]\\
|
|
& = & \left[\begin{array}{cc}
|
|
R\left(I_{3}+\Skew{\omega}\right) & t+Rv\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
However, we can make it into a retraction by using any retraction defined
|
|
for
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
, including, as below, using the exponential map
|
|
\begin_inset Formula $Re^{\Skew{\omega}}$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
\retract_{T}\left(\left[\begin{array}{c}
|
|
\omega\\
|
|
v
|
|
\end{array}\right]\right)=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
e^{\Skew{\omega}} & v\\
|
|
0 & 1
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
Re^{\Skew{\omega}} & t+Rv\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Similarly, for a second order approximation we have
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
T\exp\xihat & \approx & T(I+\xihat+\frac{\xihat^{2}}{2})\\
|
|
& = & T\left(I_{4}+\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]+\frac{1}{2}\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0
|
|
\end{array}\right]\right)\\
|
|
& = & \left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left(\left[\begin{array}{cc}
|
|
I_{3}+\Skew{\omega}+\frac{1}{2}\Skew{\omega}^{2} & v+\frac{1}{2}\Skew{\omega}v\\
|
|
0 & 1
|
|
\end{array}\right]\right)\\
|
|
& = & \left[\begin{array}{cc}
|
|
R\left(I_{3}+\Skew{\omega}+\frac{1}{2}\Skew{\omega}^{2}\right) & t+R\left[v+\left(\omega\times v\right)/2\right]\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
inspiring the retraction
|
|
\begin_inset Formula
|
|
\[
|
|
\retract_{T}\left(\left[\begin{array}{c}
|
|
\omega\\
|
|
v
|
|
\end{array}\right]\right)=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1
|
|
\end{array}\right]\left[\begin{array}{cc}
|
|
e^{\Skew{\omega}} & v+\left(\omega\times v\right)/2\\
|
|
0 & 1
|
|
\end{array}\right]=\left[\begin{array}{cc}
|
|
Re^{\Skew{\omega}} & t+R\left[v+\left(\omega\times v\right)/2\right]\\
|
|
0 & 1
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Newpage pagebreak
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
The Sphere
|
|
\begin_inset Formula $S^{2}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Definitions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The sphere
|
|
\begin_inset Formula $S^{2}$
|
|
\end_inset
|
|
|
|
is the set of all unit vectors in
|
|
\begin_inset Formula $\Rthree$
|
|
\end_inset
|
|
|
|
, i.e., all directions in three-space:
|
|
\begin_inset Formula
|
|
\[
|
|
S^{2}=\{p\in\Rthree|\left\Vert p\right\Vert =1\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The tangent space
|
|
\begin_inset Formula $T_{p}S^{2}$
|
|
\end_inset
|
|
|
|
at a point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
consists of three-vectors
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
is tangent to
|
|
\begin_inset Formula $S^{2}$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
, i.e.,
|
|
\begin_inset Formula
|
|
\[
|
|
T_{p}S^{2}\define\left\{ \xihat\in\Rthree|p^{T}\xihat=0\right\}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
While not a Lie group, we can define an exponential map, which is given
|
|
in Ma et.
|
|
al
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Ma01ijcv"
|
|
|
|
\end_inset
|
|
|
|
, as well as in this CVPR tutorial by Anuj Srivastava:
|
|
\begin_inset CommandInset href
|
|
LatexCommand href
|
|
name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\exp_{p}\xihat=\cos\left(\left\Vert \xihat\right\Vert \right)p+\sin\left(\left\Vert \xihat\right\Vert \right)\frac{\xihat}{\left\Vert \xihat\right\Vert }
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The latter also gives the inverse, i.e., get the tangent vector
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
to go from
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
z=\log_{p}q=\frac{\theta}{\sin\theta}\left(q-p\cos\theta\right)p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\theta=\cos^{-1}\left(p^{T}q\right)$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Local Coordinates
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can find a basis
|
|
\begin_inset Formula $B_{p}$
|
|
\end_inset
|
|
|
|
for the tangent space
|
|
\begin_inset Formula $T_{p}S^{2}$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $B_{p}=\left[b_{1}|b_{2}\right]$
|
|
\end_inset
|
|
|
|
a
|
|
\begin_inset Formula $3\times2$
|
|
\end_inset
|
|
|
|
matrix, by either
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Decompose
|
|
\begin_inset Formula $p=QR$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $Q$
|
|
\end_inset
|
|
|
|
orthonormal and
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
of the form
|
|
\begin_inset Formula $[1\,0\,0]^{T}$
|
|
\end_inset
|
|
|
|
, and hence
|
|
\begin_inset Formula $p=Q_{1}$
|
|
\end_inset
|
|
|
|
.
|
|
The basis
|
|
\begin_inset Formula $B_{p}=\left[Q_{2}|Q_{3}\right]$
|
|
\end_inset
|
|
|
|
, i.e., the last two columns of
|
|
\begin_inset Formula $Q$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Form
|
|
\begin_inset Formula $b_{1}=p\times a$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
(consistently) chosen to be non-parallel to
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $b_{2}=p\times b_{1}$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
To choose
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
, one way is to divide the sphere into regions, e.g., pick the axis
|
|
\begin_inset Formula $e_{i}$
|
|
\end_inset
|
|
|
|
such that
|
|
\begin_inset Formula $e_{i}^{T}p$
|
|
\end_inset
|
|
|
|
is smallest.
|
|
However, that leads to discontinuous boundaries.
|
|
Since
|
|
\begin_inset Formula $0\leq\left|e_{i}^{T}p\right|\leq1$
|
|
\end_inset
|
|
|
|
for all
|
|
\begin_inset Formula $p\in S^{2}$
|
|
\end_inset
|
|
|
|
, a better idea might be to use a mixture, e.g.,
|
|
\begin_inset Formula
|
|
\[
|
|
a=\frac{1}{2(x^{2}+y^{2}+z^{2})}\left[\begin{array}{c}
|
|
y^{2}+z^{2}\\
|
|
x^{2}+z^{2}\\
|
|
x^{2}+y^{2}
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Now we can write
|
|
\begin_inset Formula $\xihat=B_{p}\xi$
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\xi\in R^{2}$
|
|
\end_inset
|
|
|
|
the 2D coordinate in the tangent plane basis
|
|
\begin_inset Formula $B_{p}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Retraction
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The exponential map uses
|
|
\begin_inset Formula $\cos$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\sin$
|
|
\end_inset
|
|
|
|
, and is more than we need for optimization.
|
|
Suppose we have a point
|
|
\begin_inset Formula $p\in S^{2}$
|
|
\end_inset
|
|
|
|
and a 3-vector
|
|
\begin_inset Formula $\xihat\in T_{p}S^{2}$
|
|
\end_inset
|
|
|
|
, Absil
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Absil07book"
|
|
|
|
\end_inset
|
|
|
|
tells us we can simply add
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
and renormalize to get a new point
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
on the sphere.
|
|
This is what he calls a
|
|
\series bold
|
|
retraction
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $\retract_{p}(\xihat)$
|
|
\end_inset
|
|
|
|
,
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\strikeout default
|
|
\uuline default
|
|
\uwave default
|
|
\noun default
|
|
\color inherit
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
q=\retract_{p}(\xihat)=\frac{p+\xihat}{\left\Vert p+z\right\Vert }=\frac{p+\xihat}{\alpha}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\alpha$
|
|
\end_inset
|
|
|
|
the norm of
|
|
\begin_inset Formula $p+\xihat$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can also define a retraction from local coordinates
|
|
\begin_inset Formula $\xi\in\Rtwo$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
q=\retract_{p}(\xi)=\frac{p+B_{p}\xi}{\left\Vert p+B_{p}\xi\right\Vert }
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Inverse Retraction
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
If
|
|
\begin_inset Formula $\xihat=B_{p}\xi$
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $\xi\in R^{2}$
|
|
\end_inset
|
|
|
|
the 2D coordinate in the tangent plane basis
|
|
\begin_inset Formula $B_{p}$
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula
|
|
\[
|
|
\xi=\frac{B_{p}^{T}q}{p^{T}q}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Proof
|
|
We seek
|
|
\begin_inset Formula
|
|
\[
|
|
\alpha q=p+B_{p}\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
If we multiply both sides with
|
|
\begin_inset Formula $B_{p}^{T}$
|
|
\end_inset
|
|
|
|
(project on the basis
|
|
\begin_inset Formula $B_{p}$
|
|
\end_inset
|
|
|
|
) we obtain
|
|
\begin_inset Formula
|
|
\[
|
|
\alpha B_{p}^{T}q=B_{p}^{T}p+B_{p}^{T}B_{p}\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and because
|
|
\begin_inset Formula $B_{p}^{T}p=0$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $B_{p}^{T}B_{p}=I$
|
|
\end_inset
|
|
|
|
we trivially obtain
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
as the scaled projection
|
|
\begin_inset Formula $B_{p}^{T}q$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
\xi=\alpha B_{p}^{T}q
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
To recover the scale factor
|
|
\begin_inset Formula $\alpha$
|
|
\end_inset
|
|
|
|
we multiply with
|
|
\begin_inset Formula $p^{T}$
|
|
\end_inset
|
|
|
|
on both sides, and we get
|
|
\begin_inset Formula
|
|
\[
|
|
\alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Since
|
|
\begin_inset Formula $p^{T}p=1$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $p^{T}B_{p}\xi=0$
|
|
\end_inset
|
|
|
|
, we then obtain
|
|
\begin_inset Formula $\alpha=1/(p^{T}q)$
|
|
\end_inset
|
|
|
|
, which completes the proof.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Rotation acting on a 3D Direction
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Rotating a point
|
|
\begin_inset Formula $p\in S^{2}$
|
|
\end_inset
|
|
|
|
on the sphere obviously yields another point
|
|
\begin_inset Formula $q=Rp\in S^{2}$
|
|
\end_inset
|
|
|
|
, as rotation preserves the norm.
|
|
The derivative of
|
|
\begin_inset Formula $f(R,p)$
|
|
\end_inset
|
|
|
|
with respect to
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
can be found by equating
|
|
\begin_inset Formula
|
|
\[
|
|
Rp+B_{Rp}\xi=R(I+\Skew{\omega})p=Rp+R\Skew{\omega}p
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
B_{Rp}\xi=-R\Skew p\omega
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\xi=-B_{Rp}^{T}R\Skew p\omega
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
whereas with respect to
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
we have
|
|
\begin_inset Formula
|
|
\[
|
|
Rp+B_{Rp}\xi_{q}=R(p+B_{p}\xi_{p})
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\xi_{q}=B_{Rp}^{T}RB_{p}\xi_{p}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In other words, the Jacobian matrix is given by
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
f'(R,p)=\left[\begin{array}{cc}
|
|
-B_{Rp}^{T}R\Skew p & B_{Rp}^{T}RB_{p}\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Error between 3D Directions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We would like to define a distance metric
|
|
\begin_inset Formula $e(p,q)$
|
|
\end_inset
|
|
|
|
between two directions
|
|
\begin_inset Formula $p,q\in S^{2}$
|
|
\end_inset
|
|
|
|
.
|
|
An obvious choice is
|
|
\begin_inset Formula
|
|
\[
|
|
\theta=\cos^{-1}\left(p^{T}q\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which is exactly the distance along the shortest path (geodesic) on the
|
|
sphere, i.e., this is the distance metric associated with the exponential.
|
|
The advantage is that it is defined everywhere, but it involves
|
|
\begin_inset Formula $\cos^{-1}$
|
|
\end_inset
|
|
|
|
.
|
|
The derivative with respect to a change in
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
, via
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
, is then
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\theta(p,q)}{\partial\xi}=\frac{\partial\cos^{-1}\left(p^{T}q\right)}{\partial\xi}=\frac{p^{T}B_{q}}{\sqrt{1-\left(p^{T}q\right)^{2}}}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
which is also undefined for
|
|
\begin_inset Formula $p=q$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A simpler metric is derived from the retraction but only holds when
|
|
\begin_inset Formula $q\approx p$
|
|
\end_inset
|
|
|
|
.
|
|
It simply projects
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
onto the local coordinate basis
|
|
\begin_inset Formula $B_{p}$
|
|
\end_inset
|
|
|
|
defined by
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
, and takes the norm:
|
|
\begin_inset Formula
|
|
\[
|
|
\theta(p,q)=\left\Vert B_{p}^{T}q\right\Vert
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
The derivative with respect to a change in
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
, via
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
, is then
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\theta(p,q)}{\partial\xi_{q}}=\frac{\partial}{\partial\xi_{q}}\sqrt{\left(B_{p}^{T}q\right)^{2}}=\frac{1}{\sqrt{\left(B_{p}^{T}q\right)^{2}}}\left(B_{p}^{T}q\right)B_{p}^{T}B_{q}=\frac{B_{p}^{T}q}{\theta(q;p)}B_{p}^{T}B_{q}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Note that this again is undefined for
|
|
\begin_inset Formula $\theta=0$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For use in a probabilistic factor, a signed, vector-valued error will not
|
|
have the discontinuity:
|
|
\begin_inset Formula
|
|
\[
|
|
\theta(p,q)=B_{p}^{T}q
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Note this is the inverse retraction up to a scale.
|
|
The derivative with respect to a change in
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
, via
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
, is found by
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\theta(p,q)}{\partial\xi_{q}}=B_{p}^{T}\frac{\partial q}{\partial\xi_{q}}=B_{p}^{T}B_{q}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsubsection*
|
|
Application
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can use the above to find the unknown rotation between a camera and an
|
|
IMU.
|
|
If we measure the rotation between two frames as
|
|
\begin_inset Formula $c_{1}Zc_{2}$
|
|
\end_inset
|
|
|
|
, and the predicted rotation from the IMU is
|
|
\begin_inset Formula $i_{1}Ri_{2}$
|
|
\end_inset
|
|
|
|
, then we can predict
|
|
\begin_inset Formula
|
|
\[
|
|
c_{1}Zc_{2}=iRc^{T}\cdot i_{1}Ri_{2}\cdot iRc
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the axis of the incremental rotations will relate as
|
|
\begin_inset Formula
|
|
\[
|
|
p=iRc\cdot z
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
the angular velocity axis in the IMU frame, and
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
the measured axis of rotation between the two cameras.
|
|
Note this only makes sense if the rotation is non-zero.
|
|
So, given an initial estimate
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
for the unknown rotation
|
|
\begin_inset Formula $iRc$
|
|
\end_inset
|
|
|
|
between IMU and camera, the derivative of the error is (using
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Rot3action"
|
|
|
|
\end_inset
|
|
|
|
)
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial\theta(Rz;p)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}B_{Rz}\frac{\partial\left(Rz\right)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}R\Skew z
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Here the
|
|
\begin_inset Formula $2\times3$
|
|
\end_inset
|
|
|
|
matrix
|
|
\begin_inset Formula $B_{Rz}^{T}\Skew z$
|
|
\end_inset
|
|
|
|
translates changes in
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
to changes in
|
|
\begin_inset Formula $Rz$
|
|
\end_inset
|
|
|
|
, and the
|
|
\begin_inset Formula $1\times2$
|
|
\end_inset
|
|
|
|
matrix
|
|
\begin_inset Formula $B_{p}^{T}\left(Rz\right)$
|
|
\end_inset
|
|
|
|
describes the downstream effect on the error metric.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
The Essential Matrix Manifold
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We parameterize essential matrices as a pair
|
|
\begin_inset Formula $(R,t)$
|
|
\end_inset
|
|
|
|
, where
|
|
\begin_inset Formula $R\in\SOthree$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $t\in S^{2}$
|
|
\end_inset
|
|
|
|
, the unit sphere.
|
|
The epipolar matrix is then given by
|
|
\begin_inset Formula
|
|
\[
|
|
E=\Skew tR
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and the epipolar error given two corresponding points
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
is
|
|
\begin_inset Formula
|
|
\[
|
|
e(R,t;a,b)=a^{T}Eb
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We are of course interested in the derivative with respect to orientation
|
|
(using
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "eq:Rot3action"
|
|
|
|
\end_inset
|
|
|
|
)
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial(a^{T}[t]_{\times}Rb)}{\partial\omega}=a^{T}[t]_{\times}\frac{\partial(Rb)}{\partial\omega}=-a^{T}[t]_{\times}R\Skew b=-a^{T}E[b]_{\times}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and with respect to change in the direction
|
|
\begin_inset Formula $t$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial e(a^{T}[t]_{\times}Rb)}{\partial\xi}=a^{T}\frac{\partial(B\xi\times Rb)}{\partial v}=-a^{T}[Rb]_{\times}B
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where we made use of the fact that the retraction can be written as
|
|
\begin_inset Formula $t+B\xi$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $B$
|
|
\end_inset
|
|
|
|
a local basis, and we made use of
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:Dcross1"
|
|
|
|
\end_inset
|
|
|
|
:
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\strikeout off
|
|
\uuline off
|
|
\uwave off
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial(a\times b)}{\partial a}=\Skew{-b}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
2D Line Segments (Ocaml)
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The error between an infinite line
|
|
\begin_inset Formula $(a,b,c)$
|
|
\end_inset
|
|
|
|
and a 2D line segment
|
|
\begin_inset Formula $((x1,y1),(x2,y2))$
|
|
\end_inset
|
|
|
|
is defined in Line3.ml.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Line3vd (Ocaml)
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
One representation of a line is through 2 vectors
|
|
\begin_inset Formula $(v,d)$
|
|
\end_inset
|
|
|
|
, where
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
is the direction and the vector
|
|
\begin_inset Formula $d$
|
|
\end_inset
|
|
|
|
points from the orgin to the closest point on the line.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In this representation, transforming a 3D line from a world coordinate frame
|
|
to a camera at
|
|
\begin_inset Formula $(R_{w}^{c},t^{w})$
|
|
\end_inset
|
|
|
|
is done by
|
|
\begin_inset Formula
|
|
\[
|
|
v^{c}=R_{w}^{c}v^{w}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Line3 (Ocaml)
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For 3D lines, we use a parameterization due to C.J.
|
|
Taylor, using a rotation matrix
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
and 2 scalars
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
.
|
|
The line direction
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
is simply the Z-axis of the rotated frame, i.e.,
|
|
\begin_inset Formula $v=R_{3}$
|
|
\end_inset
|
|
|
|
, while the vector
|
|
\begin_inset Formula $d$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula $d=aR_{1}+bR_{2}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Now, we will
|
|
\emph on
|
|
not
|
|
\emph default
|
|
use the incremental rotation scheme we used for rotations: because the
|
|
matrix R translates from the line coordinate frame to the world frame,
|
|
we need to apply the incremental rotation on the right-side:
|
|
\begin_inset Formula
|
|
\[
|
|
R'=R(I+\Omega)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Projecting a line to 2D can be done easily, as both
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $d$
|
|
\end_inset
|
|
|
|
are also the 2D homogenous coordinates of two points on the projected line,
|
|
and hence we have
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
l & = & v\times d\\
|
|
& = & R_{3}\times\left(aR_{1}+bR_{2}\right)\\
|
|
& = & a\left(R_{3}\times R_{1}\right)+b\left(R_{3}\times R_{2}\right)\\
|
|
& = & aR_{2}-bR_{1}
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
This can be written as a rotation of a point,
|
|
\begin_inset Formula
|
|
\[
|
|
l=R\left(\begin{array}{c}
|
|
-b\\
|
|
a\\
|
|
0
|
|
\end{array}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
but because the incremental rotation is now done on the right, we need to
|
|
figure out the derivatives again:
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=R\frac{\partial(\Omega x)}{\partial\omega}=R\Skew{-x}\label{eq:rotateRight}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
and hence the derivative of the projection
|
|
\begin_inset Formula $l$
|
|
\end_inset
|
|
|
|
with respect to the rotation matrix
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
of the 3D line is
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\frac{\partial(l)}{\partial\omega}=R\Skew{\left(\begin{array}{c}
|
|
b\\
|
|
-a\\
|
|
0
|
|
\end{array}\right)}=\left[\begin{array}{ccc}
|
|
aR_{3} & bR_{3} & -(aR_{1}+bR_{2})\end{array}\right]
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
or the
|
|
\begin_inset Formula $a,b$
|
|
\end_inset
|
|
|
|
scalars:
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial(l)}{\partial a}=R_{2}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial(l)}{\partial b}=-R_{1}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Transforming a 3D line
|
|
\begin_inset Formula $(R,(a,b))$
|
|
\end_inset
|
|
|
|
from a world coordinate frame to a camera frame
|
|
\begin_inset Formula $(R_{w}^{c},t^{w})$
|
|
\end_inset
|
|
|
|
is done by
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula
|
|
\[
|
|
R'=R_{w}^{c}R
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
a'=a-R_{1}^{T}t^{w}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
b'=b-R_{2}^{T}t^{w}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Again, we need to redo the derivatives, as R is incremented from the right.
|
|
The first argument is incremented from the left, but the result is incremented
|
|
on the right:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\
|
|
I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
|
|
\Omega' & = & R'^{T}\Skew{S\omega}R'\\
|
|
\Omega' & = & \Skew{R'^{T}S\omega}\\
|
|
\omega' & = & R'^{T}S\omega
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
For the second argument
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
we now simply have:
|
|
\begin_inset Formula
|
|
\begin{eqnarray*}
|
|
AB(I+\Omega') & = & AB(I+\Omega)\\
|
|
\Omega' & = & \Omega\\
|
|
\omega' & = & \omega
|
|
\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
The scalar derivatives can be found by realizing that
|
|
\begin_inset Formula
|
|
\[
|
|
\left(\begin{array}{c}
|
|
a'\\
|
|
b'\\
|
|
...
|
|
\end{array}\right)=\left(\begin{array}{c}
|
|
a\\
|
|
b\\
|
|
0
|
|
\end{array}\right)-R^{T}t^{w}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where we don't care about the third row.
|
|
Hence
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{\partial(\left(R(I+\Omega_{2})\right)^{T}t^{w})}{\partial\omega}=-\frac{\partial(\Omega_{2}R^{T}t^{w})}{\partial\omega}=-\Skew{R^{T}t^{w}}=\left[\begin{array}{ccc}
|
|
0 & R_{3}^{T}t^{w} & -R_{2}^{T}t^{w}\\
|
|
-R_{3}^{T}t^{w} & 0 & R_{1}^{T}t^{w}\\
|
|
... & ... & 0
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
|
|
\series bold
|
|
Aligning 3D Scans
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Below is the explanation underlying Pose3.align, i.e.
|
|
aligning two point clouds using SVD.
|
|
Inspired but modified from CVOnline...
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\emph on
|
|
Our
|
|
\emph default
|
|
model is
|
|
\begin_inset Formula
|
|
\[
|
|
p^{c}=R\left(p^{w}-t\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
i.e.,
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
is from camera to world, and
|
|
\begin_inset Formula $t$
|
|
\end_inset
|
|
|
|
is the camera location in world coordinates.
|
|
The objective function is
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
\frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}+Rt\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}-t'\right)^{2}\label{eq:J}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $t'=-Rt$
|
|
\end_inset
|
|
|
|
is the location of the origin in the camera frame.
|
|
Taking the derivative with respect to
|
|
\begin_inset Formula $t'$
|
|
\end_inset
|
|
|
|
and setting to zero we have
|
|
\begin_inset Formula
|
|
\[
|
|
\sum\left(p^{c}-Rp^{w}-t'\right)=0
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
or
|
|
\begin_inset Formula
|
|
\begin{equation}
|
|
t'=\frac{1}{n}\sum\left(p^{c}-Rp^{w}\right)=\bar{p}^{c}-R\bar{p}^{w}\label{eq:t}
|
|
\end{equation}
|
|
|
|
\end_inset
|
|
|
|
here
|
|
\begin_inset Formula $\bar{p}^{c}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\bar{p}^{w}$
|
|
\end_inset
|
|
|
|
are the point cloud centroids.
|
|
Substituting back into
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:J"
|
|
|
|
\end_inset
|
|
|
|
, we get
|
|
\begin_inset Formula
|
|
\[
|
|
\frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(\left(p^{c}-\bar{p}^{c}\right)-R\left(p^{w}-\bar{p}^{w}\right)\right)^{2}=\frac{1}{2}\sum\left(\hat{p}^{c}-R\hat{p}^{w}\right)^{2}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Now, to minimize the above it suffices to maximize (see CVOnline)
|
|
\begin_inset Formula
|
|
\[
|
|
\mathop{trace}\left(R^{T}C\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $C=\sum\hat{p}^{c}\left(\hat{p}^{w}\right)^{T}$
|
|
\end_inset
|
|
|
|
is the correlation matrix.
|
|
Intuitively, the cloud of points is rotated to align with the principal
|
|
axes.
|
|
This can be achieved by SVD decomposition on
|
|
\begin_inset Formula $C$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
C=USV^{T}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
and setting
|
|
\begin_inset Formula
|
|
\[
|
|
R=UV^{T}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Clearly, from
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:t"
|
|
|
|
\end_inset
|
|
|
|
we then also recover the optimal
|
|
\begin_inset Formula $t$
|
|
\end_inset
|
|
|
|
as
|
|
\begin_inset Formula
|
|
\[
|
|
t=\bar{p}^{w}-R^{T}\bar{p}^{c}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section*
|
|
Appendix
|
|
\end_layout
|
|
|
|
\begin_layout Subsection*
|
|
Differentiation Rules
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Spivak
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Spivak65book"
|
|
|
|
\end_inset
|
|
|
|
also notes some multivariate derivative rules defined component-wise, but
|
|
they are not that useful in practice:
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Since
|
|
\begin_inset Formula $f:\Multi nm$
|
|
\end_inset
|
|
|
|
is defined in terms of
|
|
\begin_inset Formula $m$
|
|
\end_inset
|
|
|
|
component functions
|
|
\begin_inset Formula $f^{i}$
|
|
\end_inset
|
|
|
|
, then
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
is differentiable at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
iff each
|
|
\begin_inset Formula $f^{i}$
|
|
\end_inset
|
|
|
|
is, and the Jacobian matrix
|
|
\begin_inset Formula $F_{a}$
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $m\times n$
|
|
\end_inset
|
|
|
|
matrix whose
|
|
\begin_inset Formula $i^{th}$
|
|
\end_inset
|
|
|
|
row is
|
|
\begin_inset Formula $\left(f^{i}\right)'(a)$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula
|
|
\[
|
|
F_{a}\define f'(a)=\left[\begin{array}{c}
|
|
\left(f^{1}\right)'(a)\\
|
|
\vdots\\
|
|
\left(f^{m}\right)'(a)
|
|
\end{array}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Itemize
|
|
Scalar differentiation rules: if
|
|
\begin_inset Formula $f,g:\OneD n$
|
|
\end_inset
|
|
|
|
are differentiable at
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
, then
|
|
\begin_inset Formula
|
|
\[
|
|
(f+g)'(a)=F_{a}+G_{a}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
(f\cdot g)'(a)=g(a)F_{a}+f(a)G_{a}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
(f/g)'(a)=\frac{1}{g(a)^{2}}\left[g(a)F_{a}-f(a)G_{a}\right]
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection*
|
|
Tangent Spaces and the Tangent Bundle
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The following is adapted from Appendix A in
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Murray94book"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The
|
|
\series bold
|
|
tangent space
|
|
\series default
|
|
|
|
\begin_inset Formula $T_{p}M$
|
|
\end_inset
|
|
|
|
of a manifold
|
|
\begin_inset Formula $M$
|
|
\end_inset
|
|
|
|
at a point
|
|
\begin_inset Formula $p\in M$
|
|
\end_inset
|
|
|
|
is the vector space of
|
|
\series bold
|
|
tangent vectors
|
|
\series default
|
|
at
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
.
|
|
The
|
|
\series bold
|
|
tangent bundle
|
|
\series default
|
|
|
|
\begin_inset Formula $TM$
|
|
\end_inset
|
|
|
|
is the set of all tangent vectors
|
|
\begin_inset Formula
|
|
\[
|
|
TM\define\bigcup_{p\in M}T_{p}M
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
A
|
|
\series bold
|
|
vector field
|
|
\series default
|
|
|
|
\begin_inset Formula $X:M\rightarrow TM$
|
|
\end_inset
|
|
|
|
assigns a single tangent vector
|
|
\begin_inset Formula $x\in T_{p}M$
|
|
\end_inset
|
|
|
|
to each point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
If
|
|
\begin_inset Formula $F:M\rightarrow N$
|
|
\end_inset
|
|
|
|
is a smooth map from a manifold
|
|
\begin_inset Formula $M$
|
|
\end_inset
|
|
|
|
to a manifold
|
|
\begin_inset Formula $N$
|
|
\end_inset
|
|
|
|
, then we can define the
|
|
\series bold
|
|
tangent map
|
|
\series default
|
|
of
|
|
\begin_inset Formula $F$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
as the linear map
|
|
\begin_inset Formula $F_{*p}:T_{p}M\rightarrow T_{F(p)}N$
|
|
\end_inset
|
|
|
|
that maps tangent vectors in
|
|
\begin_inset Formula $T_{p}M$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
to tangent vectors in
|
|
\begin_inset Formula $T_{F(p)}N$
|
|
\end_inset
|
|
|
|
at the image
|
|
\begin_inset Formula $F(p)$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection*
|
|
Homomorphisms
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The following
|
|
\emph on
|
|
might be
|
|
\emph default
|
|
relevant
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "page 45"
|
|
key "Hall00book"
|
|
|
|
\end_inset
|
|
|
|
: suppose that
|
|
\begin_inset Formula $\Phi:G\rightarrow H$
|
|
\end_inset
|
|
|
|
is a mapping (Lie group homomorphism).
|
|
Then there exists a unique linear map
|
|
\begin_inset Formula $\phi:\gg\rightarrow\mathfrak{h}$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\phi(\xhat)\define\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(e^{t\xhat}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
such that
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
\begin_inset Formula $\Phi\left(e^{\xhat}\right)=e^{\phi\left(\xhat\right)}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
\begin_inset Formula $\phi\left(T\xhat T^{-1}\right)=\Phi(T)\phi(\xhat)\Phi(T^{-1})$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
\begin_inset Formula $\phi\left([\xhat,\yhat]\right)=\left[\phi(\xhat),\phi(\yhat)\right]$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In other words, the map
|
|
\begin_inset Formula $\phi$
|
|
\end_inset
|
|
|
|
is the derivative of
|
|
\begin_inset Formula $\Phi$
|
|
\end_inset
|
|
|
|
at the identity.
|
|
As an example, suppose
|
|
\begin_inset Formula $\Phi(g)=g^{-1}$
|
|
\end_inset
|
|
|
|
, then the corresponding derivative
|
|
\emph on
|
|
at the identity
|
|
\emph default
|
|
is
|
|
\begin_inset Formula
|
|
\[
|
|
\phi(\xhat)\define\lim_{t\rightarrow0}\frac{d}{dt}\left(e^{t\xhat}\right)^{-1}=\lim_{t\rightarrow0}\frac{d}{dt}e^{-t\xhat}=-\xhat\lim_{t\rightarrow0}e^{-t\xhat}=-\xhat
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
In general it suffices to compute
|
|
\begin_inset Formula $\phi$
|
|
\end_inset
|
|
|
|
for a basis of
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
Undercooked: What if we want the derivative of
|
|
\begin_inset Formula $\Phi$
|
|
\end_inset
|
|
|
|
at some other element
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
? In other words, if we apply
|
|
\begin_inset Formula $\Phi$
|
|
\end_inset
|
|
|
|
at
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
incremented by some Lie algebra element
|
|
\begin_inset Formula $e^{\xhat}$
|
|
\end_inset
|
|
|
|
, then we are looking for a
|
|
\begin_inset Formula $\yhat\in\gg$
|
|
\end_inset
|
|
|
|
will yield the same result:
|
|
\begin_inset Formula
|
|
\[
|
|
\Phi\left(g\right)\lim_{t\rightarrow0}\frac{d}{dt}e^{t\yhat}=\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(ge^{t\xhat}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula
|
|
\[
|
|
\lim_{t\rightarrow0}\frac{d}{dt}e^{t\yhat}=\Phi\left(g\right)^{-1}\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(ge^{t\xhat}\right)
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
Let us define two mappings
|
|
\begin_inset Formula
|
|
\[
|
|
\Phi_{1}(A)=AB\mbox{ and }\Phi_{2}(B)=AB
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
Then
|
|
\begin_inset Formula
|
|
\[
|
|
\phi_{1}(\xhat)=\lim_{t\rightarrow0}\frac{d}{dt}\Phi_{1}\left(e^{t\xhat}B\right)=
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset CommandInset bibtex
|
|
LatexCommand bibtex
|
|
bibfiles "/Users/dellaert/papers/refs"
|
|
options "plain"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_body
|
|
\end_document
|