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 false
\use_amsmath 1
\use_esint 0
\cite_engine basic
\use_bibtopic false
\paperorientation portrait
\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 Standard
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset


\end_layout

\begin_layout Standard
This document should be kept up to date and specify how each of the derivatives
 in the geometry modules are computed.
\end_layout

\begin_layout Section
General Lie group derivations
\end_layout

\begin_layout Standard
The derivatives for 
\emph on
compose
\emph default
, 
\emph on
inverse
\emph default
, and 
\emph on
between
\emph default
 can be derived from Lie group principals to work with any transformation
 type.
 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
 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
.
 For example, to find the derivative of a function 
\begin_inset Formula $f\left(X\right)$
\end_inset

, we include the differential changes in the tangent space:
\begin_inset Formula \[
f\left(X\right)\exp\partial f=f\left(X\exp\partial x\right)\]

\end_inset

and then taking the partial derivatives 
\begin_inset Formula $\frac{\partial y}{\partial x}$
\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

\series bold
\emph on
This section is not correct - math doesn't make sense and need to fix.
\end_layout

\begin_layout Standard
Starting with inverse:
\begin_inset Formula \begin{align*}
X^{-1}\exp\partial i & =\left(X\exp\left[\partial x\right]\right)^{-1}\\
 & =\left(\exp-\left[\partial x\right]\right)X^{-1}\\
\exp\partial i & =X\left(\exp-\left[\partial x\right]\right)X^{-1}\\
 & =\exp-X\left[\partial x\right]X^{-1}\\
\partial i & =-X\left[\partial x\right]X^{-1}\end{align*}

\end_inset


\end_layout

\begin_layout Standard
Compose can be derived similarly:
\begin_inset Formula \begin{align*}
AB\exp\partial c & =A\left(\exp\left[\partial a\right]\right)B\\
\exp\partial c & =B^{-1}\left(\exp\left[\partial a\right]\right)B\\
\partial c & =B^{-1}\left[\partial a\right]B\end{align*}

\end_inset


\end_layout

\begin_layout Section
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.
 The derivatives of 
\series bold
\emph on
rotate
\series default
\emph default
 are then found easily, using
\begin_inset Formula \begin{eqnarray*}
\frac{\partial x'}{\partial\delta} & = & \frac{\partial(x\cos\theta'-y\sin\theta')}{\partial\delta}\\
 & = & \frac{\partial(x(\cos\theta\cos\delta-\sin\theta\sin\delta)-y(\sin\theta\cos\delta+\cos\theta\sin\delta))}{\partial\delta}\\
 & = & x(-\cos\theta\sin\delta-\sin\theta\cos\delta)-y(-\sin\theta\sin\delta+\cos\theta\cos\delta)\\
 & = & -x\sin\theta-y\cos\theta=-y'\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula \begin{eqnarray*}
\frac{\partial y'}{\partial\delta} & = & \frac{\partial(x\sin\theta'+y\cos\theta')}{\partial\delta}\\
 & = & \frac{\partial(x(\sin\theta\cos\delta+\cos\theta\sin\delta)+y(\cos\theta\cos\delta-\sin\theta\sin\delta))}{\partial\delta}\\
 & = & x(-\sin\theta\sin\delta+\cos\theta\cos\delta)+y(-\cos\theta\sin\delta-\sin\theta\cos\delta)\\
 & = & x\cos\theta-y\sin\theta=x'\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial p'}{\partial p}=\frac{\partial(Rp)}{\partial p}=R\]

\end_inset

Similarly, unrotate
\end_layout

\begin_layout Standard
\begin_inset Formula \begin{eqnarray*}
\frac{\partial x'}{\partial\delta} & = & \frac{\partial(x\cos\theta'+y\sin\theta')}{\partial\delta}\\
 & = & \frac{\partial(x(\cos\theta\cos\delta-\sin\theta\sin\delta)+y(\sin\theta\cos\delta+\cos\theta\sin\delta))}{\partial\delta}\\
 & = & x(-\cos\theta\sin\delta-\sin\theta\cos\delta)+y(-\sin\theta\sin\delta+\cos\theta\cos\delta)\\
 & = & -x\sin\theta+y\cos\theta=y'\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula \begin{eqnarray*}
\frac{\partial y'}{\partial\delta} & = & \frac{\partial(-x\sin\theta'+y\cos\theta')}{\partial\delta}\\
 & = & \frac{\partial(-x(\sin\theta\cos\delta+\cos\theta\sin\delta)+y(\cos\theta\cos\delta-\sin\theta\sin\delta))}{\partial\delta}\\
 & = & -x(-\sin\theta\sin\delta+\cos\theta\cos\delta)+y(-\cos\theta\sin\delta-\sin\theta\cos\delta)\\
 & = & -x\cos\theta-y\sin\theta=-x'\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial p'}{\partial p}=\frac{\partial(Rp)}{\partial p}=R\]

\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 Section
Rot3
\end_layout

\begin_layout Standard
An incremental rotation is applied as (switched to right-multiply Jan 25
 2010)
\begin_inset Formula \[
R'=R(I+\Omega)\]

\end_inset

where 
\begin_inset Formula $\Omega=\Skew{\omega}$
\end_inset

 is the skew symmetric matrix corresponding to the incremental rotation
 angles 
\begin_inset Formula $\omega=(\omega_{x},\omega_{y},\omega_{z})$
\end_inset

.
 The derivatives of 
\series bold
\emph on
rotate
\series default
\emph default
 are then found easily, using 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:Dcross1"

\end_inset

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

\end_inset


\begin_inset Formula \[
\frac{\partial(Rx)}{\partial x}=R\]

\end_inset


\end_layout

\begin_layout Standard
For composition and transposing of rotation matrices the situation is a
 bit more complex.
 We want to figure out what incremental rotation 
\begin_inset Formula $\Omega'$
\end_inset

 on the composed matrix, will yield the same change as 
\begin_inset Formula $\Omega$
\end_inset

 applied to either the first (
\begin_inset Formula $A$
\end_inset

) or second argument 
\begin_inset Formula $(B$
\end_inset

).
 Hence, the derivative with respect to the second argument is now easy:
\begin_inset Formula \begin{eqnarray*}
(AB)(I+\Omega') & = & A\left[B(I+\Omega)\right]\\
AB+AB\Omega' & = & AB+AB\Omega\\
\Omega' & = & \Omega\\
\omega' & = & \omega\end{eqnarray*}

\end_inset

i.e.
 the derivative is the identity matrix.
\end_layout

\begin_layout Standard
For the first argument of 
\series bold
\emph on
compose
\series default
\emph default
, we will make use of useful property of rotation matrices 
\begin_inset Formula $A$
\end_inset

:
\begin_inset Formula \begin{eqnarray}
A\Omega A^{T} & = & A\Omega\left[\begin{array}{ccc}
a_{1} & a_{2} & a_{3}\end{array}\right]\nonumber \\
 & = & A\left[\begin{array}{ccc}
\omega\times a_{1} & \omega\times a_{2} & \omega\times a_{3}\end{array}\right]\nonumber \\
 & = & \left[\begin{array}{ccc}
a_{1}(\omega\times a_{1}) & a_{1}(\omega\times a_{2}) & a_{1}(\omega\times a_{3})\\
a_{2}(\omega\times a_{1}) & a_{2}(\omega\times a_{2}) & a_{2}(\omega\times a_{3})\\
a_{3}(\omega\times a_{1}) & a_{3}(\omega\times a_{2}) & a_{3}(\omega\times a_{3})\end{array}\right]\nonumber \\
 & = & \left[\begin{array}{ccc}
\omega(a_{1}\times a_{1}) & \omega(a_{2}\times a_{1}) & \omega(a_{3}\times a_{1})\\
\omega(a_{1}\times a_{2}) & \omega(a_{2}\times a_{2}) & \omega(a_{3}\times a_{2})\\
\omega(a_{1}\times a_{3}) & \omega(a_{2}\times a_{3}) & \omega(a_{3}\times a_{3})\end{array}\right]\nonumber \\
 & = & \left[\begin{array}{ccc}
0 & -\omega a_{3} & \omega a_{2}\\
\omega a_{3} & 0 & -\omega a_{1}\\
-\omega a_{2} & \omega a_{1} & 0\end{array}\right]\nonumber \\
 & = & \Skew{A\omega}\label{eq:property1}\end{eqnarray}

\end_inset

where 
\begin_inset Formula $a_{x}$
\end_inset

 are the 
\emph on
rows
\emph default
 of 
\begin_inset Formula $A$
\end_inset

, we made use of the orthogonality of rotation matrices, and the triple
 product rule:
\begin_inset Formula \[
a(b\times c)=b(c\times a)=c(a\times b)\]

\end_inset

The derivative in the first argument of 
\series bold
\emph on
compose
\series default
\emph default
 can then be found using 
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:property1"

\end_inset

: 
\end_layout

\begin_layout Standard
\begin_inset Formula \begin{eqnarray*}
(AB)(I+\Omega') & = & A(I+\Omega)B\\
AB+AB\Omega' & = & AB+A\Omega B\\
B\Omega' & = & \Omega B\\
\Omega' & = & B^{T}\Omega B=\Skew{B^{T}\omega}\\
\omega' & = & B^{T}\omega\end{eqnarray*}

\end_inset

The derivative of 
\series bold
\emph on
transpose
\series default
\emph default
 can be found similarly:
\begin_inset Formula \begin{eqnarray*}
A^{T}(I+\Omega') & = & \left[A(I+\Omega)\right]^{T}\\
A^{T}+A^{T}\Omega' & = & (I-\Omega)A^{T}\\
A^{T}\Omega' & = & -\Omega A^{T}\\
\Omega' & = & -A\Omega A^{T}\\
 & = & -\Skew{A\omega}\\
\omega' & = & -A\omega\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Standard
Let's find the derivative of 
\begin_inset Formula $between(A,B)=compose(inverse(A),B)$
\end_inset

, so
\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial c(i(A),B)}{\partial a}=dc1(i(A),B)di(A)=-B^{T}A=-between(B,A)\]

\end_inset


\begin_inset Formula \[
\frac{\partial c(i(A),B)}{\partial b}=dc2(i(A),B)=I_{3}\]

\end_inset

Similarly, the derivative of 
\begin_inset Formula $unrotate(R,x)=rotate(inverse(R),x)$
\end_inset

, so
\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial r(i(R),x)}{\partial\omega}=dr1(i(R),x)di(R)=R^{T}\Skew xR=\Skew{R^{T}x}\]

\end_inset


\begin_inset Formula \[
\frac{\partial r(i(R),x)}{\partial x}=i(R)=R^{T}\]

\end_inset


\end_layout

\begin_layout Section
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
The derivative of 
\begin_inset Formula $inverse=R^{T},-R^{T}t=R^{T}(I,-t)$
\end_inset

, first derivative of rotation in rotation argument:
\end_layout

\begin_layout Standard
The partials
\begin_inset Formula \[
\frac{\partial\omega^{\prime}}{\partial\omega}=\frac{\partial inv(R)}{\partial\omega}=-R\]

\end_inset


\begin_inset Formula \[
\frac{\partial t^{\prime}}{\partial\omega}=\frac{-\partial unrot(R,t)}{\partial\omega}=-skew(R^{T}t)\]

\end_inset


\begin_inset Formula \[
\frac{\partial\omega^{\prime}}{\partial t}=\mathbf{0}\]

\end_inset


\begin_inset Formula \[
\frac{\partial t^{\prime}}{\partial t}=\frac{-\partial unrot(R,t)}{\partial t}=-R^{T}\]

\end_inset


\series bold
old stuff:
\series default

\begin_inset Formula \begin{eqnarray*}
(I+\Omega')R^{T} & = & \left((I+\Omega)R\right)^{T}\\
R^{T}+\Omega'R^{T} & = & R^{T}(I-\Omega)\\
\Omega'R^{T} & = & -R^{T}\Omega\\
\Omega' & = & -R^{T}\Omega R=-\Skew{R^{T}\omega}\\
\omega' & = & -R^{T}\omega\end{eqnarray*}

\end_inset


\end_layout

\begin_layout Standard
Now 
\series bold
\emph on
compose
\series default
\emph default
, first w.r.t.
 a change in rotation in the first argument:
\begin_inset Formula \begin{align*}
AB & =\left(T_{A}R_{A}T_{B}\right)\left(R_{A}R_{B}\right)\\
\left(T_{A}R_{A}T_{B}\left(I+T^{\prime}\right)\right)\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(T_{A}R_{A}\left(I+\Omega\right)T_{B}\right)\left(R_{A}\left(I+\Omega\right)R_{B}\right)\\
\textrm{translation only:}\\
T_{A}R_{A}T_{B}\left(I+T^{\prime}\right) & =T_{A}R_{A}\left(I+\Omega\right)T_{B}\\
T_{B}\left(I+T^{\prime}\right) & =\left(I+\Omega\right)T_{B}\\
T_{B}+T_{B}T^{\prime} & =T_{B}+\Omega T_{B}\\
T^{\prime} & =T_{B}^{-1}skew(\omega)T_{B}\\
T^{\prime} & =skew(T_{B}\omega)\,???\\
\textrm{rotation only:}\\
R_{A}R_{B}\left(I+\Omega^{\prime}\right) & =R_{A}\left(I+\Omega\right)R_{B}\\
R_{B}\Omega^{\prime} & =\Omega R_{B}\\
\Omega^{\prime} & =R_{B}^{T}\Omega R_{B}\\
 & =skew(R_{B}^{T}\omega)\\
\omega^{\prime} & =R_{B}^{T}\omega\end{align*}

\end_inset


\end_layout

\begin_layout Standard
And w.r.t.
 a rotation in the second argument:
\begin_inset Formula \begin{align*}
\left(T_{A}R_{A}T_{B}\left(I+T^{\prime}\right)\right)\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(T_{A}R_{A}T_{B}\right)\left(R_{A}R_{B}\left(I+\Omega\right)\right)\\
\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(R_{A}R_{B}\left(I+\Omega\right)\right)\\
\omega^{\prime} & =\omega\\
t^{\prime} & =0\end{align*}

\end_inset

w.r.t.
 a translation in the second argument:
\begin_inset Formula \begin{align*}
\left(T_{A}R_{A}T_{B}\left(I+T^{\prime}\right)\right)\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(T_{A}R_{A}T_{B}\left(I+T\right)\right)\left(R_{A}R_{B}\right)\\
\omega^{\prime} & =0\\
t^{\prime} & =t\end{align*}

\end_inset


\end_layout

\begin_layout Standard
Finally, 
\series bold
\emph on
between
\series default
\emph default
 in the first argument:
\begin_inset Formula \begin{align*}
\frac{\partial A^{-1}B}{\partial A} & =\frac{\partial c\left(A^{-1},B\right)}{\partial A^{-1}}\frac{\partial inv(A)}{A}\\
\frac{\partial A^{-1}B}{B} & =\frac{\partial c\left(A^{-1},B\right)}{\partial B}\end{align*}

\end_inset


\end_layout

\begin_layout Section
Pose3 (gtsam, new-style exmap)
\end_layout

\begin_layout Standard
In the new-style exponential map, Pose3 is composed with a delta pose as
 follows
\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'=(I+\Omega)R$
\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula $t'=(I+\Omega)t+dt$
\end_inset


\end_layout

\begin_layout Standard
The derivative of transform_from, 
\begin_inset Formula $Rx+t$
\end_inset

:
\end_layout

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

\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 transform_to, 
\begin_inset Formula $R^{T}(x-t)$
\end_inset

, eludes me.
 The calculation below is just an attempt:
\end_layout

\begin_layout Standard
Noting that 
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\noun off
\color none

\begin_inset Formula $R'^{T}=R^{T}(I-\Omega)$
\end_inset

, and 
\begin_inset Formula $(I-\Omega)(x-(I+\Omega)t)=(I-\Omega)(x-t-\Omega t)=x-t-dt-\Omega x+\Omega^{2}t$
\end_inset


\end_layout

\begin_layout Standard
\begin_inset Formula \[
\frac{\partial(R'^{T}(x-t'))}{\partial\omega}=\frac{\partial(R^{T}(I-\Omega)(x-(I+\Omega)t))}{\partial\omega}=-\frac{\partial(R^{T}(\Omega(x-\Omega t)))}{\partial\omega}\]

\end_inset


\begin_inset Formula \[
-\frac{\partial(\Skew{R^{T}\omega}R^{T}x)}{\partial\omega}=\Skew{R^{T}x}\frac{\partial(R^{T}\omega)}{\partial\omega}=\Skew{R^{T}x}R^{T}\]

\end_inset


\begin_inset Formula \[
=\frac{\partial(R^{T}\Omega^{2}t)}{\partial\omega}+\Skew{R^{T}x}R^{T}\]

\end_inset

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 Section
Line3vd
\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
\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
2D Line Segments
\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

\series bold
Recovering Pose
\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

\end_body
\end_document