gtsam/doc/math.lyx

#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
\lyxformat 345
\begin_document
\begin_header
\textclass article
\use_default_options false
\language english
\inputencoding auto
\font_roman times
\font_sans default
\font_typewriter default
\font_default_family rmdefault
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100

\graphics default
\paperfontsize 12
\spacing single
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 0
\cite_engine basic
\use_bibtopic false
\paperorientation portrait
\leftmargin 1in
\topmargin 1in
\rightmargin 1in
\bottommargin 1in
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\defskip medskip
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\author "" 
\author "" 
\end_header

\begin_body

\begin_layout Title
Geometry Derivatives and Other Hairy Math
\end_layout

\begin_layout Author
Frank Dellaert
\end_layout

\begin_layout Plain Layout
\begin_inset Box Frameless
position "t"
hor_pos "c"
has_inner_box 1
inner_pos "t"
use_parbox 0
width "100col%"
special "none"
height "1in"
height_special "totalheight"
status collapsed

\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}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\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}{\mathfrak{\mathbb{R}^{3}}}
{\mathfrak{\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}{\mathfrak{\mathbb{R}^{6}}}
{\mathfrak{\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 Section
Derivatives of Lie Group Mappings
\end_layout

\begin_layout Subsection
New
\end_layout

\begin_layout Standard
The following is relevant 
\begin_inset CommandInset citation
LatexCommand cite
after "page 45"
key "Hall00book"

\end_inset

: suppose that 
\begin_inset Formula $\Phi:G\rightarrow H$
\end_inset

 is a 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.
 It suffices to compute 
\begin_inset Formula $\phi$
\end_inset

 for a basis of 
\begin_inset Formula $\gg$
\end_inset

.
 Since 
\begin_inset Formula \[
e^{-\xhat}=\left(e^{-\xhat}\right)^{-1}\]

\end_inset

 clearly 
\begin_inset Formula $\phi(\xhat)=-\xhat$
\end_inset

 for the inverse mapping.
\end_layout

\begin_layout Standard
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

\begin_layout Subsection
Old
\end_layout

\begin_layout Standard
The derivatives for 
\emph on
inverse, compose
\emph default
, and 
\emph on
between
\emph default
 can be derived from Lie group principles.
 
\begin_inset Note Note
status collapsed

\begin_layout Plain Layout
To find the derivatives of these functions, we look for the necessary 
\begin_inset Quotes eld
\end_inset

delta
\begin_inset Quotes erd
\end_inset

 in the tangent space of the function 
\emph on
output
\emph default
 
\begin_inset Formula $f(g)$
\end_inset

 that corresponds to a 
\begin_inset Quotes eld
\end_inset

delta
\begin_inset Quotes erd
\end_inset

 in the tangent space of the function 
\emph on
input
\emph default
 
\begin_inset Formula $g$
\end_inset

.
 
\end_layout

\end_inset

Specifically, to find the derivative of a function 
\begin_inset Formula $f\left(g\right)$
\end_inset

, we want to find the Lie algebra element 
\begin_inset Formula $\yhat\in\gg$
\end_inset

, that will result from changing 
\begin_inset Formula $g$
\end_inset

 using 
\begin_inset Formula $\xhat$
\end_inset

, also in exponential coordinates:
\begin_inset Formula \[
f\left(g\right)e^{\yhat}=f\left(ge^{\xhat}\right)\]

\end_inset

Calculating these derivatives requires that we know the form of the function
 
\begin_inset Formula $f$
\end_inset

.
\end_layout

\begin_layout Standard
Starting with 
\series bold
inverse
\series default
, i.e., 
\begin_inset Formula $f(g)=g^{-1}$
\end_inset

, we have
\begin_inset Formula \begin{align}
g^{-1}e^{\yhat} & =\left(ge^{\xhat}\right)^{-1}=e^{-\xhat}g^{-1}\nonumber \\
e^{\yhat} & =ge^{-\xhat}g^{-1}=e^{\Ad g\left(-\xhat\right)}\nonumber \\
\yhat & =\Ad g\left(-\xhat\right)\label{eq:Dinverse}\end{align}

\end_inset

In other words, and this is very intuitive in hindsight, the inverse is
 just negation of 
\begin_inset Formula $\xhat$
\end_inset

, along with an adjoint to make sure it is applied in the right frame!
\end_layout

\begin_layout Standard

\series bold
Compose
\series default
 can be derived similarly.
 Let us define two functions to find the derivatives in first and second
 arguments:
\begin_inset Formula \[
f_{1}(g)=gh\mbox{ and }f_{2}(h)=gh\]

\end_inset

 The latter is easiest, as a change 
\begin_inset Formula $\xhat$
\end_inset

 in the second argument 
\begin_inset Formula $h$
\end_inset

 simply gets applied to the result 
\begin_inset Formula $gh$
\end_inset

: 
\begin_inset Formula \begin{align}
f_{2}(h)e^{\yhat} & =f_{2}\left(he^{\xhat}\right)\nonumber \\
ghe^{\yhat} & =ghe^{\xhat}\nonumber \\
\yhat & =\xhat\label{eq:Dcompose2}\end{align}

\end_inset

The derivative for the first argument is a bit trickier:
\begin_inset Formula \begin{align}
f_{1}(g)e^{\yhat} & =f_{1}\left(ge^{\xhat}\right)\nonumber \\
ghe^{\yhat} & =ge^{\xhat}h\nonumber \\
e^{\yhat} & =h^{-1}e^{\xhat}h=e^{\Ad{h^{-1}}\xhat}\nonumber \\
\yhat & =\Ad{h^{-1}}\xhat\label{eq:Dcompose1}\end{align}

\end_inset

In other words, to apply a change 
\begin_inset Formula $\xhat$
\end_inset

 in 
\begin_inset Formula $g$
\end_inset

 we first need to undo 
\begin_inset Formula $h$
\end_inset

, then apply 
\begin_inset Formula $\xhat$
\end_inset

, and then apply 
\begin_inset Formula $h$
\end_inset

 again.
 All can be done in one step by simply applying 
\begin_inset Formula $\Ad{h^{-1}}\xhat$
\end_inset

.
\end_layout

\begin_layout Standard
Finally, let us find the derivative of 
\series bold
between
\series default
, defined as 
\begin_inset Formula $between(g,h)=compose(inverse(g),h)$
\end_inset

.
 The derivative in the second argument 
\begin_inset Formula $h$
\end_inset

 is similarly trivial: 
\begin_inset Formula $\yhat=\xhat$
\end_inset

.
 The first argument goes as follows:
\begin_inset Formula \begin{align}
f_{1}(g)e^{\yhat} & =f_{1}\left(ge^{\xhat}\right)\nonumber \\
g^{-1}he^{\yhat} & =\left(ge^{\xhat}\right)^{-1}h=e^{\left(-\xhat\right)}g^{-1}h\nonumber \\
e^{\yhat} & =\left(h^{-1}g\right)e^{\left(-\xhat\right)}\left(h^{-1}g\right)^{-1}=e^{\Ad{\left(h^{-1}g\right)}\left(-\xhat\right)}\nonumber \\
\yhat & =\Ad{\left(h^{-1}g\right)}\left(-\xhat\right)=\Ad{between\left(h,g\right)}\left(-\xhat\right)\label{eq:Dbetween1}\end{align}

\end_inset

Hence, now we undo 
\begin_inset Formula $h$
\end_inset

 and then apply the inverse 
\begin_inset Formula $\left(-\xhat\right)$
\end_inset

 in the 
\begin_inset Formula $g$
\end_inset

 frame.
\end_layout

\begin_layout Subsection*
Numerical Derivatives
\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 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,d,0,0]$
\end_inset

.
 Then take derivative, 
\begin_inset Formula \[
\deriv{y(d)}d\define\lim_{d->0}\frac{y(d)-y(0)}{d}=\lim_{d->0}\frac{1}{d}\log\left[f\left(g\right)^{-1}f\left(ge^{\hat{e_{i}}}\right)\right]\]

\end_inset

which is the basis for a numerical derivative scheme.
\end_layout

\begin_layout Standard
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

\begin_layout Section
Derivatives of Actions
\begin_inset CommandInset label
LatexCommand label
name "sec:Derivatives-of-Actions"

\end_inset


\end_layout

\begin_layout Subsection
Forward Action
\end_layout

\begin_layout Standard
The (usual) action of an 
\begin_inset Formula $n$
\end_inset

-dimensional matrix group 
\begin_inset Formula $G$
\end_inset

 is matrix-vector multiplication on 
\begin_inset Formula $\mathbb{R}^{n}$
\end_inset

, 
\begin_inset Formula \[
q=Tp\]

\end_inset

with 
\begin_inset Formula $p,q\in\mathbb{R}^{n}$
\end_inset

 and 
\begin_inset Formula $T\in GL(n)$
\end_inset

.
 Let us first do away with the derivative in 
\begin_inset Formula $p$
\end_inset

, which is easy:
\begin_inset Formula \[
\deriv{\left(Tp\right)}p=T\]

\end_inset

We would now like to know what an incremental action 
\begin_inset Formula $\xhat$
\end_inset

 would do, through the exponential map
\begin_inset Formula \[
q(x)=Te^{\xhat}p\]

\end_inset

with derivative
\begin_inset Formula \[
\deriv{q(x)}x=T\deriv{}x\left(e^{\xhat}p\right)\]

\end_inset

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 \[
e^{\xhat}p=p+\xhat p+\ldots\]

\end_inset

and the derivative of an incremental action x for matrix Lie groups becomes
\begin_inset Formula \[
\deriv{q(x)}x=T\deriv{\left(\xhat p\right)}x\define TH_{p}\]

\end_inset

where 
\begin_inset Formula $H_{p}$
\end_inset

 is an 
\begin_inset Formula $n\times n$
\end_inset

 Jacobian matrix that depends on 
\begin_inset Formula $p$
\end_inset

.
 
\begin_inset Note Note
status collapsed

\begin_layout Plain Layout
Recalling the definition 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:generators"

\end_inset

 of the map 
\begin_inset Formula $x\rightarrow\xhat$
\end_inset

, we can calculate 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\noun off
\color none

\begin_inset Formula $\xhat p$
\end_inset


\family default
\series default
\shape default
\size default
\emph default
\bar default
\noun default
\color inherit
 as (using tensor notation)
\begin_inset Formula \[
\left(\xhat p\right)_{jk}=G_{jk}^{i}x_{i}p^{k}\]

\end_inset

and hence the derivative is
\begin_inset Formula \[
\left(H_{p}\right)_{j}^{i}=G_{jk}^{i}p^{k}\]

\end_inset

and the final derivative becomes
\begin_inset Formula \[
\deriv{q(x)}x=TH_{p}\]

\end_inset


\end_layout

\end_inset


\end_layout

\begin_layout Subsection
Inverse Action
\end_layout

\begin_layout Standard
When we apply the inverse transformation 
\begin_inset Formula \[
q=T^{-1}p\]

\end_inset

we would now like to know what an incremental action 
\begin_inset Formula $\xhat$
\end_inset

 on 
\begin_inset Formula $T$
\end_inset

 would do:
\begin_inset Formula \begin{eqnarray*}
q(x) & = & \left(Te^{\xhat}\right)^{-1}p\\
 & = & e^{-\xhat}T^{-1}p\\
 & = & T^{-1}Te^{-\xhat}T^{-1}p\\
 & = & -T^{-1}\exp\left(T\xhat T^{-1}\right)p\end{eqnarray*}

\end_inset

Hence
\begin_inset Formula \begin{equation}
\deriv{q(x)}x=-T^{-1}\deriv{\left(T\xhat T^{-1}\mbox{ }p\right)}x\label{eq:inverseAction}\end{equation}

\end_inset

The derivative in 
\begin_inset Formula $p$
\end_inset

 is again easy for matrix Lie groups:
\begin_inset Formula \[
\deriv{\left(T^{-1}p\right)}p=T^{-1}\]

\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 Mappings
\end_layout

\begin_layout Standard
We have the derivative of 
\series bold
inverse
\series default
,
\begin_inset Formula \[
\frac{\partial R^{T}}{\partial\theta}=-\Ad R=-1\mbox{ }\]

\end_inset


\series bold
compose,
\series default

\begin_inset Formula \[
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\theta_{1}}=\Ad{R_{2}^{T}}=1\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\theta_{2}}=1\]

\end_inset


\series bold

\begin_inset Formula $and$
\end_inset

between:
\series default

\begin_inset Formula \[
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\theta_{1}}=-\Ad{R_{2}^{T}R_{1}}=-1\mbox{ and }\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\theta_{2}}=1\]

\end_inset


\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 corresponds to rotating a point
\begin_inset Formula \[
q=Rp\]

\end_inset

We would now like to know what an incremental rotation parameterized by
 
\begin_inset Formula $\theta$
\end_inset

 would do:
\begin_inset Formula \[
q(\text{\omega t})=Re^{\skew{\omega t}}p\]

\end_inset

The derivative is (following the exposition in Section 
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"

\end_inset

):
\begin_inset Formula \[
\deriv{q(\omega t)}t=R\deriv{}t\left(e^{\skew{\omega t}}p\right)=R\deriv{}t\left(\skew{\omega t}p\right)\]

\end_inset

Note that 
\begin_inset Formula \begin{equation}
\skew{\theta}\left[\begin{array}{c}
x\\
y\end{array}\right]=\theta R_{\pi/2}\left[\begin{array}{c}
x\\
y\end{array}\right]=\theta\left[\begin{array}{c}
-y\\
x\end{array}\right]\label{eq:RestrictedCross}\end{equation}

\end_inset

which acts like a restricted 
\begin_inset Quotes eld
\end_inset

cross product
\begin_inset Quotes erd
\end_inset

 in the plane.
 Hence 
\begin_inset Formula \[
\skew{\theta}p=\left[\begin{array}{c}
-y\\
x\end{array}\right]\theta=\omega R_{pi/2}pt\]

\end_inset

Hence, the final derivative of an action in its first argument is
\begin_inset Formula \[
\deriv{q(\omega t)}{\omega t}=\omega RR_{pi/2}p=\omega R_{pi/2}Rp=\omega R_{pi/2}q\]

\end_inset


\end_layout

\begin_layout Standard
Really need to think of relationship 
\begin_inset Formula $\omega$
\end_inset

 and 
\begin_inset Formula $t$
\end_inset

.
 We don't have a time 
\begin_inset Formula $t$
\end_inset

 in our code.
\end_layout

\begin_layout Standard
\begin_inset Newpage pagebreak
\end_inset


\end_layout

\begin_layout Section
2D Rigid Transformations
\end_layout

\begin_layout Subsection
Derivatives of Mappings
\end_layout

\begin_layout Standard
We can just define all derivatives in terms of the above adjoint map:
\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}}}=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 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 \[
\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

Analoguous to 
\begin_inset Formula $\SEthree$
\end_inset

, we can compute a velocity 
\begin_inset Formula $\xihat\hat{p}$
\end_inset

 in the local 
\begin_inset Formula $T$
\end_inset

 frame: 
\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}
\skew{\omega}p+v\\
0\end{array}\right]\]

\end_inset

By only taking the top two rows, we can write this as a velocity in 
\begin_inset Formula $\Rtwo$
\end_inset

, as the product of a 
\begin_inset Formula $2\times3$
\end_inset

 matrix 
\begin_inset Formula $H_{p}$
\end_inset

 that acts upon the exponential coordinates 
\begin_inset Formula $\xi$
\end_inset

 directly:
\begin_inset Formula \[
\skew{\omega}p+v=v+R_{\pi/2}p\omega=\left[\begin{array}{cc}
I_{2} & R_{\pi/2}p\end{array}\right]\left[\begin{array}{c}
v\\
\omega\end{array}\right]=H_{p}\xi\]

\end_inset

Hence, the final derivative of the group action is
\begin_inset Formula \[
\deriv{q(\xi)}{\xi}=R\left[\begin{array}{cc}
I_{2} & R_{\pi/2}p\end{array}\right]=\left[\begin{array}{cc}
R & R_{\pi/2}q\end{array}\right]\]

\end_inset

The derivative of the inverse action 
\begin_inset Formula $\hat{q}=T^{-1}\hat{p}$
\end_inset

 is given by 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq: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}=-T^{-1}\deriv{\left(\Ad T\hat{\xi}\right)\hat{p}}{\xi}\]

\end_inset

where the velocity now is
\begin_inset Formula \[
\left(\Ad T\hat{\xi}\right)\hat{p}=\left[\begin{array}{cc}
\skew{\omega} & Rv-\omega R_{\pi/2}t\\
0 & 0\end{array}\right]\left[\begin{array}{c}
p\\
1\end{array}\right]=\left[\begin{array}{c}
Rv+R_{\pi/2}(p-t)\omega\\
0\end{array}\right]\]

\end_inset

and hence
\begin_inset Formula \[
\deriv{q(\xi)}{\xi}=-R^{T}\left[\begin{array}{cc}
R & R_{\pi/2}(p-t)\end{array}\right]=\left[\begin{array}{cc}
-I_{2} & -R_{\pi/2}q\end{array}\right]\]

\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 Mappings
\end_layout

\begin_layout Standard
Hence, we are now in a position to simply posit the derivative of 
\series bold
inverse
\series default
,
\begin_inset Formula \begin{eqnarray*}
\Skew{\omega'} & = & \Ad R\left(\Skew{-\omega}\right)=\Skew{R(-\omega)}\\
\frac{\partial R^{T}}{\partial\omega} & = & -R\end{eqnarray*}

\end_inset


\series bold
compose,
\series default

\begin_inset Formula \[
\Skew{\omega'}=\Ad{R_{2}^{T}}\left(\Skew{\omega}\right)=\Skew{R_{2}^{T}\omega}\]

\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


\series bold
between
\series default
 in its first argument,
\begin_inset Formula \begin{eqnarray*}
\Skew{\omega'} & = & \Ad{R_{2}^{T}R_{1}}\left(\Skew{-\omega}\right)=\Skew{R_{2}^{T}R_{1}(-\omega)}\\
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}} & = & -R_{2}^{T}R_{1}=-between(R_{2},R_{1})\end{eqnarray*}

\end_inset

and between in its second argument,
\begin_inset Formula \begin{eqnarray*}
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{2}} & = & I_{3}\end{eqnarray*}

\end_inset


\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 corresponds to rotating a point
\begin_inset Formula \[
q=Rp\]

\end_inset

We would now like to know what an incremental rotation parameterized by
 
\begin_inset Formula $\omega$
\end_inset

 would do:
\begin_inset Formula \[
q(\omega)=Re^{\Skew{\omega}}p\]

\end_inset

hence the derivative (following the exposition in Section 
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"

\end_inset

):
\begin_inset Formula \[
\deriv{q(\omega)}{\omega}=R\deriv{}{\omega}\left(e^{\Skew{\omega}}p\right)=R\deriv{}{\omega}\left(\Skew{\omega}p\right)=RH_{p}\]

\end_inset

To calculate 
\begin_inset Formula $H_{p}$
\end_inset

 we make use of 
\begin_inset Formula \[
\Skew{\omega}p=\omega\times p=-p\times\omega=\Skew{-p}\omega\]

\end_inset

Hence, the final derivative of an action in its first argument is
\begin_inset Formula \[
\deriv{q(\omega)}{\omega}=RH_{p}=R\Skew{-p}\]

\end_inset


\end_layout

\begin_layout Standard
The derivative of the inverse action is given by 
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:inverseAction"

\end_inset

, specialized to 
\begin_inset Formula $\SOthree$
\end_inset

:
\end_layout

\begin_layout Standard
\begin_inset Formula \[
\deriv{q(\omega)}{\omega}=-R^{T}\deriv{\left(\Skew{R\omega\mbox{ }}p\right)}{\omega}=R^{T}\Skew pR=\Skew{R^{T}p}\]

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Newpage pagebreak
\end_inset


\end_layout

\begin_layout Section
3D Rigid Transformations
\end_layout

\begin_layout Subsection
Derivatives of Mappings
\end_layout

\begin_layout Standard
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

(but unit test on the above fails !!!), 
\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}}=\left[\begin{array}{cc}
R_{2}^{T} & 0\\
\Skew{-R_{2}^{T}t_{2}}R_{2}^{T} & R_{2}^{T}\end{array}\right]=\left[\begin{array}{cc}
R_{2}^{T} & 0\\
R_{2}^{T}\Skew{-t_{2}} & R_{2}^{T}\end{array}\right]\]

\end_inset

compose 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_{21}}=-\left[\begin{array}{cc}
R & 0\\
\Skew tR & R\end{array}\right]\]

\end_inset

with 
\begin_inset Formula \[
T_{12}=\left[\begin{array}{cc}
R & t\\
0 & 1\end{array}\right]=T_{1}^{^{-1}}T_{2}=between(T_{2},T_{1})\]

\end_inset

and between in its second argument,
\begin_inset Formula \begin{eqnarray*}
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6}\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Subsection
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]=\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

We would now like to know what an incremental rotation parameterized by
 
\begin_inset Formula $\xi$
\end_inset

 would do:
\begin_inset Formula \[
\hat{q}(\xi)=Te^{\xihat}\hat{p}\]

\end_inset

hence the derivative (following the exposition in Section 
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"

\end_inset

):
\begin_inset Formula \[
\deriv{\hat{q}(\xi)}{\xi}=T\deriv{}{\xi}\left(\xihat\hat{p}\right)=TH_{p}\]

\end_inset

where 
\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): 
\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]\]

\end_inset

Notice how velocities are anologous to points at infinity in projective
 geometry: they correspond to free vectors indicating a direction and magnitude
 of change.
 
\end_layout

\begin_layout Standard
By only taking the top three rows, we can write this as a velocity in 
\begin_inset Formula $\Rthree$
\end_inset

, as the product of a 
\begin_inset Formula $3\times6$
\end_inset

 matrix 
\begin_inset Formula $H_{p}$
\end_inset

 that acts upon the exponential coordinates 
\begin_inset Formula $\xi$
\end_inset

 directly:
\begin_inset Formula \[
\omega\times p+v=-p\times\omega+v=\left[\begin{array}{cc}
-\Skew p & I_{3}\end{array}\right]\left[\begin{array}{c}
\omega\\
v\end{array}\right]=H_{p}\xi\]

\end_inset

Hence, the final derivative of the group action is
\begin_inset Formula \[
\deriv{\hat{q}(\xi)}{\xi}=T\hat{H}_{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]\]

\end_inset

in homogenous coordinates.
 In 
\begin_inset Formula $\Rthree$
\end_inset

 this becomes:
\begin_inset Formula \[
\deriv{q(\xi)}{\xi}=R\left[\begin{array}{cc}
-\Skew p & I_{3}\end{array}\right]\]

\end_inset


\end_layout

\begin_layout Subsection
Pose3 (gtsam, old-style exmap)
\end_layout

\begin_layout Standard
In the old-style, we have 
\end_layout

\begin_layout Standard

\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\noun off
\color none
\begin_inset Formula $R'=R(I+\Omega)$
\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula $t'=t+dt$
\end_inset


\end_layout

\begin_layout Standard
In this case, the derivative of 
\series bold
\emph on
transform_from
\series default
\emph default
, 
\begin_inset Formula $Rx+t$
\end_inset

:
\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial(R(I+\Omega)x+t)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=\frac{\partial(R\left(\omega\times x\right))}{\partial\omega}=R\Skew{-x}\]

\end_inset

and with respect to 
\begin_inset Formula $dt$
\end_inset

 is easy:
\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial(Rx+t+dt)}{\partial dt}=I\]

\end_inset

The derivative of 
\series bold
\emph on
transform_to
\series default
\emph default
, 
\begin_inset Formula $inv(R)(x-t)$
\end_inset

 we can obtain using the chain rule:
\begin_inset Formula \[
\frac{\partial(inv(R)(x-t))}{\partial\omega}=\frac{\partial unrot(R,(x-t))}{\partial\omega}=skew(R^{T}\left(x-t\right))\]

\end_inset


\end_layout

\begin_layout Standard
and with respect to 
\begin_inset Formula $dt$
\end_inset

 is easy:
\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial(R^{T}(x-t-dt))}{\partial dt}=-R^{T}\]

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Newpage pagebreak
\end_inset


\end_layout

\begin_layout Section
2D Line Segments (Ocaml)
\end_layout

\begin_layout Standard
The error between an infinite line 
\begin_inset Formula $(a,b,c)$
\end_inset

 and a 2D line segment 
\begin_inset Formula $((x1,y1),(x2,y2))$
\end_inset

 is defined in Line3.ml.
\end_layout

\begin_layout Section
Line3vd (Ocaml)
\end_layout

\begin_layout Standard
One representation of a line is through 2 vectors 
\begin_inset Formula $(v,d)$
\end_inset

, where 
\begin_inset Formula $v$
\end_inset

 is the direction and the vector 
\begin_inset Formula $d$
\end_inset

 points from the orgin to the closest point on the line.
\end_layout

\begin_layout Standard
In this representation, transforming a 3D line from a world coordinate frame
 to a camera at 
\begin_inset Formula $(R_{w}^{c},t^{w})$
\end_inset

 is done by
\begin_inset Formula \[
v^{c}=R_{w}^{c}v^{w}\]

\end_inset


\begin_inset Formula \[
d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)\]

\end_inset


\end_layout

\begin_layout Section
Line3 (Ocaml)
\end_layout

\begin_layout Standard
For 3D lines, we use a parameterization due to C.J.
 Taylor, using a rotation matrix 
\begin_inset Formula $R$
\end_inset

 and 2 scalars 
\begin_inset Formula $a$
\end_inset

 and 
\begin_inset Formula $b$
\end_inset

.
 The line direction 
\begin_inset Formula $v$
\end_inset

 is simply the Z-axis of the rotated frame, i.e., 
\begin_inset Formula $v=R_{3}$
\end_inset

, while the vector 
\begin_inset Formula $d$
\end_inset

 is given by 
\begin_inset Formula $d=aR_{1}+bR_{2}$
\end_inset

.
\end_layout

\begin_layout Standard
Now, we will 
\emph on
not
\emph default
 use the incremental rotation scheme we used for rotations: because the
 matrix R translates from the line coordinate frame to the world frame,
 we need to apply the incremental rotation on the right-side:
\begin_inset Formula \[
R'=R(I+\Omega)\]

\end_inset

Projecting a line to 2D can be done easily, as both 
\begin_inset Formula $v$
\end_inset

 and 
\begin_inset Formula $d$
\end_inset

 are also the 2D homogenous coordinates of two points on the projected line,
 and hence we have
\begin_inset Formula \begin{eqnarray*}
l & = & v\times d\\
 & = & R_{3}\times\left(aR_{1}+bR_{2}\right)\\
 & = & a\left(R_{3}\times R_{1}\right)+b\left(R_{3}\times R_{2}\right)\\
 & = & aR_{2}-bR_{1}\end{eqnarray*}

\end_inset

This can be written as a rotation of a point,
\begin_inset Formula \[
l=R\left(\begin{array}{c}
-b\\
a\\
0\end{array}\right)\]

\end_inset

but because the incremental rotation is now done on the right, we need to
 figure out the derivatives again:
\begin_inset Formula \begin{equation}
\frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=R\frac{\partial(\Omega x)}{\partial\omega}=R\Skew{-x}\label{eq:rotateRight}\end{equation}

\end_inset

and hence the derivative of the projection 
\begin_inset Formula $l$
\end_inset

 with respect to the rotation matrix 
\begin_inset Formula $R$
\end_inset

of the 3D line is 
\begin_inset Formula \begin{equation}
\frac{\partial(l)}{\partial\omega}=R\Skew{\left(\begin{array}{c}
b\\
-a\\
0\end{array}\right)}=\left[\begin{array}{ccc}
aR_{3} & bR_{3} & -(aR_{1}+bR_{2})\end{array}\right]\end{equation}

\end_inset

or the 
\begin_inset Formula $a,b$
\end_inset

 scalars:
\begin_inset Formula \[
\frac{\partial(l)}{\partial a}=R_{2}\]

\end_inset


\begin_inset Formula \[
\frac{\partial(l)}{\partial b}=-R_{1}\]

\end_inset


\end_layout

\begin_layout Standard
Transforming a 3D line 
\begin_inset Formula $(R,(a,b))$
\end_inset

 from a world coordinate frame to a camera frame 
\begin_inset Formula $(R_{w}^{c},t^{w})$
\end_inset

 is done by
\end_layout

\begin_layout Standard
\begin_inset Formula \[
R'=R_{w}^{c}R\]

\end_inset


\begin_inset Formula \[
a'=a-R_{1}^{T}t^{w}\]

\end_inset


\begin_inset Formula \[
b'=b-R_{2}^{T}t^{w}\]

\end_inset

Again, we need to redo the derivatives, as R is incremented from the right.
 The first argument is incremented from the left, but the result is incremented
 on the right:
\begin_inset Formula \begin{eqnarray*}
R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\
I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
\Omega' & = & R'^{T}\Skew{S\omega}R'\\
\Omega' & = & \Skew{R'^{T}S\omega}\\
\omega' & = & R'^{T}S\omega\end{eqnarray*}

\end_inset

For the second argument 
\begin_inset Formula $R$
\end_inset

 we now simply have:
\begin_inset Formula \begin{eqnarray*}
AB(I+\Omega') & = & AB(I+\Omega)\\
\Omega' & = & \Omega\\
\omega' & = & \omega\end{eqnarray*}

\end_inset

The scalar derivatives can be found by realizing that 
\begin_inset Formula \[
\left(\begin{array}{c}
a'\\
b'\\
...\end{array}\right)=\left(\begin{array}{c}
a\\
b\\
0\end{array}\right)-R^{T}t^{w}\]

\end_inset

where we don't care about the third row.
 Hence
\begin_inset Formula \[
\frac{\partial(\left(R(I+\Omega_{2})\right)^{T}t^{w})}{\partial\omega}=-\frac{\partial(\Omega_{2}R^{T}t^{w})}{\partial\omega}=-\Skew{R^{T}t^{w}}=\left[\begin{array}{ccc}
0 & R_{3}^{T}t^{w} & -R_{2}^{T}t^{w}\\
-R_{3}^{T}t^{w} & 0 & R_{1}^{T}t^{w}\\
... & ... & 0\end{array}\right]\]

\end_inset


\end_layout

\begin_layout Section

\series bold
Aligning 3D Scans
\end_layout

\begin_layout Standard
Below is the explanaition 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 Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
bibfiles "/Users/dellaert/papers/refs"
options "plain"

\end_inset


\end_layout

\end_body
\end_document