#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 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 . 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 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 \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 Derivatives of Actions \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 When a Lie group \begin_inset Formula $G$ \end_inset acts on a vector space \begin_inset Formula $V$ \end_inset , we are interested in the derivatives of \begin_inset Formula \[ f_{1}\left(g\right)=gv\mbox{ and }f_{2}(v)=gv\] \end_inset with \begin_inset Formula $f_{1}:G\rightarrow V$ \end_inset and \begin_inset Formula $f_{2}:V\rightarrow V$ \end_inset . The brilliance of Lie group theory is that we only need to know how the generators of the group act around the group's identity element \begin_inset Formula $g=id$ \end_inset , and then we can use the Adjoint map to effectuate that action in the correct frame of reference. Specifically, if \begin_inset Formula \[ H_{v}=\left[\begin{array}{ccc} \frac{\partial f_{1}}{\partial x_{1}} & \ldots & \frac{\partial f_{1}}{\partial x_{n}}\end{array}\right]\rvert_{g=id}\] \end_inset is the \begin_inset Formula $m\times n$ \end_inset Jacobian of the group action on \begin_inset Formula $\mbox{v\in}V$ \end_inset with respect to an incremental change \begin_inset Formula $x$ \end_inset , we have \begin_inset Formula \[ \Jac{f_{1}}xg=H_{v}\Ad g\] \end_inset The meaning of \begin_inset Formula $H$ \end_inset will depend on the group \begin_inset Formula $G$ \end_inset and will be very intuitive! \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]\] \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 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 The beauty of Lie group theory comes in play when we talk about the derivative of a group action. 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 \[ \deriv q{\omega}=\deriv{}{\omega}\left(Rp\right)=\deriv{}{\omega}\left(e^{\Skew{\omega}}p\right)\] \end_inset Since \begin_inset Formula \[ e^{A}=I+A+\frac{A^{2}}{2!}+\frac{A^{3}}{3!}+\ldots\] \end_inset and derivative is linear and we are only interested in first order we have \begin_inset Formula \[ \deriv{e^{\Skew{\omega}}}{\omega}=\deriv{\Skew{\omega}}{\omega}=\omega_{x}G_{1}+\omega_{y}G_{2}+\omega_{z}G_{3}\] \end_inset Specifically, the generators for \begin_inset Formula $\SOthree$ \end_inset are \begin_inset Formula \[ G_{1}=\left(\begin{array}{ccc} 0 & 0 & 0\\ 0 & 0 & -1\\ 0 & 1 & 0\end{array}\right)\mbox{}G_{2}=\left(\begin{array}{ccc} 0 & 0 & 1\\ 0 & 0 & 0\\ -1 & 0 & 0\end{array}\right)\mbox{ }G_{1}=\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. When given an incremental angular velocity \begin_inset Formula $\omega$ \end_inset , we obtain the effect of the group action around the identity: \begin_inset Formula \[ H\omega=\omega_{x}G_{1}+\omega_{y}G_{2}+\omega_{z}G_{3}=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y}\\ \omega_{z} & 0 & -\omega_{x}\\ -\omega_{y} & \omega_{x} & 0\end{array}\right]=\Skew{\omega}\] \end_inset Hence, at the origin, the effect of an incremental action \begin_inset Formula $\omega$ \end_inset on a point \begin_inset Formula $p$ \end_inset is a velocity \begin_inset Formula \[ \Skew{\omega}p=\omega\times p\] \end_inset We can write this as a \begin_inset Formula $3\times3$ \end_inset Jacobian \begin_inset Formula $H_{p}$ \end_inset that is multipled with \begin_inset Formula $\omega$ \end_inset , \begin_inset Formula \[ \omega\times p=-p\times\omega=-\Skew p\omega=H_{p}\omega\] \end_inset Now, if we want to apply this in a frame \begin_inset Formula $R$ \end_inset , we need to do something quite similar to the Adjoint map: (a) transform the point \begin_inset Formula $p$ \end_inset to the origin using \begin_inset Formula $R$ \end_inset , apply the action \begin_inset Formula $\Skew{\omega}$ \end_inset , and transform back with \begin_inset Formula $R^{T}$ \end_inset . In short \begin_inset Formula \[ q=R^{T}e^{\Skew{\omega}}Rp=\exp\left(\Ad{R^{T}}\Skew{\omega}\right)p=\exp\left(\Skew{R^{T}\omega}\right)p\] \end_inset and hence the velocity becomes \begin_inset Formula \[ \Skew{R^{T}\omega}\times p=-\Skew pR^{T}\omega=-R^{T}R\Skew pR^{T}\omega=-R^{T}\Skew{Rp}\omega=R^{T}H_{Rp}\omega\] \end_inset This is quite intuitive in hindsight: we transform \begin_inset Formula $p$ \end_inset to \begin_inset Formula $Rp$ \end_inset , calculate the velocity by \begin_inset Formula $H_{Rp}$ \end_inset , and transform back by the rotation \begin_inset Formula $R^{T}$ \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. 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 \[ q=\left[\begin{array}{cc} R & t\\ 0 & 1\end{array}\right]\left[\begin{array}{c} p\\ 1\end{array}\right]\] \end_inset \end_layout \begin_layout Standard 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_{1}=\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 and hence a twist around the origin applies to homogeneous coordinates \begin_inset Formula $\hat{p}\in\mathbb{R}^{4}$ \end_inset as \begin_inset Formula \[ \sum_{i}\xi_{i}G_{i}=\left(\begin{array}{cccc} 0 & -\omega_{z} & \omega_{y} & v_{x}\\ \omega_{z} & 0 & -\omega_{x} & v_{y}\\ -\omega_{y} & \omega_{x} & 0 & v_{z}\\ 0 & 0 & 0 & 0\end{array}\right)=\left[\begin{array}{cc} \Skew{\omega} & v\\ 0 & 0\end{array}\right]=\xihat\] \end_inset Hence, at the origin, an incremental action \begin_inset Formula $\xi$ \end_inset on a point \begin_inset Formula $p$ \end_inset induces the velocity \begin_inset Formula \[ \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 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 Now, if we want to apply this in a frame \begin_inset Formula $T$ \end_inset , we can (1) transform the point \begin_inset Formula $p$ \end_inset to the origin using \begin_inset Formula $T^{-1}$ \end_inset , apply the action \begin_inset Formula $\xihat$ \end_inset , and transform back to \begin_inset Formula $T$ \end_inset . In short \begin_inset Formula \[ \left[\begin{array}{c} q\\ 1\end{array}\right]=Te^{\xihat}T^{-1}\left[\begin{array}{c} p\\ 1\end{array}\right]=\exp\left(\Ad T\xihat\right)\left[\begin{array}{c} p\\ 1\end{array}\right]\] \end_inset To get the velocity of the point in frame \begin_inset Formula $T$ \end_inset , we have \begin_inset Formula \[ H(p)\Ad T\xi=\left[\begin{array}{cc} -\Skew p & I_{3}\end{array}\right]\left[\begin{array}{cc} R & 0\\ \Skew tR & R\end{array}\right]\xi=R\left[\begin{array}{cc} -\Skew{T^{-1}p} & I_{3}\end{array}\right]\xi=H(T^{-1}p)\xi\] \end_inset where I made use of \begin_inset Formula $\Skew{t-p}R=-RR^{T}\Skew{p-t}R=-R\Skew{R^{T}(p-t)}=-R\Skew{T^{-1}p}$ \end_inset . This is intuitive in hindsight: we transform \begin_inset Formula $p$ \end_inset back to the orgin by \begin_inset Formula $T^{-1}p$ \end_inset , apply \begin_inset Formula $H(.)$ \end_inset to get a velocity, and only need the rotation \begin_inset Formula $R$ \end_inset to transform it back to the orginal frame (as velocities are not affected by translation). \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 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 \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 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. 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 Part Old Stuff \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 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