diff --git a/doc/math.lyx b/doc/math.lyx
index be743a02b..2a4ba3cec 100644
--- a/doc/math.lyx
+++ b/doc/math.lyx
@@ -20,12 +20,16 @@
 \spacing single
 \use_hyperref false
 \papersize default
-\use_geometry false
+\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
@@ -50,40 +54,253 @@ Geometry Derivatives and Other Hairy Math
 Frank Dellaert
 \end_layout
 
+\begin_layout Section
+Review of Lie Groups
+\end_layout
+
 \begin_layout Standard
 \begin_inset FormulaMacro
-\newcommand{\Skew}[1]{[#1]_{\times}}
-{[#1]_{\times}}
+\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 Standard
-This document should be kept up to date and specify how each of the derivatives
- in the geometry modules are computed.
+\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 Standard
+A Lie group 
+\begin_inset Formula $G$
+\end_inset
+
+ is a manifold that possesses a smooth group operation.
+ Associated with it is a Lie Algebra 
+\begin_inset Formula $\gg$
+\end_inset
+
+ which, loosely speaking, can be identified with the tangent space at the
+ identity and completely defines how the groups behaves around the identity.
+ There is a mapping from 
+\begin_inset Formula $\gg$
+\end_inset
+
+ back to 
+\begin_inset Formula $G$
+\end_inset
+
+, called the exponential map
+\begin_inset Formula \[
+\exp:\gg\rightarrow G\]
+
+\end_inset
+
+and a corresponding inverse 
+\begin_inset Formula \[
+\log:G\rightarrow\gg\]
+
+\end_inset
+
+that maps elements in G to an element in 
+\begin_inset Formula $\gg$
+\end_inset
+
+.
+ For 
+\begin_inset Formula $n$
+\end_inset
+
+-dimensional matrix Lie groups, the Lie algebra 
+\begin_inset Formula $\gg$
+\end_inset
+
+ is isomorphic to 
+\begin_inset Formula $\mathbb{R}^{n}$
+\end_inset
+
+, and we can define the map
+\begin_inset Formula \[
+\hat{}:\mathbb{R}^{n}\rightarrow\gg\]
+
+\end_inset
+
+
+\begin_inset Formula \[
+\hat{}:x\rightarrow\xhat\]
+
+\end_inset
+
+which maps 
+\begin_inset Formula $n$
+\end_inset
+
+-vectors 
+\begin_inset Formula $x\in$
+\end_inset
+
+
+\begin_inset Formula $\Rn$
+\end_inset
+
+ to elements of 
+\begin_inset Formula $\gg$
+\end_inset
+
+.
+ In the case of matrix Lie groups, the elements 
+\begin_inset Formula $\xhat$
+\end_inset
+
+ of 
+\begin_inset Formula $\gg$
+\end_inset
+
+ are 
+\begin_inset Formula $n\times n$
+\end_inset
+
+ matrices.
+\end_layout
+
+\begin_layout Standard
+Below we frequently make use of the equality
+\begin_inset Foot
+status collapsed
+
+\begin_layout Plain Layout
+http://en.wikipedia.org/wiki/Exponential_map
+\end_layout
+
+\end_inset
+
+
+\begin_inset Formula \[
+ge^{\xhat}g^{-1}=e^{\Ad g{\xhat}}\]
+
+\end_inset
+
+where 
+\begin_inset Formula $\Ad g:\gg\rightarrow\mathfrak{\gg}$
+\end_inset
+
+ is a map parameterized by a group element 
+\begin_inset Formula $g$
+\end_inset
+
+.
+ The intuitive explanation is that a change 
+\begin_inset Formula $\exp\left(\xhat\right)$
+\end_inset
+
+ defined around the orgin, but applied at the group element 
+\begin_inset Formula $g$
+\end_inset
+
+, can be written in one step by taking the adjoint 
+\begin_inset Formula $\Ad g{\xhat}$
+\end_inset
+
+ of 
+\begin_inset Formula $\xhat$
+\end_inset
+
+.
+ In the case of a matrix group the ajoint can be written as 
+\begin_inset Foot
+status collapsed
+
+\begin_layout Plain Layout
+http://en.wikipedia.org/wiki/Adjoint_representation_of_a_Lie_group
+\end_layout
+
+\end_inset
+
+
+\begin_inset Formula \[
+\Ad T{\xhat}\define Te^{\xhat}T^{-1}\]
+
+\end_inset
+
+and hence we have
+\end_layout
+
+\begin_layout Standard
+\begin_inset Formula \[
+Te^{\xhat}T^{-1}=e^{T\xhat T^{-1}}\]
+
+\end_inset
+
+where both 
+\begin_inset Formula $T$
+\end_inset
+
+ and 
+\begin_inset Formula $\xhat$
+\end_inset
+
+ are 
+\begin_inset Formula $n\times n$
+\end_inset
+
+ matrices for an 
+\begin_inset Formula $n$
+\end_inset
+
+-dimensional Lie group.
+ Below we introduce the most important Lie groups that we deal with.
 \end_layout
 
 \begin_layout Section
-General Lie group derivations
+Derivatives of Mappings
 \end_layout
 
 \begin_layout Standard
 The derivatives for 
 \emph on
-compose
-\emph default
-, 
-\emph on
-inverse
+inverse, compose
 \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 
+ 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
 
@@ -95,6 +312,10 @@ delta
 \emph on
 output
 \emph default
+ 
+\begin_inset Formula $f(g)$
+\end_inset
+
  that corresponds to a 
 \begin_inset Quotes eld
 \end_inset
@@ -107,23 +328,39 @@ delta
 \emph on
 input
 \emph default
-.
- For example, to find the derivative of a function 
-\begin_inset Formula $f\left(X\right)$
+ 
+\begin_inset Formula $g$
 \end_inset
 
-, we include the differential changes in the tangent space:
+.
+ 
+\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(X\right)\exp\partial f=f\left(X\exp\partial x\right)\]
+f\left(g\right)e^{\yhat}=f\left(ge^{\xhat}\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
+Calculating these derivatives requires that we know the form of the function
  
 \begin_inset Formula $f$
 \end_inset
@@ -131,21 +368,419 @@ and then taking the partial derivatives
 .
 \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
+
+
+\end_layout
+
+\begin_layout Standard
+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 Section
+Important Lie Groups
+\end_layout
+
+\begin_layout Subsection
+3D Rotations
+\end_layout
+
+\begin_layout Standard
+\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 Standard
+The Lie group 
+\begin_inset Formula $\SOthree$
+\end_inset
+
+ is a subgroup of the general linear group 
+\begin_inset Formula $GL(3)$
+\end_inset
+
+ of 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ invertible matrices.
+ Its Lie algebra 
+\begin_inset Formula $\sothree$
+\end_inset
+
+ is the vector space of 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ skew-symmetric matrices.
+ The exponential map can be computed in closed form using Rodrigues' formula.
+\end_layout
+
+\begin_layout Standard
+Since 
+\begin_inset Formula $\SOthree$
+\end_inset
+
+ is a three-dimensional manifold, 
+\begin_inset Formula $\sothree$
+\end_inset
+
+ is isomorphic to 
+\begin_inset Formula $\Rthree$
+\end_inset
+
+ and we define the map
+\begin_inset Formula \[
+\hat{}:\Rthree\rightarrow\sothree\]
+
+\end_inset
+
+
+\begin_inset Formula \[
+\hat{}:\omega\rightarrow\what=\Skew{\omega}\]
+
+\end_inset
+
+which maps 3-vectors 
+\begin_inset Formula $\omega$
+\end_inset
+
+ to skew-symmetric matrices 
+\begin_inset Formula $\Skew{\omega}$
+\end_inset
+
+ :
+\begin_inset Formula \[
+\Skew{\omega}=\left[\begin{array}{ccc}
+0 & -\omega_{z} & \omega_{y}\\
+\omega_{z} & 0 & -\omega_{x}\\
+-\omega_{y} & \omega_{x} & 0\end{array}\right]\]
+
+\end_inset
+
+For every 
+\begin_inset Formula $3-$
+\end_inset
+
+vector 
+\begin_inset Formula $\omega$
+\end_inset
+
+ there is a corresponding rotation matrix
+\begin_inset Formula \[
+R=e^{\Skew{\omega}}\]
+
+\end_inset
+
+and this is defines the canonical parameterization of 
+\begin_inset Formula $\SOthree$
+\end_inset
+
+, with 
+\begin_inset Formula $\omega$
+\end_inset
+
+ known as the canonical or exponential coordinates.
+ It is equivalent to the axis-angle representation for rotations, where
+ the unit vector 
+\begin_inset Formula $\omega/\left\Vert \omega\right\Vert $
+\end_inset
+
+ defines the rotation axis, and its magnitude the amount of rotation 
+\begin_inset Formula $\theta$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Standard
+We can prove the following identity for rotation matrices 
+\begin_inset Formula $R$
+\end_inset
+
+,
+\begin_inset Formula \begin{eqnarray}
+R\Skew{\omega}R^{T} & = & R\Skew{\omega}\left[\begin{array}{ccc}
+a_{1} & a_{2} & a_{3}\end{array}\right]\nonumber \\
+ & = & R\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{R\omega}\label{eq:property1}\end{eqnarray}
+
+\end_inset
+
+where 
+\begin_inset Formula $a_{1}$
+\end_inset
+
+, 
+\begin_inset Formula $a_{2}$
+\end_inset
+
+, and 
+\begin_inset Formula $a_{3}$
+\end_inset
+
+ are the 
 \emph on
-This section is not correct - math doesn't make sense and need to fix.
+rows
+\emph default
+ of 
+\begin_inset Formula $R$
+\end_inset
+
+.
+ Above 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
+
+Hence, given property 
+\begin_inset CommandInset ref
+LatexCommand eqref
+reference "eq:property1"
+
+\end_inset
+
+, the adjoint map for 
+\begin_inset Formula $\sothree$
+\end_inset
+
+ simplifies to
+\begin_inset Formula \[
+\Ad R{\Skew{\omega}}=R\Skew{\omega}R^{T}=\Skew{R\omega}\]
+
+\end_inset
+
+and this can be expressed in exponential coordinates simply by rotating
+ the axis 
+\begin_inset Formula $\omega$
+\end_inset
+
+ to 
+\begin_inset Formula $R\omega$
+\end_inset
+
+.
+ 
 \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*}
+As an example, to apply an axis-angle rotation 
+\begin_inset Formula $\omega$
+\end_inset
+
+ to a point 
+\begin_inset Formula $p$
+\end_inset
+
+ in the frame 
+\begin_inset Formula $R$
+\end_inset
+
+, we could:
+\end_layout
+
+\begin_layout Enumerate
+First transform 
+\begin_inset Formula $p$
+\end_inset
+
+ back to the world frame, apply 
+\begin_inset Formula $\omega$
+\end_inset
+
+, and then rotate back:
+\begin_inset Formula \[
+q=Re^{\Skew{\omega}}R^{T}\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+Immediately apply the transformed axis-angle transformation 
+\begin_inset Formula $\Ad R{\Skew{\omega}}=\Skew{R\omega}$
+\end_inset
+
+:
+\begin_inset Formula \[
+q=e^{\Skew{R\omega}}p\]
 
 \end_inset
 
@@ -153,15 +788,656 @@ X^{-1}\exp\partial i & =\left(X\exp\left[\partial x\right]\right)^{-1}\\
 \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*}
+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
+ in its first argument,
+\begin_inset Formula \begin{eqnarray*}
+\Skew{\omega'} & = & \Ad{R_{2}^{T}}\left(\Skew{\omega}\right)=\Skew{R_{2}^{T}\omega}\\
+\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}} & = & R_{2}^{T}\end{eqnarray*}
+
+\end_inset
+
+compose in its second argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}} & = & I_{3}\end{eqnarray*}
+
+\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
+3D Rigid Transformations
+\end_layout
+
+\begin_layout Standard
+\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
+
+\begin_layout Standard
+The Lie group 
+\begin_inset Formula $\SEthree$
+\end_inset
+
+ is a subgroup of the general linear group 
+\begin_inset Formula $GL(4)$
+\end_inset
+
+ of 
+\begin_inset Formula $4\times4$
+\end_inset
+
+ invertible matrices of the form
+\begin_inset Formula \[
+T\define\left[\begin{array}{cc}
+R & t\\
+0 & 1\end{array}\right]\]
+
+\end_inset
+
+where 
+\begin_inset Formula $R\in\SOthree$
+\end_inset
+
+ is a rotation matrix and 
+\begin_inset Formula $t\in\Rthree$
+\end_inset
+
+ is a translation vector.
+ Its Lie algebra 
+\begin_inset Formula $\sethree$
+\end_inset
+
+ is the vector space of 
+\begin_inset Formula $4\times4$
+\end_inset
+
+ twists 
+\begin_inset Formula $\xihat$
+\end_inset
+
+ parameterized by the 
+\emph on
+twist coordinates
+\emph default
+ 
+\begin_inset Formula $\xi\in\Rsix$
+\end_inset
+
+, with the mapping 
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Murray94book"
+
+\end_inset
+
+ 
+\begin_inset Formula \[
+\xi\define\left[\begin{array}{c}
+\omega\\
+v\end{array}\right]\rightarrow\xihat\define\left[\begin{array}{cc}
+\Skew{\omega} & v\\
+0 & 0\end{array}\right]\]
+
+\end_inset
+
+Note we follow Frank Park's convention and reserve the first three components
+ for rotation, and the last three for translation.
+ Applying the exponential map to a twist 
+\begin_inset Formula $\xi$
+\end_inset
+
+ yields a screw motion yielding an element in 
+\begin_inset Formula $\SEthree$
+\end_inset
+
+: 
+\begin_inset Formula \[
+T=\exp\xihat\]
+
+\end_inset
+
+A closed form solution for the exponential map is given in 
+\begin_inset CommandInset citation
+LatexCommand cite
+after "page 42"
+key "Murray94book"
+
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Standard
+The adjoint is 
+\begin_inset Formula \begin{eqnarray*}
+\Ad T{\xihat} & = & T\xihat T^{-1}\\
+ & = & \left[\begin{array}{cc}
+R & t\\
+0 & 1\end{array}\right]\left[\begin{array}{cc}
+\Skew{\omega} & v\\
+0 & 0\end{array}\right]\left[\begin{array}{cc}
+R^{T} & -R^{T}t\\
+0 & 1\end{array}\right]\\
+ & = & \left[\begin{array}{cc}
+\Skew{R\omega} & -\Skew{R\omega}t+Rv\\
+0 & 0\end{array}\right]\\
+ & = & \left[\begin{array}{cc}
+\Skew{R\omega} & t\times R\omega+Rv\\
+0 & 0\end{array}\right]\end{eqnarray*}
+
+\end_inset
+
+From this we can express the Adjoint map in terms of twist coordinates (see
+ also 
+\begin_inset CommandInset citation
+LatexCommand cite
+key "Murray94book"
+
+\end_inset
+
+ and FP):
+\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 \begin{eqnarray*}
+\frac{\partial T^{-1}}{\partial\xi} & = & -\left[\begin{array}{cc}
+R & 0\\
+\Skew tR & R\end{array}\right]\end{eqnarray*}
+
+\end_inset
+
+(but unit test on the above fails !!!), 
+\series bold
+compose
+\series default
+ in its first argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}} & = & \left[\begin{array}{cc}
+R_{2}^{T} & 0\\
+\Skew{-R_{2}^{T}t}R_{2}^{T} & R_{2}^{T}\end{array}\right]\end{eqnarray*}
+
+\end_inset
+
+compose in its second argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}} & = & I_{6}\end{eqnarray*}
+
+\end_inset
+
+
+\series bold
+between
+\series default
+ in its first argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & -\left[\begin{array}{cc}
+R & 0\\
+\Skew tR & R\end{array}\right]\end{eqnarray*}
+
+\end_inset
+
+with 
+\begin_inset Formula \[
+\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
+2D Rotations
+\end_layout
+
+\begin_layout Standard
+\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 Standard
+The Lie group 
+\begin_inset Formula $\SOtwo$
+\end_inset
+
+ is a subgroup of the general linear group 
+\begin_inset Formula $GL(2)$
+\end_inset
+
+ of 
+\begin_inset Formula $2\times2$
+\end_inset
+
+ invertible matrices.
+ Its Lie algebra 
+\begin_inset Formula $\sotwo$
+\end_inset
+
+ is the vector space of 
+\begin_inset Formula $2\times2$
+\end_inset
+
+ skew-symmetric matrices.
+ Though simpler than 
+\begin_inset Formula $\SOthree$
+\end_inset
+
+ it is 
+\emph on
+commutative
+\emph default
+ and hence things simplify in ways that do not generalize well, so we treat
+ it only now.
+ Since 
+\begin_inset Formula $\SOtwo$
+\end_inset
+
+ is a one-dimensional manifold, 
+\begin_inset Formula $\sotwo$
+\end_inset
+
+ is isomorphic to 
+\begin_inset Formula $\mathbb{R}$
+\end_inset
+
+ and we define
+\begin_inset Formula \[
+\hat{}:\mathbb{R}\rightarrow\sotwo\]
+
+\end_inset
+
+
+\begin_inset Formula \[
+\hat{}:\theta\rightarrow\that=\skew{\theta}\]
+
+\end_inset
+
+which maps the angle 
+\begin_inset Formula $\theta$
+\end_inset
+
+ to the 
+\begin_inset Formula $2\times2$
+\end_inset
+
+ skew-symmetric matrix 
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\noun off
+\color none
+
+\begin_inset Formula $\skew{\theta}$
+\end_inset
+
+:
+\family default
+\series default
+\shape default
+\size default
+\emph default
+\bar default
+\noun default
+\color inherit
+
+\begin_inset Formula \[
+\skew{\theta}=\left[\begin{array}{cc}
+0 & -\theta\\
+\theta & 0\end{array}\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.
+\end_layout
+
+\begin_layout Standard
+The exponential map can be computed in closed form as
+\begin_inset Formula \[
+R=e^{\skew{\theta}}=\left[\begin{array}{cc}
+\cos\theta & -\sin\theta\\
+\sin\theta & \cos\theta\end{array}\right]\]
+
+\end_inset
+
+The adjoint map for 
+\begin_inset Formula $\sotwo$
+\end_inset
+
+ is trivially equal to the identity, as is the case for 
+\emph on
+all
+\emph default
+ commutative groups, and we have the derivative of 
+\series bold
+inverse
+\series default
+,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial R^{T}}{\partial\theta} & = & -\Ad R=-1\end{eqnarray*}
+
+\end_inset
+
+
+\series bold
+compose
+\series default
+ in its first argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(R_{1}R_{2}\right)}{\partial\theta_{1}} & = & \Ad{R_{2}^{T}}=1\end{eqnarray*}
+
+\end_inset
+
+compose in its second argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(R_{1}R_{2}\right)}{\partial\theta_{2}} & = & 1\end{eqnarray*}
+
+\end_inset
+
+
+\series bold
+between
+\series default
+ in its first argument,
+\begin_inset Formula \begin{eqnarray*}
+\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\theta_{1}} & = & -\Ad{R_{2}^{T}R_{1}}=-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\theta_{2}} & = & 1\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsection
+2D Rigid Transformations
+\end_layout
+
+\begin_layout Standard
+\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 Standard
+The Lie group 
+\begin_inset Formula $\SEtwo$
+\end_inset
+
+ is a subgroup of the general linear group 
+\begin_inset Formula $GL(3)$
+\end_inset
+
+ of 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ invertible matrices of the form
+\begin_inset Formula \[
+T\define\left[\begin{array}{cc}
+R & t\\
+0 & 1\end{array}\right]\]
+
+\end_inset
+
+where 
+\begin_inset Formula $R\in\SOtwo$
+\end_inset
+
+ is a rotation matrix and 
+\begin_inset Formula $t\in\Rtwo$
+\end_inset
+
+ is a translation vector.
+ Its Lie algebra 
+\begin_inset Formula $\setwo$
+\end_inset
+
+ is the vector space of 
+\begin_inset Formula $3\times3$
+\end_inset
+
+ twists 
+\begin_inset Formula $\xihat$
+\end_inset
+
+ parameterized by the 
+\emph on
+twist coordinates
+\emph default
+ 
+\begin_inset Formula $\xi\in\Rthree$
+\end_inset
+
+, with the mapping 
+\begin_inset Formula \[
+\xi\define\left[\begin{array}{c}
+v\\
+\omega\end{array}\right]\rightarrow\xihat\define\left[\begin{array}{cc}
+\skew{\omega} & v\\
+0 & 0\end{array}\right]\]
+
+\end_inset
+
+Note we think of robots as having a pose 
+\begin_inset Formula $(x,y,\theta)$
+\end_inset
+
+ and hence I switched the order above, reserving the first two components
+ for translation and the last for rotation.
+ Applying the exponential map to a twist 
+\begin_inset Formula $\xi$
+\end_inset
+
+ yields a screw motion yielding an element in 
+\begin_inset Formula $\SEtwo$
+\end_inset
+
+: 
+\begin_inset Formula \[
+T=\exp\xihat\]
+
+\end_inset
+
+A closed form solution for the exponential map is in the works...
+\end_layout
+
+\begin_layout Standard
+The adjoint is 
+\begin_inset Formula \begin{eqnarray*}
+\Ad T{\xihat} & = & T\xihat T^{-1}\\
+ & = & \left[\begin{array}{cc}
+R & t\\
+0 & 1\end{array}\right]\left[\begin{array}{cc}
+\skew{\omega} & v\\
+0 & 0\end{array}\right]\left[\begin{array}{cc}
+R^{T} & -R^{T}t\\
+0 & 1\end{array}\right]\\
+ & = & \left[\begin{array}{cc}
+\skew{\omega} & -\skew{\omega}t+Rv\\
+0 & 0\end{array}\right]\\
+ & = & \left[\begin{array}{cc}
+\skew{\omega} & Rv-\omega R_{\pi/2}t\\
+0 & 0\end{array}\right]\end{eqnarray*}
+
+\end_inset
+
+From this we can express the Adjoint map in terms of plane twist coordinates:
+\begin_inset Formula \[
+\left[\begin{array}{c}
+v'\\
+\omega'\end{array}\right]=\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
+
+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 Part
+Old Stuff
 \end_layout
 
 \begin_layout Section
@@ -360,179 +1636,6 @@ reference "eq:Dcross1"
 \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
@@ -1263,6 +2366,17 @@ 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
diff --git a/doc/math.pdf b/doc/math.pdf
new file mode 100644
index 000000000..9b1443560
Binary files /dev/null and b/doc/math.pdf differ