#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 Section Review of Lie Groups \end_layout \begin_layout Subsection A Manifold and a Group \end_layout \begin_layout Standard \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 Standard \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 . \end_layout \begin_layout Subsection Lie Algebra \end_layout \begin_layout Standard The Lie Algebra \begin_inset Formula $\gg$ \end_inset is called an algebra because it is endowed with a binary operation, the Lie bracket \begin_inset Formula $[X,Y]$ \end_inset , the properties of which are closely related to the group operation of \begin_inset Formula $G$ \end_inset . For example, in matrix Lie groups, the Lie bracket is given by \begin_inset Formula $[A,B]\define AB-BA$ \end_inset . The Lie bracket does not mimick the group operation, as in non-commutative Lie groups we do not have the usual simplification \begin_inset Formula \[ e^{Z}=e^{X}e^{Y}\neq e^{X+Y}\] \end_inset where \begin_inset Formula $X$ \end_inset , \begin_inset Formula $Y$ \end_inset , and \begin_inset Formula $Z$ \end_inset elements of the Lie algebra \begin_inset Formula $\gg$ \end_inset . Instead, \begin_inset Formula $Z$ \end_inset can be calculated using the Baker-Campbell-Hausdorff (BCH) formula: \begin_inset Foot status collapsed \begin_layout Plain Layout http://en.wikipedia.org/wiki/Baker–Campbell–Hausdorff_formula \end_layout \end_inset \begin_inset Formula \[ Z=X+Y+[X,Y]/2+[X-Y,[X,Y]]/12-[Y,[X,[X,Y]]]/24+\ldots\] \end_inset For commutative groups the bracket is zero and we recover \begin_inset Formula $Z=X+Y$ \end_inset . For non-commutative groups we can use the BCH formula to approximate it. \end_layout \begin_layout Subsection Exponential Coordinates \end_layout \begin_layout Standard 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 mapping \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, and the map is given by \begin_inset Formula \begin{equation} \xhat=\sum_{i=1}^{n}x_{i}G^{i}\label{eq:generators}\end{equation} \end_inset where the \begin_inset Formula $G^{i}$ \end_inset are \begin_inset Formula $n\times n$ \end_inset matrices known as the Lie group generators. The meaning of the map \begin_inset Formula $x\rightarrow\xhat$ \end_inset will depend on the group \begin_inset Formula $G$ \end_inset and will be very intuitive. \end_layout \begin_layout Subsection The Adjoint Map \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 Derivatives of Mappings \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 Section Derivatives of Actions \begin_inset CommandInset label LatexCommand label name "sec:Derivatives-of-Actions" \end_inset \end_layout \begin_layout Standard \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 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 at the origin is \begin_inset Formula \[ H_{p}\define\deriv{e^{\xhat}p}x=\deriv{\left(\xhat p\right)}x\] \end_inset 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 \begin_layout Section 3D Rotations \end_layout \begin_layout Subsection Basics \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]=\omega_{x}G^{x}+\omega_{y}G^{y}+\omega_{z}G^{z}\] \end_inset where the \begin_inset Formula $G^{i}$ \end_inset are the generators for \begin_inset Formula $\SOthree$ \end_inset , \begin_inset Formula \[ G^{x}=\left(\begin{array}{ccc} 0 & 0 & 0\\ 0 & 0 & -1\\ 0 & 1 & 0\end{array}\right)\mbox{}G^{y}=\left(\begin{array}{ccc} 0 & 0 & 1\\ 0 & 0 & 0\\ -1 & 0 & 0\end{array}\right)\mbox{ }G^{z}=\left(\begin{array}{ccc} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0\end{array}\right)\] \end_inset corresponding to a rotation around \begin_inset Formula $X$ \end_inset , \begin_inset Formula $Y$ \end_inset , and \begin_inset Formula $Z$ \end_inset , respectively. The Lie bracket \begin_inset Formula $[x,y]$ \end_inset corresponds to the cross product \begin_inset Formula $x\times y$ \end_inset in \begin_inset Formula $\Rthree$ \end_inset . \end_layout \begin_layout Standard 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 Subsection The Adjoint Map \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 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 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 \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 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 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 Section 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. Hence, with this parameterization, the generators for \begin_inset Formula $\SEthree$ \end_inset are \begin_inset Formula \[ G^{1}=\left(\begin{array}{cccc} 0 & 0 & 0 & 0\\ 0 & 0 & -1 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\end{array}\right)\mbox{}G^{2}=\left(\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0\\ -1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\end{array}\right)\mbox{ }G^{3}=\left(\begin{array}{cccc} 0 & -1 & 0 & 0\\ 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\end{array}\right)\] \end_inset \begin_inset Formula \[ G^{4}=\left(\begin{array}{cccc} 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\end{array}\right)\mbox{}G^{5}=\left(\begin{array}{cccc} 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\end{array}\right)\mbox{ }G^{6}=\left(\begin{array}{cccc} 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\end{array}\right)\] \end_inset 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 Subsection The Adjoint Map \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 \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 \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 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 Section 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 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 \end_layout \begin_layout Subsection Derivatives of Mappings \end_layout \begin_layout Standard 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 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{\theta})=Re^{\skew{\theta}}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{\theta}}p\right)=R\deriv{}{\omega}\left(\skew{\theta}p\right)=RH_{p}\] \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=H_{p}\theta\] \end_inset with \begin_inset Formula $H_{p}=R_{pi/2}p$ \end_inset . Hence, the final derivative of an action in its first argument is \begin_inset Formula \[ \deriv{q(\theta)}{\theta}=RH_{p}=RR_{pi/2}p=R_{pi/2}Rp=R_{pi/2}q\] \end_inset \end_layout \begin_layout Section 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. \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none The Lie group generators are \begin_inset Formula \[ G^{x}=\left[\begin{array}{ccc} 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0\end{array}\right]\mbox{ }G^{y}=\left[\begin{array}{ccc} 0 & 0 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0\end{array}\right]\mbox{ }G^{\theta}=\left[\begin{array}{ccc} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0\end{array}\right]\] \end_inset \family default \series default \shape default \size default \emph default \bar default \noun default \color inherit 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 Subsection The Adjoint Map \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 \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 \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. \end_layout \begin_layout Standard Derivatives of 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 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 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 \begin_layout Standard \begin_inset CommandInset bibtex LatexCommand bibtex bibfiles "/Users/dellaert/papers/refs" options "plain" \end_inset \end_layout \end_body \end_document