#LyX 2.3 created this file. For more info see http://www.lyx.org/
\lyxformat 544
\begin_document
\begin_header
\save_transient_properties true
\origin unavailable
\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" "default"
\font_sans "default" "default"
\font_typewriter "default" "default"
\font_math "auto" "auto"
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100 100
\font_tt_scale 100 100
\use_microtype false
\use_dash_ligatures true
\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_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 0
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 0
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 0
\use_minted 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
\is_math_indent 0
\math_numbering_side default
\quotes_style english
\dynamic_quotes 0
\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"
thickness "0.4pt"
separation "3pt"
shadowsize "4pt"
framecolor "black"
backgroundcolor "none"
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"
literal "true"

\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"
literal "true"

\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"
literal "true"

\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 a mapping
 from 
\series bold
local coordinates
\series default
 
\begin_inset Formula $\xi$
\end_inset

 back to a neighborhood in 
\begin_inset Formula $G$
\end_inset

 around 
\begin_inset Formula $a$
\end_inset

, 
\begin_inset Formula 
\begin{equation}
a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap}
\end{equation}

\end_inset

with 
\begin_inset Formula $\xi\in\Reals n$
\end_inset

 for an 
\begin_inset Formula $n$
\end_inset

-dimensional Lie group.
 Above, 
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
\end_inset

 is 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 t$
\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 t}\in\sothree$
\end_inset

, and is given by
\begin_inset Formula 
\[
\Skew{\omega t}=\left[\begin{array}{ccc}
0 & -\omega_{z} & \omega_{y}\\
\omega_{z} & 0 & -\omega_{x}\\
-\omega_{y} & \omega_{x} & 0
\end{array}\right]t
\]

\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 t=Re^{\Skew{\omega t}}$
\end_inset

.
\end_layout

\begin_layout Subsection
Local Coordinates vs.
 Tangent Vectors
\end_layout

\begin_layout Standard
In differential geometry, 
\series bold
tangent vectors
\series default
 
\begin_inset Formula $v\in T_{a}G$
\end_inset

 at 
\begin_inset Formula $a$
\end_inset

 are elements of the Lie algebra 
\begin_inset Formula $\mathfrak{g}$
\end_inset

, and are defined as
\begin_inset Formula 
\[
v\define\Jac{\gamma(t)}t{t=0}
\]

\end_inset

where 
\begin_inset Formula $\gamma$
\end_inset

 is some curve that passes through 
\begin_inset Formula $a$
\end_inset

 at 
\begin_inset Formula $t=0$
\end_inset

, i.e.
 
\begin_inset Formula $\gamma(0)=a$
\end_inset

.
 In particular, for any fixed local coordinate 
\begin_inset Formula $\xi$
\end_inset

 the map 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:expmap"

\end_inset

 can be used to define a 
\series bold
geodesic curve
\series default
 on the group manifold defined by 
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
\end_inset

, and the corresponding tangent vector is given by 
\begin_inset Formula 
\begin{equation}
\Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector}
\end{equation}

\end_inset

This defines the mapping between local coordinates 
\begin_inset Formula $\xi\in\Rn$
\end_inset

 and actual tangent vectors 
\begin_inset Formula $a\xihat\in g$
\end_inset

: the vector 
\begin_inset Formula $\xi$
\end_inset

 defines a direction of travel on the manifold, but does so in the local
 coordinate frame 
\begin_inset Formula $a$
\end_inset

.
\end_layout

\begin_layout Example
Assume a rigid body's attitude is described by 
\begin_inset Formula $R_{b}^{n}(t)$
\end_inset

, where the indices denote the navigation frame 
\begin_inset Formula $N$
\end_inset

 and body frame 
\begin_inset Formula $B$
\end_inset

, respectively.
 An extrinsically calibrated gyroscope measures the angular velocity 
\begin_inset Formula $\omega^{b}$
\end_inset

, in the body frame, and the corresponding tangent vector is 
\begin_inset Formula 
\[
\dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}}
\]

\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 local 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

 approximates the function 
\begin_inset Formula $f$
\end_inset

 from 
\begin_inset Formula $G$
\end_inset

 to 
\begin_inset Formula $\Reals m$
\end_inset

 in a neighborhood around 
\begin_inset Formula $a$
\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 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 a 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{m+n}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

where 
\begin_inset Formula $m$
\end_inset

 is the dimensionality of the manifold 
\begin_inset Formula $G$
\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 m\rightarrow\Reals{n\times m}$
\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 m$
\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"
literal "true"

\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 "subsec: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 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.
 As with pushforwards above, we will be looking for an 
\begin_inset Formula $n\times n$
\end_inset

 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
Jacobian 
\begin_inset Formula $f'(\xi)$
\end_inset

 such that
\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}
f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp}
\end{equation}

\end_inset

Differential geometry tells us that for any Lie algebra element 
\begin_inset Formula $\xihat\in\mathfrak{g}$
\end_inset

 there exists a 
\emph on
linear
\emph default
 map 
\begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$
\end_inset

, which is given by
\begin_inset Foot
status collapsed

\begin_layout Plain Layout
See 
\begin_inset Flex URL
status open

\begin_layout Plain Layout

http://deltaepsilons.wordpress.com/2009/11/06/
\end_layout

\end_inset

 or 
\begin_inset Flex URL
status open

\begin_layout Plain Layout

https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map
\end_layout

\end_inset

.
\end_layout

\end_inset


\begin_inset Formula 
\begin{equation}
d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp}
\end{equation}

\end_inset

with 
\begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$
\end_inset

 and 
\begin_inset Formula $ad_{\xihat}$
\end_inset

 itself a linear map taking 
\begin_inset Formula $\hat{x}$
\end_inset

 to 
\begin_inset Formula $[\xihat,\hat{x}]$
\end_inset

, the Lie bracket.
 The actual formula above is not really as important as the fact that the
 linear map exists, although it is expressed directly in terms of tangent
 vectors to 
\begin_inset Formula $\mathfrak{g}$
\end_inset

 and 
\begin_inset Formula $G$
\end_inset

.
 Equation 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:dexp"

\end_inset

 is a tangent vector, and comparing with 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:tangent-vector"

\end_inset

 we see that it maps to local coordinates 
\begin_inset Formula $y$
\end_inset

 as follows:
\begin_inset Formula 
\[
\yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}
\]

\end_inset

which can be used to construct the Jacobian 
\begin_inset Formula $f'(\xi)$
\end_inset

.
\end_layout

\begin_layout Example
For 
\begin_inset Formula $\SOthree$
\end_inset

, the operator 
\begin_inset Formula $ad_{\xihat}$
\end_inset

 is simply a matrix multiplication when representing 
\begin_inset Formula $\sothree$
\end_inset

 using 3-vectors, i.e., 
\begin_inset Formula $ad_{\xihat}x=\xihat x$
\end_inset

, and the 
\begin_inset Formula $3\times3$
\end_inset

 Jacobian corresponding to 
\begin_inset Formula $d\exp$
\end_inset

 is 
\begin_inset Formula 
\[
f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k}
\]

\end_inset

which, similar to the exponential map, has a simple closed form expression
 for 
\begin_inset Formula $\SOthree$
\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"
literal "true"

\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 "subsec: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 "subsec: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"
literal "true"

\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
Derivative of Adjoint
\begin_inset CommandInset label
LatexCommand label
name "subsec:pose3_adjoint_deriv"

\end_inset


\end_layout

\begin_layout Standard
Consider 
\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$
\end_inset

 is defined as 
\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$
\end_inset

.
 The derivative is notated (see Section 
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"
plural "false"
caps "false"
noprefix "false"

\end_inset

):
\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b})
\]

\end_inset

First, computing 
\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$
\end_inset

 is easy, as its matrix is simply 
\begin_inset Formula $Ad_{T}$
\end_inset

:
\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b})
\]

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T}
\]

\end_inset

We will derive 
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$
\end_inset

 using two approaches.
 In the first, we'll define 
\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$
\end_inset

.
 From Section 
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"
plural "false"
caps "false"
noprefix "false"

\end_inset

,
\end_layout

\begin_layout Standard
\begin_inset Formula 
\begin{align*}
D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\
D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1}
\end{align*}

\end_inset

Now we can use the definition of the Adjoint representation 
\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$
\end_inset

 (aka conjugation by 
\begin_inset Formula $g$
\end_inset

) then apply product rule and simplify:
\end_layout

\begin_layout Standard
\begin_inset Formula 
\begin{align*}
D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\
 & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\
 & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\
 & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\
 & =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\
 & =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\
D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}})
\end{align*}

\end_inset

Where 
\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$
\end_inset

 is the adjoint map of the lie algebra.
\end_layout

\begin_layout Standard
The second, perhaps more intuitive way of deriving 
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$
\end_inset

, would be to use the fact that the derivative at the origin 
\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$
\end_inset

 by definition of the adjoint 
\begin_inset Formula $ad_{\xi}$
\end_inset

.
 Then applying the property 
\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$
\end_inset

,
\end_layout

\begin_layout Standard
\begin_inset Formula 
\[
D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right)
\]

\end_inset


\end_layout

\begin_layout Subsection
Derivative of AdjointTranspose
\end_layout

\begin_layout Standard
The transpose of the Adjoint, 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$
\end_inset

, is useful as a way to change the reference frame of vectors in the dual
 space 
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
(note the 
\begin_inset Formula $^{*}$
\end_inset

 denoting that we are now in the dual space)
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
.
 To be more concrete, where
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
as 
\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$
\end_inset

 converts the 
\emph on
twist
\emph default
 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $\xi_{b}$
\end_inset

 from the 
\begin_inset Formula $T$
\end_inset

 frame,
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
\end_inset

 converts the 
\family default
\series default
\shape default
\size default
\emph on
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
wrench
\emph default
 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none

\begin_inset Formula $\xi_{b}^{*}$
\end_inset

 from the 
\begin_inset Formula $T$
\end_inset

 frame
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
.
 It's difficult to apply a similar derivation as in Section 
\begin_inset CommandInset ref
LatexCommand ref
reference "subsec:pose3_adjoint_deriv"
plural "false"
caps "false"
noprefix "false"

\end_inset

 for the derivative of 
\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
\end_inset

 because 
\begin_inset Formula $Ad_{T}^{T}$
\end_inset

 cannot be naturally defined as a conjugation, so we resort to crunching
 through the algebra.
 The details are omitted but the result is a form that vaguely resembles
 (but does not exactly match) 
\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$
\end_inset

:
\end_layout

\begin_layout Standard
\begin_inset Formula 
\begin{align*}
\begin{bmatrix}\omega_{T}\\
v_{T}
\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\
D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\
\hat{v}_{T} & 0
\end{bmatrix}
\end{align*}

\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_{2}} & = & 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"
literal "true"

\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"
literal "false"

\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"
literal "true"

\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 origin 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
\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


\end_layout

\begin_layout Subsection
Projecting Line3
\end_layout

\begin_layout Standard
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 Subsection
Action of 
\begin_inset Formula $\SEthree$
\end_inset

 on the line
\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 $T_{c}^{w}=(R_{c}^{w},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

where 
\begin_inset Formula $R_{1}$
\end_inset

 and 
\begin_inset Formula $R_{2}$
\end_inset

 are the columns of 
\begin_inset Formula $R$
\end_inset

 , as before.
 
\end_layout

\begin_layout Standard
To find the derivatives, the transformation of a line 
\begin_inset Formula $l^{w}=(R,a,b)$
\end_inset

 from world coordinates to a camera coordinate frame 
\begin_inset Formula $T_{c}^{w}$
\end_inset

, specified in world coordinates, can be written as a function 
\begin_inset Formula $f:\SEthree\times L\rightarrow L$
\end_inset

, as given above, i.e., 
\begin_inset Formula 
\[
f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right).
\]

\end_inset

Let us find the Jacobian 
\begin_inset Formula $J_{1}$
\end_inset

 of 
\begin_inset Formula $f$
\end_inset

 with respect to the first argument 
\begin_inset Formula $T_{c}^{w}$
\end_inset

, which should obey
\begin_inset Formula 
\begin{align*}
f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi
\end{align*}

\end_inset

Note that
\begin_inset Formula 
\[
T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc}
R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\
0 & 1
\end{array}\right]
\]

\end_inset

Let's write this out separately for each of 
\begin_inset Formula $R,a,b$
\end_inset

:
\begin_inset Formula 
\begin{align*}
\left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\
a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\
b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v
\end{align*}

\end_inset

Simplifying, we get:
\begin_inset Formula 
\begin{align*}
-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
-R_{1}^{T}R_{c}^{w} & \approx J_{av}\\
-R_{2}^{T}R_{c}^{w} & \approx J_{bv}
\end{align*}

\end_inset

which gives the expressions for 
\begin_inset Formula $J_{av}$
\end_inset

 and 
\begin_inset Formula $J_{bv}$
\end_inset

.
 The top line can be further simplified:
\begin_inset Formula 
\begin{align*}
-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\
-R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\
-\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\
-R'^{T} & \approx J_{R\omega}
\end{align*}

\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"
literal "true"

\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"
literal "true"

\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"
literal "true"

\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 "refs"
options "plain"

\end_inset


\end_layout

\end_body
\end_document
