#LyX 2.3 created this file. For more info see http://www.lyx.org/ \lyxformat 544 \begin_document \begin_header \save_transient_properties true \origin unavailable \textclass article \use_default_options false \begin_modules eqs-within-sections figs-within-sections theorems-ams-bytype \end_modules \maintain_unincluded_children false \language english \language_package default \inputencoding auto \fontencoding global \font_roman "times" "default" \font_sans "default" "default" \font_typewriter "default" "default" \font_math "auto" "auto" \font_default_family rmdefault \use_non_tex_fonts false \font_sc false \font_osf false \font_sf_scale 100 100 \font_tt_scale 100 100 \use_microtype false \use_dash_ligatures true \graphics default \default_output_format default \output_sync 0 \bibtex_command default \index_command default \paperfontsize 12 \spacing single \use_hyperref false \papersize default \use_geometry true \use_package amsmath 1 \use_package amssymb 1 \use_package cancel 1 \use_package esint 0 \use_package mathdots 1 \use_package mathtools 1 \use_package mhchem 1 \use_package stackrel 0 \use_package stmaryrd 1 \use_package undertilde 1 \cite_engine basic \cite_engine_type default \biblio_style plain \use_bibtopic false \use_indices false \paperorientation portrait \suppress_date false \justification true \use_refstyle 0 \use_minted 0 \index Index \shortcut idx \color #008000 \end_index \leftmargin 1in \topmargin 1in \rightmargin 1in \bottommargin 1in \secnumdepth 3 \tocdepth 3 \paragraph_separation indent \paragraph_indentation default \is_math_indent 0 \math_numbering_side default \quotes_style english \dynamic_quotes 0 \papercolumns 1 \papersides 1 \paperpagestyle default \tracking_changes false \output_changes false \html_math_output 0 \html_css_as_file 0 \html_be_strict false \end_header \begin_body \begin_layout Title Derivatives and Differentials \end_layout \begin_layout Author Frank Dellaert \end_layout \begin_layout Standard \begin_inset Box Frameless position "t" hor_pos "c" has_inner_box 1 inner_pos "t" use_parbox 0 use_makebox 0 width "100col%" special "none" height "1in" height_special "totalheight" thickness "0.4pt" separation "3pt" shadowsize "4pt" framecolor "black" backgroundcolor "none" status collapsed \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\SqrMah}[3]{\Vert{#1}-{#2}\Vert_{#3}^{2}} {\Vert{#1}-{#2}\Vert_{#3}^{2}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\SqrZMah}[2]{\Vert{#1}\Vert_{#2}^{2}} {\Vert{#1}\Vert_{#2}^{2}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Rone}{\mathbb{R}} {\mathbb{R}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Reals}[1]{\mathbb{R}^{#1}} {\mathbb{R}^{#1}} \end_inset \begin_inset FormulaMacro \newcommand{\OneD}[1]{\Reals{#1}\rightarrow\Rone} {\Reals{#1}\rightarrow\Rone} \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Multi}[2]{\Reals{#1}\rightarrow\Reals{#2}} {\Reals{#1}\rightarrow\Reals{#2}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Man}{\mathcal{M}} {\mathcal{M}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset Note Comment status open \begin_layout Plain Layout Derivatives \end_layout \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}} {\frac{\partial#1}{\partial#2}} \end_inset \begin_inset FormulaMacro \newcommand{\at}[2]{#1\biggr\rvert_{#2}} {#1\biggr\rvert_{#2}} \end_inset \begin_inset FormulaMacro \newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} } {\at{\deriv{#1}{#2}}{#3}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset Note Comment status open \begin_layout Plain Layout Lie Groups \end_layout \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\xhat}{\hat{x}} {\hat{x}} \end_inset \begin_inset FormulaMacro \newcommand{\yhat}{\hat{y}} {\hat{y}} \end_inset \begin_inset FormulaMacro \newcommand{\Ad}[1]{Ad_{#1}} {Ad_{#1}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\define}{\stackrel{\Delta}{=}} {\stackrel{\Delta}{=}} \end_inset \begin_inset FormulaMacro \newcommand{\gg}{\mathfrak{g}} {\mathfrak{g}} \end_inset \begin_inset FormulaMacro \newcommand{\Rn}{\mathbb{R}^{n}} {\mathbb{R}^{n}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset Note Comment status open \begin_layout Plain Layout SO(2) \end_layout \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Rtwo}{\mathbb{R}^{2}} {\mathbb{R}^{2}} \end_inset \begin_inset FormulaMacro \newcommand{\SOtwo}{SO(2)} {SO(2)} \end_inset \begin_inset FormulaMacro \newcommand{\sotwo}{\mathfrak{so(2)}} {\mathfrak{so(2)}} \end_inset \begin_inset FormulaMacro \newcommand{\that}{\hat{\theta}} {\hat{\theta}} \end_inset \begin_inset FormulaMacro \newcommand{\skew}[1]{[#1]_{+}} {[#1]_{+}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset Note Comment status open \begin_layout Plain Layout SE(2) \end_layout \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\SEtwo}{SE(2)} {SE(2)} \end_inset \begin_inset FormulaMacro \newcommand{\setwo}{\mathfrak{se(2)}} {\mathfrak{se(2)}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset Note Comment status open \begin_layout Plain Layout SO(3) \end_layout \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Rthree}{\mathbb{R}^{3}} {\mathbb{R}^{3}} \end_inset \begin_inset FormulaMacro \newcommand{\SOthree}{SO(3)} {SO(3)} \end_inset \begin_inset FormulaMacro \newcommand{\sothree}{\mathfrak{so(3)}} {\mathfrak{so(3)}} \end_inset \begin_inset FormulaMacro \newcommand{\what}{\hat{\omega}} {\hat{\omega}} \end_inset \begin_inset FormulaMacro \newcommand{\Skew}[1]{[#1]_{\times}} {[#1]_{\times}} \end_inset \end_layout \begin_layout Plain Layout \begin_inset Note Comment status open \begin_layout Plain Layout SE(3) \end_layout \end_inset \end_layout \begin_layout Plain Layout \begin_inset FormulaMacro \newcommand{\Rsix}{\mathbb{R}^{6}} {\mathbb{R}^{6}} \end_inset \begin_inset FormulaMacro \newcommand{\SEthree}{SE(3)} {SE(3)} \end_inset \begin_inset FormulaMacro \newcommand{\sethree}{\mathfrak{se(3)}} {\mathfrak{se(3)}} \end_inset \begin_inset FormulaMacro \newcommand{\xihat}{\hat{\xi}} {\hat{\xi}} \end_inset \end_layout \end_inset \end_layout \begin_layout Part Theory \end_layout \begin_layout Section Optimization \end_layout \begin_layout Standard We will be concerned with minimizing a non-linear least squares objective of the form \begin_inset Formula \begin{equation} x^{*}=\arg\min_{x}\SqrMah{h(x)}z{\Sigma}\label{eq:objective} \end{equation} \end_inset where \begin_inset Formula $x\in\Man$ \end_inset is a point on an \begin_inset Formula $n$ \end_inset -dimensional manifold (which could be \begin_inset Formula $\Reals n$ \end_inset , an n-dimensional Lie group \begin_inset Formula $G$ \end_inset , or a general manifold \begin_inset Formula $\Man)$ \end_inset , \begin_inset Formula $z\in\Reals m$ \end_inset is an observed measurement, \begin_inset Formula $h:\Man\rightarrow\Reals m$ \end_inset is a measurement function that predicts \begin_inset Formula $z$ \end_inset from \begin_inset Formula $x$ \end_inset , and \begin_inset Formula $\SqrZMah e{\Sigma}\define e^{T}\Sigma^{-1}e$ \end_inset is the squared Mahalanobis distance with covariance \begin_inset Formula $\Sigma$ \end_inset . \end_layout \begin_layout Standard To minimize \begin_inset CommandInset ref LatexCommand eqref reference "eq:objective" \end_inset we need a notion of how the non-linear measurement function \begin_inset Formula $h(x)$ \end_inset behaves in the neighborhood of a linearization point \begin_inset Formula $a$ \end_inset . Loosely speaking, we would like to define an \begin_inset Formula $m\times n$ \end_inset Jacobian matrix \begin_inset Formula $H_{a}$ \end_inset such that \begin_inset Formula \begin{equation} h(a\oplus\xi)\approx h(a)+H_{a}\xi\label{eq:LocalBehavior} \end{equation} \end_inset with \begin_inset Formula $\xi\in\Reals n$ \end_inset , and the operation \begin_inset Formula $\oplus$ \end_inset \begin_inset Quotes eld \end_inset increments \begin_inset Quotes erd \end_inset \begin_inset Formula $a\in\Man$ \end_inset . Below we more formally develop this notion, first for functions from \begin_inset Formula $\Multi nm$ \end_inset , then for Lie groups, and finally for manifolds. \end_layout \begin_layout Standard Once equipped with the approximation \begin_inset CommandInset ref LatexCommand eqref reference "eq:LocalBehavior" \end_inset , we can minimize the objective function \begin_inset CommandInset ref LatexCommand eqref reference "eq:objective" \end_inset with respect to \begin_inset Formula $\delta x$ \end_inset instead: \begin_inset Formula \begin{equation} \xi^{*}=\arg\min_{\xi}\SqrMah{h(a)+H_{a}\xi}z{\Sigma}\label{eq:ApproximateObjective} \end{equation} \end_inset This can be done by setting the derivative of \begin_inset CommandInset ref LatexCommand eqref reference "eq:ApproximateObjective" \end_inset to zero, \begin_inset Note Note status collapsed \begin_layout Plain Layout \begin_inset Formula \[ \frac{1}{2}H_{a}^{T}(h(a)+H_{a}\xi-z)=0 \] \end_inset \end_layout \end_inset yielding the \series bold normal equations \series default , \begin_inset Formula \[ H_{a}^{T}H_{a}\xi=H_{a}^{T}\left(z-h(a)\right) \] \end_inset which can be solved using Cholesky factorization. Of course, we might have to iterate this multiple times, and use a trust-region method to bound \begin_inset Formula $\xi$ \end_inset when the approximation \begin_inset CommandInset ref LatexCommand eqref reference "eq:LocalBehavior" \end_inset is not good. \end_layout \begin_layout Section Multivariate Differentiation \end_layout \begin_layout Subsection Derivatives \end_layout \begin_layout Standard For a vector space \begin_inset Formula $\Reals n$ \end_inset , the notion of an increment is just done by vector addition \begin_inset Formula \[ a\oplus\xi\define a+\xi \] \end_inset and for the approximation \begin_inset CommandInset ref LatexCommand ref reference "eq:LocalBehavior" \end_inset we will use a Taylor expansion using multivariate differentiation. However, loosely following \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" literal "true" \end_inset , we use a perhaps unfamiliar way to define derivatives: \end_layout \begin_layout Definition \begin_inset CommandInset label LatexCommand label name "def:differentiable" \end_inset We define a function \begin_inset Formula $f:\Multi nm$ \end_inset to be \series bold differentiable \series default at \begin_inset Formula $a$ \end_inset if there exists a matrix \begin_inset Formula $f'(a)\in\Reals{m\times n}$ \end_inset such that \begin_inset Formula \[ \lim_{\delta x\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f(a+\xi)\right|}{\left|\xi\right|}=0 \] \end_inset where \begin_inset Formula $\left|e\right|\define\sqrt{e^{T}e}$ \end_inset is the usual norm. If \begin_inset Formula $f$ \end_inset is differentiable, then the matrix \begin_inset Formula $f'(a)$ \end_inset is called the \series bold Jacobian matrix \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset , and the linear map \begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$ \end_inset is called the \series bold derivative \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset . When no confusion is likely, we use the notation \begin_inset Formula $F_{a}\define f'(a)$ \end_inset to stress that \begin_inset Formula $f'(a)$ \end_inset is a matrix. \end_layout \begin_layout Standard The benefit of using this definition is that it generalizes the notion of a scalar derivative \begin_inset Formula $f'(a):\Rone\rightarrow\Rone$ \end_inset to multivariate functions from \begin_inset Formula $\Multi nm$ \end_inset . In particular, the derivative \begin_inset Formula $Df_{a}$ \end_inset maps vector increments \begin_inset Formula $\xi$ \end_inset on \begin_inset Formula $a$ \end_inset to increments \begin_inset Formula $f'(a)\xi$ \end_inset on \begin_inset Formula $f(a)$ \end_inset , such that this linear map locally approximates \begin_inset Formula $f$ \end_inset : \begin_inset Formula \[ f(a+\xi)\approx f(a)+f'(a)\xi \] \end_inset \end_layout \begin_layout Example \begin_inset CommandInset label LatexCommand label name "ex:projection" \end_inset The function \begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$ \end_inset projects a 3D point \begin_inset Formula $(x,y,z)$ \end_inset to the image plane, and has the Jacobian matrix \begin_inset Formula \[ \pi'(x,y,z)=\frac{1}{z}\left[\begin{array}{ccc} 1 & 0 & -x/z\\ 0 & 1 & -y/z \end{array}\right] \] \end_inset \end_layout \begin_layout Subsection Properties of Derivatives \end_layout \begin_layout Standard This notion of a multivariate derivative obeys the usual rules: \end_layout \begin_layout Theorem (Chain rule) If \begin_inset Formula $f:\Multi np$ \end_inset is differentiable at \begin_inset Formula $a$ \end_inset and \begin_inset Formula $g:\Multi pm$ \end_inset is differentiable at \begin_inset Formula $f(a)$ \end_inset , \begin_inset Note Note status collapsed \begin_layout Plain Layout \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none then \begin_inset Formula $D(g\circ f)_{a}=Dg_{f(a)}\circ Df_{a}$ \end_inset and \end_layout \end_inset then the Jacobian matrix \begin_inset Formula $H_{a}$ \end_inset of \begin_inset Formula $h=g\circ f$ \end_inset at \begin_inset Formula $a$ \end_inset is the \begin_inset Formula $m\times n$ \end_inset matrix product \begin_inset Formula \[ H_{a}=G_{f(a)}F_{a} \] \end_inset where \begin_inset Formula $G_{f(a)}$ \end_inset is the \begin_inset Formula $m\times p$ \end_inset Jacobian matrix of \begin_inset Formula $g$ \end_inset evaluated at \begin_inset Formula $f(a)$ \end_inset , and \begin_inset Formula $F_{a}$ \end_inset is the \begin_inset Formula $p\times n$ \end_inset Jacobian matrix of \begin_inset Formula $f$ \end_inset evaluated at \begin_inset Formula $a$ \end_inset . \end_layout \begin_layout Proof See \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" literal "true" \end_inset \end_layout \begin_layout Example \begin_inset CommandInset label LatexCommand label name "ex:chain-rule" \end_inset If we follow the projection \begin_inset Formula $\pi$ \end_inset by a calibration step \begin_inset Formula $\gamma:(x,y)\mapsto(u_{0}+fx,u_{0}+fy)$ \end_inset , with \begin_inset Formula \[ \gamma'(x,y)=\left[\begin{array}{cc} f & 0\\ 0 & f \end{array}\right] \] \end_inset then the combined function \begin_inset Formula $\gamma\circ\pi$ \end_inset has the Jacobian matrix \begin_inset Formula \[ (\gamma\circ\pi)'(x,y)=\frac{f}{z}\left[\begin{array}{ccc} 1 & 0 & -x/z\\ 0 & 1 & -y/z \end{array}\right] \] \end_inset \end_layout \begin_layout Theorem (Inverse) If \begin_inset Formula $f:\Multi nn$ \end_inset is differentiable and has a differentiable inverse \begin_inset Formula $g\define f^{-1}$ \end_inset , then its Jacobian matrix \begin_inset Formula $G_{a}$ \end_inset at \begin_inset Formula $a$ \end_inset is just the inverse of that of \begin_inset Formula $f$ \end_inset , evaluated at \begin_inset Formula $g(a)$ \end_inset : \begin_inset Formula \[ G_{a}=\left[F_{g(a)}\right]^{-1} \] \end_inset \end_layout \begin_layout Proof See \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" literal "true" \end_inset \end_layout \begin_layout Example \begin_inset CommandInset label LatexCommand label name "ex:inverse" \end_inset The function \begin_inset Formula $f:(x,y)\mapsto(x^{2},xy)$ \end_inset has the Jacobian matrix \end_layout \begin_layout Example \begin_inset Formula \[ F_{(x,y)}=\left[\begin{array}{cc} 2x & 0\\ y & x \end{array}\right] \] \end_inset and, for \begin_inset Formula $x\geq0$ \end_inset , its inverse is the function \begin_inset Formula $g:(x,y)\mapsto(x^{1/2},x^{-1/2}y)$ \end_inset with the Jacobian matrix \begin_inset Formula \[ G_{(x,y)}=\frac{1}{2}\left[\begin{array}{cc} x^{-1/2} & 0\\ -x^{-3/2}y & 2x^{-1/2} \end{array}\right] \] \end_inset It is easily verified that \begin_inset Formula \[ g'(a,b)f'(a^{1/2},a^{-1/2}b)=\frac{1}{2}\left[\begin{array}{cc} a^{-1/2} & 0\\ -a^{-3/2}b & 2a^{-1/2} \end{array}\right]\left[\begin{array}{cc} 2a^{1/2} & 0\\ a^{-1/2}b & a^{1/2} \end{array}\right]=\left[\begin{array}{cc} 1 & 0\\ 0 & 1 \end{array}\right] \] \end_inset \end_layout \begin_layout Problem Verify the above for \begin_inset Formula $(a,b)=(4,6)$ \end_inset . Sketch the situation graphically to get insight. \end_layout \begin_layout Subsection Computing Multivariate Derivatives \end_layout \begin_layout Standard Computing derivatives is made easy by defining the concept of a partial derivative: \end_layout \begin_layout Definition For \begin_inset Formula $f:\OneD n$ \end_inset , the \series bold partial derivative \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset , \series bold \series default \begin_inset Formula \[ D_{j}f(a)\define\lim_{h\rightarrow0}\frac{f\left(a^{1},\ldots,a^{j}+h,\ldots,a^{n}\right)-f\left(a^{1},\ldots,a^{n}\right)}{h} \] \end_inset which is the ordinary derivative of the scalar function \begin_inset Formula $g(x)\define f\left(a^{1},\ldots,x,\ldots,a^{n}\right)$ \end_inset . \end_layout \begin_layout Standard Using this definition, one can show that the Jacobian matrix \begin_inset Formula $F_{a}$ \end_inset of a differentiable \emph on multivariate \emph default function \begin_inset Formula $f:\Multi nm$ \end_inset consists simply of the \begin_inset Formula $m\times n$ \end_inset partial derivatives \begin_inset Formula $D_{j}f^{i}(a)$ \end_inset , evaluated at \begin_inset Formula $a\in\Reals n$ \end_inset : \begin_inset Formula \[ F_{a}=\left[\begin{array}{ccc} D_{1}f^{1}(a) & \cdots & D_{n}f^{1}(a)\\ \vdots & \ddots & \vdots\\ D_{1}f^{m}(a) & \ldots & D_{n}f^{m}(a) \end{array}\right] \] \end_inset \end_layout \begin_layout Problem Verify the derivatives in Examples \begin_inset CommandInset ref LatexCommand ref reference "ex:projection" \end_inset to \begin_inset CommandInset ref LatexCommand ref reference "ex:inverse" \end_inset . \end_layout \begin_layout Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Section Multivariate Functions on Lie Groups \end_layout \begin_layout Subsection Lie Groups \end_layout \begin_layout Standard Lie groups are not as easy to treat as the vector space \begin_inset Formula $\Reals n$ \end_inset but nevertheless have a lot of structure. To generalize the concept of the total derivative above we just need to replace \begin_inset Formula $a\oplus\xi$ \end_inset in \begin_inset CommandInset ref LatexCommand eqref reference "eq:ApproximateObjective" \end_inset with a suitable operation in the Lie group \begin_inset Formula $G$ \end_inset . In particular, the notion of an exponential map allows us to define a mapping from \series bold local coordinates \series default \begin_inset Formula $\xi$ \end_inset back to a neighborhood in \begin_inset Formula $G$ \end_inset around \begin_inset Formula $a$ \end_inset , \begin_inset Formula \begin{equation} a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap} \end{equation} \end_inset with \begin_inset Formula $\xi\in\Reals n$ \end_inset for an \begin_inset Formula $n$ \end_inset -dimensional Lie group. Above, \begin_inset Formula $\hat{\xi}\in\mathfrak{g}$ \end_inset is the Lie algebra element corresponding to the vector \begin_inset Formula $\xi$ \end_inset , and \begin_inset Formula $\exp\hat{\xi}$ \end_inset the exponential map. Note that if \begin_inset Formula $G$ \end_inset is equal to \begin_inset Formula $\Reals n$ \end_inset then composing with the exponential map \begin_inset Formula $ae^{\xihat}$ \end_inset is just vector addition \begin_inset Formula $a+\xi$ \end_inset . \end_layout \begin_layout Example For the Lie group \begin_inset Formula $\SOthree$ \end_inset of 3D rotations the vector \begin_inset Formula $\xi$ \end_inset is denoted as \begin_inset Formula $\omega t$ \end_inset and represents an angular displacement. The Lie algebra element \begin_inset Formula $\xihat$ \end_inset is a skew symmetric matrix denoted as \begin_inset Formula $\Skew{\omega t}\in\sothree$ \end_inset , and is given by \begin_inset Formula \[ \Skew{\omega t}=\left[\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y}\\ \omega_{z} & 0 & -\omega_{x}\\ -\omega_{y} & \omega_{x} & 0 \end{array}\right]t \] \end_inset Finally, the increment \begin_inset Formula $a\oplus\xi=ae^{\xihat}$ \end_inset corresponds to an incremental rotation \begin_inset Formula $R\oplus\omega t=Re^{\Skew{\omega t}}$ \end_inset . \end_layout \begin_layout Subsection Local Coordinates vs. Tangent Vectors \end_layout \begin_layout Standard In differential geometry, \series bold tangent vectors \series default \begin_inset Formula $v\in T_{a}G$ \end_inset at \begin_inset Formula $a$ \end_inset are elements of the Lie algebra \begin_inset Formula $\mathfrak{g}$ \end_inset , and are defined as \begin_inset Formula \[ v\define\Jac{\gamma(t)}t{t=0} \] \end_inset where \begin_inset Formula $\gamma$ \end_inset is some curve that passes through \begin_inset Formula $a$ \end_inset at \begin_inset Formula $t=0$ \end_inset , i.e. \begin_inset Formula $\gamma(0)=a$ \end_inset . In particular, for any fixed local coordinate \begin_inset Formula $\xi$ \end_inset the map \begin_inset CommandInset ref LatexCommand eqref reference "eq:expmap" \end_inset can be used to define a \series bold geodesic curve \series default on the group manifold defined by \begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$ \end_inset , and the corresponding tangent vector is given by \begin_inset Formula \begin{equation} \Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector} \end{equation} \end_inset This defines the mapping between local coordinates \begin_inset Formula $\xi\in\Rn$ \end_inset and actual tangent vectors \begin_inset Formula $a\xihat\in g$ \end_inset : the vector \begin_inset Formula $\xi$ \end_inset defines a direction of travel on the manifold, but does so in the local coordinate frame \begin_inset Formula $a$ \end_inset . \end_layout \begin_layout Example Assume a rigid body's attitude is described by \begin_inset Formula $R_{b}^{n}(t)$ \end_inset , where the indices denote the navigation frame \begin_inset Formula $N$ \end_inset and body frame \begin_inset Formula $B$ \end_inset , respectively. An extrinsically calibrated gyroscope measures the angular velocity \begin_inset Formula $\omega^{b}$ \end_inset , in the body frame, and the corresponding tangent vector is \begin_inset Formula \[ \dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}} \] \end_inset \end_layout \begin_layout Subsection Derivatives \end_layout \begin_layout Standard We can generalize Definition \begin_inset CommandInset ref LatexCommand ref reference "def:differentiable" \end_inset to map local coordinates \begin_inset Formula $\xi$ \end_inset to increments \begin_inset Formula $f'(a)\xi$ \end_inset on \begin_inset Formula $f(a)$ \end_inset , such that the linear map \begin_inset Formula $Df_{a}$ \end_inset approximates the function \begin_inset Formula $f$ \end_inset from \begin_inset Formula $G$ \end_inset to \begin_inset Formula $\Reals m$ \end_inset in a neighborhood around \begin_inset Formula $a$ \end_inset : \begin_inset Formula \[ f(ae^{\xihat})\approx f(a)+f'(a)\xi \] \end_inset \end_layout \begin_layout Definition We define a function \begin_inset Formula $f:G\rightarrow\Reals m$ \end_inset to be \series bold differentiable \series default at \begin_inset Formula $a\in G$ \end_inset if there exists a matrix \begin_inset Formula $f'(a)\in\Reals{m\times n}$ \end_inset such that \begin_inset Formula \[ \lim_{\xi\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f(ae^{\hat{\xi}})\right|}{\left|\xi\right|}=0 \] \end_inset If \begin_inset Formula $f$ \end_inset is differentiable, then the matrix \begin_inset Formula $f'(a)$ \end_inset is called the \series bold Jacobian matrix \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset , and the linear map \begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$ \end_inset is called the \series bold derivative \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset . \end_layout \begin_layout Subsection Derivative of an Action \begin_inset CommandInset label LatexCommand label name "sec:Derivatives-of-Actions" \end_inset \end_layout \begin_layout Standard The (usual) action of a matrix group \begin_inset Formula $G$ \end_inset is matrix-vector multiplication on \begin_inset Formula $\mathbb{R}^{n}$ \end_inset , i.e., \begin_inset Formula $f:G\times\Reals n\rightarrow\Reals n$ \end_inset with \begin_inset Formula \[ f(T,p)=Tp \] \end_inset Since this is a function defined on the product \begin_inset Formula $G\times\Reals n$ \end_inset the derivative is a linear transformation \begin_inset Formula $Df:\Multi{m+n}n$ \end_inset with \begin_inset Formula \[ Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p)}\left(\delta p\right) \] \end_inset where \begin_inset Formula $m$ \end_inset is the dimensionality of the manifold \begin_inset Formula $G$ \end_inset . \end_layout \begin_layout Theorem \begin_inset CommandInset label LatexCommand label name "th:Action" \end_inset The Jacobian matrix of the group action \begin_inset Formula $f(T,p)=Tp$ \end_inset at \begin_inset Formula $(T,p)$ \end_inset is given by \begin_inset Formula \[ F_{(T,p)}=\left[\begin{array}{cc} TH(p) & T\end{array}\right]=T\left[\begin{array}{cc} H(p) & I_{n}\end{array}\right] \] \end_inset with \begin_inset Formula $H:\Reals m\rightarrow\Reals{n\times m}$ \end_inset a linear mapping that depends on \begin_inset Formula $p$ \end_inset , and \begin_inset Formula $I_{n}$ \end_inset the \begin_inset Formula $n\times n$ \end_inset identity matrix. \end_layout \begin_layout Proof First, the derivative \begin_inset Formula $D_{2}f$ \end_inset with respect to \begin_inset Formula $p$ \end_inset is easy, as its matrix is simply T: \begin_inset Formula \[ f(T,p+\delta p)=T(p+\delta p)=Tp+T\delta p=f(T,p)+D_{2}f(\delta p) \] \end_inset For the derivative \begin_inset Formula $D_{1}f$ \end_inset with respect to a change in the first argument \begin_inset Formula $T$ \end_inset , we want to find the linear map \begin_inset Formula $D_{1}f$ \end_inset such that \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula \[ Tp+D_{1}f(\xi)\approx f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p \] \end_inset \family default \series default \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit 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 \[ Te^{\hat{\xi}}p\approx T(I+\hat{\xi})p=Tp+T\hat{\xi}p \] \end_inset and \begin_inset Formula $D_{1}f(\xi)=T\hat{\xi}p$ \end_inset . \begin_inset Note Note status collapsed \begin_layout Plain Layout Note also that \begin_inset Formula \[ T\hat{\xi}p=\left(T\hat{\xi}T^{-1}\right)Tp=\left(\Ad T\xihat\right)\left(Tp\right) \] \end_inset \end_layout \end_inset Hence, to complete the proof, we need to show that \begin_inset Formula \begin{equation} \xihat p=H(p)\xi\label{eq:Hp} \end{equation} \end_inset with \begin_inset Formula $H(p)$ \end_inset an \begin_inset Formula $n\times m$ \end_inset matrix that depends on \begin_inset Formula $p$ \end_inset . Expressing the map \begin_inset Formula $\xi\rightarrow\hat{\xi}$ \end_inset in terms of the Lie algebra generators \begin_inset Formula $G^{i}$ \end_inset , using tensors and Einstein summation, we have \begin_inset Formula $\hat{\xi}_{j}^{i}=G_{jk}^{i}\xi^{k}$ \end_inset allowing us to calculate \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none \begin_inset Formula $\hat{\xi}p$ \end_inset \family default \series default \shape default \size default \emph default \bar default \noun default \color inherit as \begin_inset Formula \[ \left(\hat{\xi}p\right)^{i}=\hat{\xi}_{j}^{i}p^{j}=G_{jk}^{i}\xi^{k}p^{j}=\left(G_{jk}^{i}p^{j}\right)\xi^{k}=H_{k}^{i}(p)\xi^{k} \] \end_inset \end_layout \begin_layout Example For 3D rotations \begin_inset Formula $R\in\SOthree$ \end_inset , we have \begin_inset Formula $\hat{\omega}=\Skew{\omega}$ \end_inset and \begin_inset Formula \[ G_{k=1}:\left(\begin{array}{ccc} 0 & 0 & 0\\ 0 & 0 & -1\\ 0 & 1 & 0 \end{array}\right)\mbox{}G_{k=2}:\left(\begin{array}{ccc} 0 & 0 & 1\\ 0 & 0 & 0\\ -1 & 0 & 0 \end{array}\right)\mbox{ }G_{k=3}:\left(\begin{array}{ccc} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0 \end{array}\right) \] \end_inset \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none The matrices \begin_inset Formula $\left(G_{k}^{i}\right)_{j}$ \end_inset are obtained by assembling the \begin_inset Formula $j^{th}$ \end_inset columns of the generators above, yielding \begin_inset Formula $H(p)$ \end_inset equal to: \begin_inset Formula \[ \left(\begin{array}{ccc} 0 & 0 & 0\\ 0 & 0 & 1\\ 0 & -1 & 0 \end{array}\right)p^{1}+\left(\begin{array}{ccc} 0 & 0 & -1\\ 0 & 0 & 0\\ 1 & 0 & 0 \end{array}\right)p^{2}+\left(\begin{array}{ccc} 0 & 1 & 0\\ -1 & 0 & 0\\ 0 & 0 & 0 \end{array}\right)p^{3}=\left(\begin{array}{ccc} 0 & p^{3} & -p^{2}\\ -p^{3} & 0 & p^{1}\\ p^{2} & -p^{1} & 0 \end{array}\right)=\Skew{-p} \] \end_inset \family default \series default \shape default \size default \emph default \bar default \noun default \color inherit Hence, the Jacobian matrix of \begin_inset Formula $f(R,p)=Rp$ \end_inset is given by \begin_inset Formula \[ F_{(R,p)}=R\left(\begin{array}{cc} \Skew{-p} & I_{3}\end{array}\right) \] \end_inset \end_layout \begin_layout Subsection Derivative of an Inverse Action \end_layout \begin_layout Standard Applying the action by the inverse of \begin_inset Formula $T\in G$ \end_inset yields a function \begin_inset Formula $g:G\times\Reals n\rightarrow\Reals n$ \end_inset defined by \begin_inset Formula \[ g(T,p)=T^{-1}p \] \end_inset \end_layout \begin_layout Theorem \begin_inset CommandInset label LatexCommand label name "Th:InverseAction" \end_inset The Jacobian matrix of the inverse group action \begin_inset Formula $g(T,p)=T^{-1}p$ \end_inset is given by \begin_inset Formula \[ G_{(T,p)}=\left[\begin{array}{cc} -H(T^{-1}p) & T^{-1}\end{array}\right] \] \end_inset where \begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$ \end_inset is the same mapping as before. \end_layout \begin_layout Proof Again, the derivative \begin_inset Formula $D_{2}g$ \end_inset with respect to in \begin_inset Formula $p$ \end_inset is easy, the matrix of which is simply \begin_inset Formula $T^{-1}$ \end_inset : \begin_inset Formula \[ g(T,p+\delta p)=T^{-1}(p+\delta p)=T^{-1}p+T^{-1}\delta p=g(T,p)+D_{2}g(\delta p) \] \end_inset Conversely, a change in \begin_inset Formula $T$ \end_inset yields \begin_inset Formula \[ g(Te^{\xihat},p)=\left(Te^{\xihat}\right)^{-1}p=e^{-\xihat}T^{-1}p \] \end_inset Similar to before, if we expand the matrix exponential we get \begin_inset Formula \[ e^{-A}=I-A+\frac{A^{2}}{2!}-\frac{A^{3}}{3!}+\ldots \] \end_inset so \begin_inset Formula \[ e^{-\xihat}T^{-1}p\approx(I-\xihat)T^{-1}p=g(T,p)-\xihat\left(T^{-1}p\right) \] \end_inset \end_layout \begin_layout Example For 3D rotations \begin_inset Formula $R\in\SOthree$ \end_inset we have \begin_inset Formula $R^{-1}=R^{T}$ \end_inset , \begin_inset Formula $H(p)=-\Skew p$ \end_inset , and hence the Jacobian matrix of \begin_inset Formula $g(R,p)=R^{T}p$ \end_inset is given by \begin_inset Formula \[ G_{(R,p)}=\left(\begin{array}{cc} \Skew{R^{T}p} & R^{T}\end{array}\right) \] \end_inset \end_layout \begin_layout Standard \begin_inset Note Note status collapsed \begin_layout Plain Layout My earlier attempt: because the wedge operator is linear, we have \end_layout \begin_layout Plain Layout \begin_inset Formula \begin{eqnarray*} f(\xi+x) & = & \exp\widehat{\left(\xi+x\right)}\\ & = & \exp\left(\xihat+\hat{x}\right) \end{eqnarray*} \end_inset However, except for commutative Lie groups, it is not true that \begin_inset Formula $\exp\left(\xihat+\hat{x}\right)=\exp\xihat\exp\hat{x}$ \end_inset . However, if we expand the matrix exponential to second order and assume \begin_inset Formula $x\rightarrow0$ \end_inset we do have \begin_inset Formula \[ \exp\left(\xihat+\hat{x}\right)\approx I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat \] \end_inset Now, if we ask what \begin_inset Formula $\hat{y}$ \end_inset would effect the same change: \begin_inset Formula \begin{eqnarray*} \exp\xihat\exp\yhat & = & I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat\\ \exp\xihat(I+\yhat) & = & I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat\\ \left(\exp\xihat\right)\yhat & = & \xhat+\xhat\xihat \end{eqnarray*} \end_inset \end_layout \end_inset \end_layout \begin_layout Section Instantaneous Velocity \end_layout \begin_layout Standard For matrix Lie groups, if we have a matrix \begin_inset Formula $T_{b}^{n}(t)$ \end_inset that depends on a parameter \begin_inset Formula $t$ \end_inset , i.e., \begin_inset Formula $T_{b}^{n}(t)$ \end_inset follows a curve on the manifold, then it would be of interest to find the velocity of a point \begin_inset Formula $q^{n}(t)=T_{b}^{n}(t)p^{b}$ \end_inset acted upon by \begin_inset Formula $T_{b}^{n}(t)$ \end_inset . We can express the velocity of \begin_inset Formula $q(t)$ \end_inset in both the n-frame and b-frame: \begin_inset Formula \[ \dot{q}^{n}=\dot{T}_{b}^{n}p^{b}=\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}p^{n}\mbox{\,\,\,\,\ and\,\,\,\,}\dot{q}^{b}=\left(T_{b}^{n}\right)^{-1}\dot{q}^{n}=\left(T_{b}^{n}\right)^{-1}\dot{T}_{b}^{n}p^{b} \] \end_inset Both the matrices \begin_inset Formula $\xihat_{nb}^{n}\define\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}$ \end_inset and \begin_inset Formula $\xihat_{nb}^{b}\define\left(T_{b}^{n}\right)^{-1}\dot{T}_{b}^{n}$ \end_inset are skew-symmetric Lie algebra elements that describe the \series bold instantaneous velocity \series default \begin_inset CommandInset citation LatexCommand cite after "page 51 for rotations, page 419 for SE(3)" key "Murray94book" literal "true" \end_inset . We will revisit this for both rotations and rigid 3D transformations. \end_layout \begin_layout Section Differentials: Smooth Mapping between Lie Groups \end_layout \begin_layout Subsection Motivation and Definition \end_layout \begin_layout Standard The above shows how to compute the derivative of a function \begin_inset Formula $f:G\rightarrow\Reals m$ \end_inset . However, what if the argument to \begin_inset Formula $f$ \end_inset is itself the result of a mapping between Lie groups? In other words, \begin_inset Formula $f=g\circ\varphi$ \end_inset , with \begin_inset Formula $g:G\rightarrow\Reals m$ \end_inset and where \begin_inset Formula $\varphi:H\rightarrow G$ \end_inset is a smooth mapping from the \begin_inset Formula $n$ \end_inset -dimensional Lie group \begin_inset Formula $H$ \end_inset to the \begin_inset Formula $p$ \end_inset -dimensional Lie group \begin_inset Formula $G$ \end_inset . In this case, one would expect that we can arrive at \begin_inset Formula $Df_{a}$ \end_inset by composing linear maps, as follows: \begin_inset Formula \[ f'(a)=(g\circ\varphi)'(a)=G_{\varphi(a)}\varphi'(a) \] \end_inset where \begin_inset Formula $\varphi'(a)$ \end_inset is an \begin_inset Formula $n\times p$ \end_inset matrix that is the best linear approximation to the map \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula $\varphi:H\rightarrow G$ \end_inset . The corresponding linear map \begin_inset Formula $D\varphi_{a}$ \end_inset is called the \family default \series bold \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit differential \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \family default \series default \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit or \series bold pushforward \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none of \begin_inset Formula $ $ \end_inset the mapping \begin_inset Formula $\varphi$ \end_inset at \begin_inset Formula $a$ \end_inset . \end_layout \begin_layout Standard \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none Because a rigorous definition will lead us too far astray, here we only informally define the pushforward of \begin_inset Formula $\varphi$ \end_inset at \begin_inset Formula $a$ \end_inset as the linear map \begin_inset Formula $D\varphi_{a}:\Multi np$ \end_inset such that \begin_inset Formula $D\varphi_{a}\left(\xi\right)\define\varphi'(a)\xi$ \end_inset and \family default \series default \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit \begin_inset Formula \begin{equation} \varphi\left(ae^{\xihat}\right)\approx\varphi\left(a\right)\exp\left(\widehat{\varphi'(a)\xi}\right)\label{eq:pushforward} \end{equation} \end_inset with equality for \begin_inset Formula $\xi\rightarrow0$ \end_inset . We call \begin_inset Formula $\varphi'(a)$ \end_inset the \series bold Jacobian matrix \series default of the map \begin_inset Formula $\varphi$ \end_inset at \begin_inset Formula $a$ \end_inset . Below we show that even with this informal definition we can deduce the pushforward in a number of useful cases. \end_layout \begin_layout Subsection Left Multiplication with a Constant \end_layout \begin_layout Theorem Suppose \begin_inset Formula $G$ \end_inset is an \begin_inset Formula $n$ \end_inset -dimensional Lie group, and \begin_inset Formula $\varphi:G\rightarrow G$ \end_inset is defined as \begin_inset Formula $\varphi(g)=hg$ \end_inset , with \begin_inset Formula $h\in G$ \end_inset a constant. Then \begin_inset Formula $D\varphi_{a}$ \end_inset is the identity mapping and \begin_inset Formula \[ \varphi'(a)=I_{n} \] \end_inset \end_layout \begin_layout Proof Defining \begin_inset Formula $y=D\varphi_{a}x$ \end_inset as in \begin_inset CommandInset ref LatexCommand eqref reference "eq:pushforward" \end_inset , we have \begin_inset Formula \begin{eqnarray*} \varphi(a)e^{\yhat} & = & \varphi(ae^{\xhat})\\ hae^{\yhat} & = & hae^{\xhat}\\ y & = & x \end{eqnarray*} \end_inset \end_layout \begin_layout Subsection Pushforward of the Inverse Mapping \end_layout \begin_layout Standard A well known property of Lie groups is the the fact that applying an incremental change \begin_inset Formula $\xihat$ \end_inset in a different frame \begin_inset Formula $g$ \end_inset can be applied in a single step by applying the change \begin_inset Formula $Ad_{g}\xihat$ \end_inset in the original frame, \begin_inset Formula \begin{equation} ge^{\xihat}g^{-1}=\exp\left(Ad_{g}\xihat\right)\label{eq:Adjoint2} \end{equation} \end_inset where \begin_inset Formula $Ad_{g}:\mathfrak{g}\rightarrow\mathfrak{g}$ \end_inset is the \series bold adjoint representation \series default . This comes in handy in the following: \end_layout \begin_layout Theorem Suppose that \begin_inset Formula $\varphi:G\rightarrow G$ \end_inset is defined as the mapping from an element \begin_inset Formula $g$ \end_inset to its \series bold inverse \series default \begin_inset Formula $g^{-1}$ \end_inset , i.e., \begin_inset Formula $\varphi(g)=g^{-1}$ \end_inset , then the pushforward \begin_inset Formula $D\varphi_{a}$ \end_inset satisfies \begin_inset Formula \begin{align} \left(D\varphi_{a}x\right)\hat{} & =-Ad_{a}\xhat\label{eq:Dinverse} \end{align} \end_inset \end_layout \begin_layout Standard \begin_inset ERT status open \begin_layout Plain Layout \backslash noindent \end_layout \end_inset In other words, and this is intuitive in hindsight, approximating the inverse is accomplished by negation of \begin_inset Formula $\xihat$ \end_inset , along with an adjoint to make sure it is applied in the right frame. \begin_inset ERT status open \begin_layout Plain Layout \backslash noindent \end_layout \end_inset Note, however, that \begin_inset CommandInset ref LatexCommand eqref reference "eq:Dinverse" \end_inset does not immediately yield a useful expression for the Jacobian matrix \begin_inset Formula $\varphi'(a)$ \end_inset , but in many important cases this will turn out to be easy. \end_layout \begin_layout Proof Defining \begin_inset Formula $y=D\varphi_{a}x$ \end_inset as in \begin_inset CommandInset ref LatexCommand eqref reference "eq:pushforward" \end_inset , we have \begin_inset Formula \begin{eqnarray*} \varphi(a)e^{\yhat} & = & \varphi(ae^{\xhat})\\ a^{-1}e^{\yhat} & = & \left(ae^{\xhat}\right)^{-1}\\ e^{\yhat} & = & ae^{-\xhat}a^{-1}\\ \yhat & = & -\Ad a\xhat \end{eqnarray*} \end_inset \end_layout \begin_layout Example For 3D rotations \begin_inset Formula $R\in\SOthree$ \end_inset we have \begin_inset Formula \[ Ad_{g}(\hat{\omega})=R\hat{\omega}R^{T}=\Skew{R\omega} \] \end_inset and hence the pushforward for the inverse mapping \begin_inset Formula $\varphi(R)=R^{T}$ \end_inset has the matrix \begin_inset Formula $\varphi'(R)=-R$ \end_inset . \end_layout \begin_layout Subsection Right Multiplication with a Constant \end_layout \begin_layout Theorem Suppose \begin_inset Formula $\varphi:G\rightarrow G$ \end_inset is defined as \begin_inset Formula $\varphi(g)=gh$ \end_inset , with \begin_inset Formula $h\in G$ \end_inset a constant. Then \begin_inset Formula $D\varphi_{a}$ \end_inset satisfies \begin_inset Formula \[ \left(D\varphi_{a}x\right)\hat{}=\Ad{h^{-1}}\xhat \] \end_inset \end_layout \begin_layout Proof Defining \begin_inset Formula $y=D\varphi_{a}x$ \end_inset as in \begin_inset CommandInset ref LatexCommand eqref reference "eq:pushforward" \end_inset , we have \begin_inset Formula \begin{align*} \varphi(a)e^{\yhat} & =\varphi(ae^{\xhat})\\ ahe & =ae^{\xhat}h\\ e^{\yhat} & =h^{-1}e^{\xhat}h=\exp\left(\Ad{h^{-1}}\xhat\right)\\ \yhat & =\Ad{h^{-1}}\xhat \end{align*} \end_inset \end_layout \begin_layout Example In the case of 3D rotations, right multiplication with a constant rotation \begin_inset Formula $R$ \end_inset is done through the mapping \begin_inset Formula $\varphi(A)=AR$ \end_inset , and satisfies \begin_inset Formula \[ \Skew{D\varphi_{A}x}=\Ad{R^{T}}\Skew x \] \end_inset For 3D rotations \begin_inset Formula $R\in\SOthree$ \end_inset we have \begin_inset Formula \[ Ad_{R^{T}}(\hat{\omega})=R^{T}\hat{\omega}R=\Skew{R^{T}\omega} \] \end_inset and hence the Jacobian matrix of \begin_inset Formula $\varphi$ \end_inset at \begin_inset Formula $A$ \end_inset is \begin_inset Formula $\varphi'(A)=R^{T}$ \end_inset . \end_layout \begin_layout Subsection Pushforward of Compose \end_layout \begin_layout Theorem If we define the mapping \begin_inset Formula $\varphi:G\times G\rightarrow G$ \end_inset as the product of two group elements \begin_inset Formula $g,h\in G$ \end_inset , i.e., \begin_inset Formula $\varphi(g,h)=gh$ \end_inset , then the pushforward will satisfy \begin_inset Formula \[ D\varphi_{(a,b)}(x,y)=D_{1}\varphi_{(a,b)}x+D_{2}\varphi_{(a,b)}y \] \end_inset with \begin_inset Formula \[ \left(D_{1}\varphi_{(a,b)}x\right)\hat{}=\Ad{b^{-1}}\xhat\mbox{\;\ and\;}D_{2}\varphi_{(a,b)}y=y \] \end_inset \end_layout \begin_layout Proof Looking at the first argument, the proof is very similar to right multiplication with a constant \begin_inset Formula $b$ \end_inset . Indeed, defining \begin_inset Formula $y=D\varphi_{a}x$ \end_inset as in \begin_inset CommandInset ref LatexCommand eqref reference "eq:pushforward" \end_inset , we have \begin_inset Formula \begin{align} \varphi(a,b)e^{\yhat} & =\varphi(ae^{\xhat},b)\nonumber \\ abe^{\yhat} & =ae^{\xhat}b\nonumber \\ e^{\yhat} & =b^{-1}e^{\xhat}b=\exp\left(\Ad{b^{-1}}\xhat\right)\nonumber \\ \yhat & =\Ad{b^{-1}}\xhat\label{eq:Dcompose1} \end{align} \end_inset In other words, to apply an incremental change \begin_inset Formula $\xhat$ \end_inset to \begin_inset Formula $a$ \end_inset we first need to undo \begin_inset Formula $b$ \end_inset , then apply \begin_inset Formula $\xhat$ \end_inset , and then apply \begin_inset Formula $b$ \end_inset again. Using \begin_inset CommandInset ref LatexCommand eqref reference "eq:Adjoint2" \end_inset this can be done in one step by simply applying \begin_inset Formula $\Ad{b^{-1}}\xhat$ \end_inset . \end_layout \begin_layout Proof The second argument is quite a bit easier and simply yields the identity mapping: \begin_inset Formula \begin{align} \varphi(a,b)e^{\yhat} & =\varphi(a,be^{\xhat})\nonumber \\ abe^{\yhat} & =abe^{\xhat}\nonumber \\ y & =x\label{eq:Dcompose2} \end{align} \end_inset \end_layout \begin_layout Standard \begin_inset Note Note status open \begin_layout Plain Layout In summary, the Jacobian matrix of \begin_inset Formula $\varphi(g,h)=gh$ \end_inset at \begin_inset Formula $(a,b)\in G\times G$ \end_inset is given by \begin_inset Formula \[ \varphi'(a,b)=? \] \end_inset \end_layout \end_inset \end_layout \begin_layout Example For 3D rotations \begin_inset Formula $A,B\in\SOthree$ \end_inset we have \begin_inset Formula $\varphi(A,B)=AB$ \end_inset , and \begin_inset Formula $\Ad{B^{T}}\Skew{\omega}=\Skew{B^{T}\omega}$ \end_inset , hence the Jacobian matrix \begin_inset Formula $\varphi'(A,B)$ \end_inset of composing two rotations is given by \begin_inset Formula \[ \varphi'(A,B)=\left[\begin{array}{cc} B^{T} & I_{3}\end{array}\right] \] \end_inset \end_layout \begin_layout Subsection \begin_inset CommandInset label LatexCommand label name "subsec:Pushforward-of-Between" \end_inset Pushforward of Between \end_layout \begin_layout Standard Finally, let us find the pushforward of \series bold between \series default , defined as \begin_inset Formula $\varphi(g,h)=g^{-1}h$ \end_inset . For the first argument we reason as: \begin_inset Formula \begin{align} \varphi(g,h)e^{\yhat} & =\varphi(ge^{\xhat},h)\nonumber \\ g^{-1}he^{\yhat} & =\left(ge^{\xhat}\right)^{-1}h=e^{-\xhat}g^{-1}h\nonumber \\ e^{\yhat} & =\left(h^{-1}g\right)e^{-\xhat}\left(h^{-1}g\right)^{-1}=\exp\Ad{\left(h^{-1}g\right)}(-\xhat)\nonumber \\ \yhat & =-\Ad{\left(h^{-1}g\right)}\xhat=-\Ad{\varphi\left(h,g\right)}\xhat\label{eq:Dbetween1} \end{align} \end_inset The second argument yields the identity mapping. \end_layout \begin_layout Example For 3D rotations \begin_inset Formula $A,B\in\SOthree$ \end_inset we have \begin_inset Formula $\varphi(A,B)=A^{T}B$ \end_inset , and \begin_inset Formula $\Ad{B^{T}A}\Skew{-\omega}=\Skew{-B^{T}A\omega}$ \end_inset , hence the Jacobian matrix \begin_inset Formula $\varphi'(A,B)$ \end_inset of between is given by \begin_inset Formula \[ \varphi'(A,B)=\left[\begin{array}{cc} \left(-B^{T}A\right) & I_{3}\end{array}\right] \] \end_inset \end_layout \begin_layout Subsection Numerical PushForward \end_layout \begin_layout Standard Let's examine \begin_inset Formula \[ f\left(g\right)e^{\yhat}=f\left(ge^{\xhat}\right) \] \end_inset and multiply with \begin_inset Formula $f(g)^{-1}$ \end_inset on both sides: \begin_inset Formula \[ e^{\yhat}=f\left(g\right)^{-1}f\left(ge^{\xhat}\right) \] \end_inset We then take the log (which in our case returns \begin_inset Formula $y$ \end_inset , not \begin_inset Formula $\yhat$ \end_inset ): \begin_inset Formula \[ y(x)=\log\left[f\left(g\right)^{-1}f\left(ge^{\xhat}\right)\right] \] \end_inset Let us look at \begin_inset Formula $x=0$ \end_inset , and perturb in direction \begin_inset Formula $i$ \end_inset , \begin_inset Formula $e_{i}=[0,0,1,0,0]$ \end_inset . Then take derivative, \begin_inset Formula \[ \deriv{y(d)}d\define\lim_{d\rightarrow0}\frac{y(d)-y(0)}{d}=\lim_{d\rightarrow0}\frac{1}{d}\log\left[f\left(g\right)^{-1}f\left(ge^{\widehat{de_{i}}}\right)\right] \] \end_inset which is the basis for a numerical derivative scheme. \begin_inset Note Note status collapsed \begin_layout Plain Layout Not understood yet: Let us also look at a chain rule. If we know the behavior at the origin \begin_inset Formula $I$ \end_inset , we can extrapolate \begin_inset Formula \[ f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g) \] \end_inset \end_layout \end_inset \end_layout \begin_layout Subsection Derivative of the Exponential Map \end_layout \begin_layout Theorem \begin_inset CommandInset label LatexCommand label name "D-exp" \end_inset The derivative of the function \begin_inset Formula $f:\Reals n\rightarrow G$ \end_inset that applies the wedge operator followed by the exponential map, i.e., \begin_inset Formula $f(\xi)=\exp\xihat$ \end_inset , is the identity map for \begin_inset Formula $\xi=0$ \end_inset . \end_layout \begin_layout Proof For \begin_inset Formula $\xi=0$ \end_inset , we have \begin_inset Formula \begin{eqnarray*} f(\xi)e^{\yhat} & = & f(\xi+x)\\ f(0)e^{\yhat} & = & f(0+x)\\ e^{\yhat} & = & e^{\xhat} \end{eqnarray*} \end_inset \end_layout \begin_layout Corollary The derivative of the inverse \begin_inset Formula $f^{-1}$ \end_inset is the identity as well, i.e., for \begin_inset Formula $T=e$ \end_inset , the identity element in \begin_inset Formula $G$ \end_inset . \end_layout \begin_layout Standard For \begin_inset Formula $\xi\neq0$ \end_inset , things are not simple. As with pushforwards above, we will be looking for an \begin_inset Formula $n\times n$ \end_inset \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none Jacobian \begin_inset Formula $f'(\xi)$ \end_inset such that \family default \series default \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit \begin_inset Formula \begin{equation} f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp} \end{equation} \end_inset Differential geometry tells us that for any Lie algebra element \begin_inset Formula $\xihat\in\mathfrak{g}$ \end_inset there exists a \emph on linear \emph default map \begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$ \end_inset , which is given by \begin_inset Foot status collapsed \begin_layout Plain Layout See \begin_inset Flex URL status open \begin_layout Plain Layout http://deltaepsilons.wordpress.com/2009/11/06/ \end_layout \end_inset or \begin_inset Flex URL status open \begin_layout Plain Layout https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map \end_layout \end_inset . \end_layout \end_inset \begin_inset Formula \begin{equation} d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp} \end{equation} \end_inset with \begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$ \end_inset and \begin_inset Formula $ad_{\xihat}$ \end_inset itself a linear map taking \begin_inset Formula $\hat{x}$ \end_inset to \begin_inset Formula $[\xihat,\hat{x}]$ \end_inset , the Lie bracket. The actual formula above is not really as important as the fact that the linear map exists, although it is expressed directly in terms of tangent vectors to \begin_inset Formula $\mathfrak{g}$ \end_inset and \begin_inset Formula $G$ \end_inset . Equation \begin_inset CommandInset ref LatexCommand eqref reference "eq:dexp" \end_inset is a tangent vector, and comparing with \begin_inset CommandInset ref LatexCommand eqref reference "eq:tangent-vector" \end_inset we see that it maps to local coordinates \begin_inset Formula $y$ \end_inset as follows: \begin_inset Formula \[ \yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x} \] \end_inset which can be used to construct the Jacobian \begin_inset Formula $f'(\xi)$ \end_inset . \end_layout \begin_layout Example For \begin_inset Formula $\SOthree$ \end_inset , the operator \begin_inset Formula $ad_{\xihat}$ \end_inset is simply a matrix multiplication when representing \begin_inset Formula $\sothree$ \end_inset using 3-vectors, i.e., \begin_inset Formula $ad_{\xihat}x=\xihat x$ \end_inset , and the \begin_inset Formula $3\times3$ \end_inset Jacobian corresponding to \begin_inset Formula $d\exp$ \end_inset is \begin_inset Formula \[ f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k} \] \end_inset which, similar to the exponential map, has a simple closed form expression for \begin_inset Formula $\SOthree$ \end_inset . \end_layout \begin_layout Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Section General Manifolds \end_layout \begin_layout Subsection Retractions \end_layout \begin_layout Standard \begin_inset FormulaMacro \newcommand{\retract}{\mathcal{R}} {\mathcal{R}} \end_inset \end_layout \begin_layout Standard General manifolds that are not Lie groups do not have an exponential map, but can still be handled by defining a \series bold retraction \series default \begin_inset Formula $\retract:\Man\times\Reals n\rightarrow\Man$ \end_inset , such that \begin_inset Formula \[ a\oplus\xi\define\retract_{a}\left(\xi\right) \] \end_inset A retraction \begin_inset CommandInset citation LatexCommand cite key "Absil07book" literal "true" \end_inset is required to be tangent to geodesics on the manifold \begin_inset Formula $\Man$ \end_inset at \begin_inset Formula $a$ \end_inset . We can define many retractions for a manifold \begin_inset Formula $\Man$ \end_inset , even for those with more structure. For the vector space \begin_inset Formula $\Reals n$ \end_inset the retraction is just vector addition, and for Lie groups the obvious retraction is simply the exponential map, i.e., \begin_inset Formula $\retract_{a}(\xi)=a\cdot\exp\xihat$ \end_inset . However, one can choose other, possibly computationally attractive retractions, as long as around a they agree with the geodesic induced by the exponential map, i.e., \begin_inset Formula \[ \lim_{\xi\rightarrow0}\frac{\left|a\cdot\exp\xihat-\retract_{a}\left(\xi\right)\right|}{\left|\xi\right|}=0 \] \end_inset \end_layout \begin_layout Example For \begin_inset Formula $\SEthree$ \end_inset , instead of using the true exponential map it is computationally more efficient to define the retraction, which uses a first order approximation of the translation update \begin_inset Formula \[ \retract_{T}\left(\left[\begin{array}{c} \omega\\ v \end{array}\right]\right)=\left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} e^{\Skew{\omega}} & v\\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} Re^{\Skew{\omega}} & t+Rv\\ 0 & 1 \end{array}\right] \] \end_inset \end_layout \begin_layout Subsection Derivatives \end_layout \begin_layout Standard Equipped with a retraction, then, we can generalize the notion of a derivative for functions \begin_inset Formula $f$ \end_inset from general a manifold \begin_inset Formula $\Man$ \end_inset to \begin_inset Formula $\Reals m$ \end_inset : \end_layout \begin_layout Definition We define a function \begin_inset Formula $f:\Man\rightarrow\Reals m$ \end_inset to be \series bold differentiable \series default at \begin_inset Formula $a\in\Man$ \end_inset if there exists a matrix \begin_inset Formula $f'(a)$ \end_inset such that \begin_inset Formula \[ \lim_{\xi\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f\left(\retract_{a}(\xi)\right)\right|}{\left|\xi\right|}=0 \] \end_inset with \begin_inset Formula $\xi\in\Reals n$ \end_inset for an \begin_inset Formula $n$ \end_inset -dimensional manifold, and \begin_inset Formula $\retract_{a}:\Reals n\rightarrow\Man$ \end_inset a retraction \begin_inset Formula $\retract$ \end_inset at \begin_inset Formula $a$ \end_inset . If \begin_inset Formula $f$ \end_inset is differentiable, then \begin_inset Formula $f'(a)$ \end_inset is called the \series bold Jacobian matrix \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset , and the linear transformation \begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$ \end_inset is called the \series bold derivative \series default of \begin_inset Formula $f$ \end_inset at \begin_inset Formula $a$ \end_inset . \end_layout \begin_layout Definition For manifolds that are also Lie groups, the derivative of any function \begin_inset Formula $f:G\rightarrow\Reals m$ \end_inset will agree no matter what retraction \begin_inset Formula $\retract$ \end_inset is used. \end_layout \begin_layout Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Part Practice \end_layout \begin_layout Standard Below we apply the results derived in the theory part to the geometric objects we use in GTSAM. Above we preferred the modern notation \begin_inset Formula $D_{1}f$ \end_inset for the partial derivative. Below (because this was written earlier) we use the more classical notation \begin_inset Formula \[ \deriv{f(x,y)}x \] \end_inset In addition, for Lie groups we will abuse the notation and take \begin_inset Formula \[ \at{\deriv{\varphi(g)}{\xi}}a \] \end_inset to be the Jacobian matrix \begin_inset Formula $\varphi'($ \end_inset a) of the mapping \begin_inset Formula $\varphi$ \end_inset at \begin_inset Formula $a\in G$ \end_inset , associated with the pushforward \begin_inset Formula $D\varphi_{a}$ \end_inset . \end_layout \begin_layout Section SLAM Example \end_layout \begin_layout Standard Let us examine a visual SLAM example. We have 2D measurements \begin_inset Formula $z_{ij}$ \end_inset , where each measurement is predicted by \begin_inset Formula \[ z_{ij}=h(T_{i},p_{j})=\pi(T_{i}^{-1}p_{j}) \] \end_inset where \begin_inset Formula $T_{i}$ \end_inset is the 3D pose of the \begin_inset Formula $i^{th}$ \end_inset camera, \begin_inset Formula $p_{j}$ \end_inset is the location of the \begin_inset Formula $j^{th}$ \end_inset point, and \begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$ \end_inset is the camera projection function from Example \begin_inset CommandInset ref LatexCommand ref reference "ex:projection" \end_inset . \end_layout \begin_layout Section BetweenFactor \end_layout \begin_layout Standard \series bold \emph on BetweenFactor \series default \emph default is a factor in GTSAM that is used ubiquitously to process measurements indicating the relative pose between two unknown poses \begin_inset Formula $T_{1}$ \end_inset and \begin_inset Formula $T_{2}$ \end_inset . Let us assume the measured relative pose is \begin_inset Formula $Z$ \end_inset , then the code that calculates the error in \series bold \emph on BetweenFactor \series default \emph default first calculates the predicted relative pose \begin_inset Formula $T_{12}$ \end_inset , and then evaluates the error between the measured and predicted relative pose: \end_layout \begin_layout LyX-Code T12 = between(T1, T2); \end_layout \begin_layout LyX-Code return localCoordinates(Z, T12); \end_layout \begin_layout Standard where we recall that the function \series bold \emph on between \series default \emph default is given in group theoretic notation as \begin_inset Formula \[ \varphi(g,h)=g^{-1}h \] \end_inset The function \series bold \emph on localCoordinates \series default \emph default itself also calls \series bold \emph on between \series default \emph default , and converts to canonical coordinates: \end_layout \begin_layout LyX-Code localCoordinates(Z,T12) = Logmap(between(Z, T12)); \end_layout \begin_layout Standard Hence, given two elements \begin_inset Formula $T_{1}$ \end_inset and \begin_inset Formula $T_{2}$ \end_inset , \series bold \emph on BetweenFactor \series default \emph default evaluates \begin_inset Formula $g:G\times G\rightarrow\Reals n$ \end_inset , \begin_inset Formula \[ g(T_{1},T_{2};Z)=f^{-1}\left(\varphi(Z,\varphi(T_{1},T_{2})\right)=f^{-1}\left(Z^{-1}\left(T_{1}^{-1}T_{2}\right)\right) \] \end_inset where \begin_inset Formula $f^{-1}$ \end_inset is the inverse of the map \begin_inset Formula $f:\xi\mapsto\exp\xihat$ \end_inset . If we assume that the measurement has only small error, then \begin_inset Formula $Z\approx T_{1}^{-1}T_{2}$ \end_inset , and hence we have \begin_inset Formula $Z^{-1}T_{1}^{-1}T_{2}\approx e$ \end_inset , and we can invoke Theorem \begin_inset CommandInset ref LatexCommand ref reference "D-exp" \end_inset , which says that the derivative of the exponential map \begin_inset Formula $f:\xi\mapsto\exp\xihat$ \end_inset is identity at \begin_inset Formula $\xi=0$ \end_inset , as well as its inverse. \end_layout \begin_layout Standard Finally, because the derivative of \series bold \emph on between \series default \emph default is identity in its second argument, the derivative of the \series bold \emph on BetweenFactor \series default \emph default error is identical to the derivative of pushforward of \begin_inset Formula $\varphi(T_{1},T_{2})$ \end_inset , derived in Section \begin_inset CommandInset ref LatexCommand ref reference "subsec:Pushforward-of-Between" \end_inset . \end_layout \begin_layout Section Point3 \end_layout \begin_layout Standard A cross product \begin_inset Formula $a\times b$ \end_inset can be written as a matrix multiplication \begin_inset Formula \[ a\times b=\Skew ab \] \end_inset where \begin_inset Formula $\Skew a$ \end_inset is a skew-symmetric matrix defined as \begin_inset Formula \[ \Skew{x,y,z}=\left[\begin{array}{ccc} 0 & -z & y\\ z & 0 & -x\\ -y & x & 0 \end{array}\right] \] \end_inset We also have \begin_inset Formula \[ a^{T}\Skew b=-(\Skew ba)^{T}=-(a\times b)^{T} \] \end_inset The derivative of a cross product \begin_inset Formula \begin{equation} \frac{\partial(a\times b)}{\partial a}=\Skew{-b}\label{eq:Dcross1} \end{equation} \end_inset \begin_inset Formula \begin{equation} \frac{\partial(a\times b)}{\partial b}=\Skew a\label{eq:Dcross2} \end{equation} \end_inset \end_layout \begin_layout Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Section 2D Rotations \end_layout \begin_layout Subsection Rot2 in GTSAM \end_layout \begin_layout Standard A rotation is stored as \begin_inset Formula $(\cos\theta,\sin\theta)$ \end_inset . An incremental rotation is applied using the trigonometric sum rule: \begin_inset Formula \[ \cos\theta'=\cos\theta\cos\delta-\sin\theta\sin\delta \] \end_inset \begin_inset Formula \[ \sin\theta'=\sin\theta\cos\delta+\cos\theta\sin\delta \] \end_inset where \begin_inset Formula $\delta$ \end_inset is an incremental rotation angle. \end_layout \begin_layout Subsection Derivatives of 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 \begin_inset Formula $f(R,p)$ \end_inset corresponds to rotating the 2D point \begin_inset Formula $p$ \end_inset \begin_inset Formula \[ f(R,p)=Rp \] \end_inset According to Theorem \begin_inset CommandInset ref LatexCommand ref reference "th:Action" \end_inset , the Jacobian matrix of \begin_inset Formula $f$ \end_inset is given by \begin_inset Formula \[ f'(R,p)=\left[\begin{array}{cc} RH(p) & R\end{array}\right] \] \end_inset with \begin_inset Formula $H:\Reals 2\rightarrow\Reals{2\times2}$ \end_inset a linear mapping that depends on \begin_inset Formula $p$ \end_inset . In the case of \begin_inset Formula $\SOtwo$ \end_inset , we can find \begin_inset Formula $H(p)$ \end_inset by equating (as in Equation \begin_inset CommandInset ref LatexCommand ref reference "eq:Hp" \end_inset ): \begin_inset Formula \[ \skew wp=\left[\begin{array}{cc} 0 & -\omega\\ \omega & 0 \end{array}\right]\left[\begin{array}{c} x\\ y \end{array}\right]=\left[\begin{array}{c} -y\\ x \end{array}\right]\omega=H(p)\omega \] \end_inset Note that \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula \[ H(p)=\left[\begin{array}{c} -y\\ x \end{array}\right]=\left[\begin{array}{cc} 0 & -1\\ 1 & 0 \end{array}\right]\left[\begin{array}{c} x\\ y \end{array}\right]=R_{\pi/2}p \] \end_inset and since 2D rotations commute, we also have, with \begin_inset Formula $q=Rp$ \end_inset \family default \series default \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit : \begin_inset Formula \[ f'(R,p)=\left[\begin{array}{cc} R\left(R_{\pi/2}p\right) & R\end{array}\right]=\left[\begin{array}{cc} R_{\pi/2}q & R\end{array}\right] \] \end_inset \end_layout \begin_layout Subsection Pushforwards of Mappings \end_layout \begin_layout Standard Since \begin_inset Formula $\Ad R\skew{\omega}=\skew{\omega}$ \end_inset , we have the derivative of \series bold inverse \series default , \begin_inset Formula \[ \frac{\partial R^{T}}{\partial\omega}=-\Ad R=-1\mbox{ } \] \end_inset \series bold compose, \series default \begin_inset Formula \[ \frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}}=\Ad{R_{2}^{T}}=1\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=1 \] \end_inset and \series bold between: \series default \begin_inset Formula \[ \frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}}=-\Ad{R_{2}^{T}R_{1}}=-1\mbox{ and }\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{2}}=1 \] \end_inset \end_layout \begin_layout Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Section 2D Rigid Transformations \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 \[ f(T,p)=\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 To find the derivative, we write the quantity \begin_inset Formula $\xihat\hat{p}$ \end_inset as the product of the \begin_inset Formula $3\times3$ \end_inset matrix \begin_inset Formula $H(p)$ \end_inset with \begin_inset Formula $\xi$ \end_inset : \begin_inset Formula \begin{equation} \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]=\left[\begin{array}{cc} I_{2} & R_{\pi/2}p\\ 0 & 0 \end{array}\right]\left[\begin{array}{c} v\\ \omega \end{array}\right]=H(p)\xi\label{eq:HpSE2} \end{equation} \end_inset Hence, by Theorem \begin_inset CommandInset ref LatexCommand ref reference "th:Action" \end_inset we have \begin_inset Formula \begin{equation} \deriv{\left(T\hat{p}\right)}{\xi}=TH(p)=\left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} I_{2} & R_{\pi/2}p\\ 0 & 0 \end{array}\right]=\left[\begin{array}{cc} R & RR_{\pi/2}p\\ 0 & 0 \end{array}\right]=\left[\begin{array}{cc} R & R_{\pi/2}q\\ 0 & 0 \end{array}\right]\label{eq:SE2Action} \end{equation} \end_inset Note that, looking only at the top rows of \begin_inset CommandInset ref LatexCommand eqref reference "eq:HpSE2" \end_inset and \begin_inset CommandInset ref LatexCommand eqref reference "eq:SE2Action" \end_inset , we can recognize the quantity \begin_inset Formula $\skew{\omega}p+v=v+\omega\left(R_{\pi/2}p\right)$ \end_inset as the velocity of \begin_inset Formula $p$ \end_inset in \begin_inset Formula $\Rtwo$ \end_inset , and \begin_inset Formula $\left[\begin{array}{cc} R & R_{\pi/2}q\end{array}\right]$ \end_inset is the derivative of the action on \begin_inset Formula $\Rtwo$ \end_inset . \end_layout \begin_layout Standard The derivative of the inverse action \begin_inset Formula $g(T,p)=T^{-1}\hat{p}$ \end_inset is given by Theorem \begin_inset CommandInset ref LatexCommand ref reference "Th:InverseAction" \end_inset specialized to \begin_inset Formula $\SEtwo$ \end_inset : \end_layout \begin_layout Standard \begin_inset Formula \[ \deriv{\left(T^{-1}\hat{p}\right)}{\xi}=-H(T^{-1}p)=\left[\begin{array}{cc} -I_{2} & -R_{\pi/2}\left(T^{-1}p\right)\\ 0 & 0 \end{array}\right] \] \end_inset \end_layout \begin_layout Subsection Pushforwards of Mappings \end_layout \begin_layout Standard We can just define all derivatives in terms of the adjoint map, which in the case of \begin_inset Formula $\SEtwo$ \end_inset , in twist coordinates, is the linear mapping \begin_inset Formula \[ \Ad T\xi=\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 and we have \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}}}\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 Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Section 3D Rotations \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 \begin_inset Formula $f(R,p)$ \end_inset corresponds to rotating a point \begin_inset Formula \[ q=f(R,p)=Rp \] \end_inset To calculate \begin_inset Formula $H(p)$ \end_inset for use in Theorem \begin_inset CommandInset ref LatexCommand eqref reference "th:Action" \end_inset we make use of \begin_inset Formula \[ \Skew{\omega}p=\omega\times p=-p\times\omega=\Skew{-p}\omega \] \end_inset so \begin_inset Formula $H(p)\define\Skew{-p}$ \end_inset . Hence, the final derivative of an action in its first argument is \begin_inset Formula \begin{equation} \deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p\label{eq:Rot3action} \end{equation} \end_inset Likewise, according to Theorem \begin_inset CommandInset ref LatexCommand ref reference "Th:InverseAction" \end_inset , the derivative of the inverse action is given by \end_layout \begin_layout Standard \begin_inset Formula \[ \deriv{\left(R^{T}p\right)}{\omega}=-H(R^{T}p)=\Skew{R^{T}p} \] \end_inset \end_layout \begin_layout Subsection \begin_inset CommandInset label LatexCommand label name "subsec:3DAngularVelocities" \end_inset Instantaneous Velocity \end_layout \begin_layout Standard For 3D rotations \begin_inset Formula $R_{b}^{n}$ \end_inset from a body frame \begin_inset Formula $b$ \end_inset to a navigation frame \begin_inset Formula $n$ \end_inset we have the spatial angular velocity \begin_inset Formula $\omega_{nb}^{n}$ \end_inset measured in the navigation frame, \begin_inset Formula \[ \Skew{\omega_{nb}^{n}}\define\dot{R}_{b}^{n}\left(R_{b}^{n}\right)^{T}=\dot{R}_{b}^{n}R_{n}^{b} \] \end_inset and the body angular velocity \begin_inset Formula $\omega_{nb}^{b}$ \end_inset measured in the body frame: \begin_inset Formula \[ \Skew{\omega_{nb}^{b}}\define\left(R_{b}^{n}\right)^{T}\dot{R}_{b}^{n}=R_{n}^{b}\dot{R}_{b}^{n} \] \end_inset These quantities can be used to derive the velocity of a point \begin_inset Formula $p$ \end_inset , and we choose between spatial or body angular velocity depending on the frame in which we choose to represent \begin_inset Formula $p$ \end_inset : \begin_inset Formula \[ v^{n}=\Skew{\omega_{nb}^{n}}p^{n}=\omega_{nb}^{n}\times p^{n} \] \end_inset \begin_inset Formula \[ v^{b}=\Skew{\omega_{nb}^{b}}p^{b}=\omega_{nb}^{b}\times p^{b} \] \end_inset We can transform these skew-symmetric matrices from navigation to body frame by conjugating, \begin_inset Formula \[ \Skew{\omega_{nb}^{b}}=R_{n}^{b}\Skew{\omega_{nb}^{n}}R_{b}^{n} \] \end_inset but because the adjoint representation satisfies \begin_inset Formula \[ Ad_{R}\Skew{\omega}\define R\Skew{\omega}R^{T}=\Skew{R\omega} \] \end_inset we can even more easily transform between spatial and body angular velocities as 3-vectors: \begin_inset Formula \[ \omega_{nb}^{b}=R_{n}^{b}\omega_{nb}^{n} \] \end_inset \end_layout \begin_layout Subsection Pushforwards of Mappings \end_layout \begin_layout Standard For \begin_inset Formula $\SOthree$ \end_inset we have \begin_inset Formula $\Ad R\Skew{\omega}=\Skew{R\omega}$ \end_inset and, in terms of angular velocities: \begin_inset Formula $\Ad R\omega=R\omega$ \end_inset . Hence, the Jacobian matrix of the \series bold inverse \series default mapping is (see Equation \begin_inset CommandInset ref LatexCommand ref reference "eq:Dinverse" \end_inset ) \begin_inset Formula \[ \frac{\partial R^{T}}{\partial\omega}=-\Ad R=-R \] \end_inset for \series bold compose \series default we have (Equations \begin_inset CommandInset ref LatexCommand ref reference "eq:Dcompose1" \end_inset and \begin_inset CommandInset ref LatexCommand ref reference "eq:Dcompose2" \end_inset ): \begin_inset Formula \[ \frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}}=R_{2}^{T}\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=I_{3} \] \end_inset and \series bold between \series default (Equation \begin_inset CommandInset ref LatexCommand ref reference "eq:Dbetween1" \end_inset ): \begin_inset Formula \[ \frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}}=-R_{2}^{T}R_{1}=-between(R_{2},R_{1})\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=I_{3} \] \end_inset \end_layout \begin_layout Subsection Retractions \end_layout \begin_layout Standard Absil \begin_inset CommandInset citation LatexCommand cite after "page 58" key "Absil07book" literal "true" \end_inset discusses two possible retractions for \begin_inset Formula $\SOthree$ \end_inset based on the QR decomposition or the polar decomposition of the matrix \begin_inset Formula $R\Skew{\omega}$ \end_inset , but they are expensive. Another retraction is based on the Cayley transform \begin_inset Formula $\mathcal{C}:\sothree\rightarrow\SOthree$ \end_inset , a mapping from the skew-symmetric matrices to rotation matrices: \begin_inset Formula \[ Q=\mathcal{C}(\Omega)=(I-\Omega)(I+\Omega)^{-1} \] \end_inset Interestingly, the inverse Cayley transform \begin_inset Formula $\mathcal{C}^{-1}:\SOthree\rightarrow\sothree$ \end_inset has the same form: \begin_inset Formula \[ \Omega=\mathcal{C}^{-1}(Q)=(I-Q)(I+Q)^{-1} \] \end_inset The retraction needs a factor \begin_inset Formula $-\frac{1}{2}$ \end_inset however, to make it locally align with a geodesic: \begin_inset Formula \[ R'=\retract_{R}(\omega)=R\mathcal{C}(-\frac{1}{2}\Skew{\omega}) \] \end_inset Note that given \begin_inset Formula $\omega=(x,y,z)$ \end_inset this has the closed-form expression below \begin_inset Formula \[ \frac{1}{4+x^{2}+y^{2}+z^{2}}\left[\begin{array}{ccc} 4+x^{2}-y^{2}-z^{2} & 2xy-4z & 2xz+4y\\ 2xy+4z & 4-x^{2}+y^{2}-z^{2} & 2yz-4x\\ 2xz-4y & 2yz+4x & 4-x^{2}-y^{2}+z^{2} \end{array}\right] \] \end_inset \begin_inset Formula \[ =\frac{1}{4+x^{2}+y^{2}+z^{2}}\left\{ 4(I+\Skew{\omega})+\left[\begin{array}{ccc} x^{2}-y^{2}-z^{2} & 2xy & 2xz\\ 2xy & -x^{2}+y^{2}-z^{2} & 2yz\\ 2xz & 2yz & -x^{2}-y^{2}+z^{2} \end{array}\right]\right\} \] \end_inset so it can be seen to be a second-order correction on \begin_inset Formula $(I+\Skew{\omega})$ \end_inset . The corresponding approximation to the logarithmic map is: \begin_inset Formula \[ \Skew{\omega}=\retract_{R}^{-1}(R')=-2\mathcal{C}^{-1}\left(R^{T}R'\right) \] \end_inset \end_layout \begin_layout Section 3D Rigid Transformations \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]=f(T,p)=\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 The quantity \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), and equating it to \begin_inset Formula $H(p)\xi$ \end_inset as in Equation \begin_inset CommandInset ref LatexCommand ref reference "eq:Hp" \end_inset yields the \begin_inset Formula $4\times6$ \end_inset matrix \begin_inset Formula $H(p)$ \end_inset \begin_inset Foot status collapsed \begin_layout Plain Layout \begin_inset Formula $H(p)$ \end_inset can also be obtained by taking the \begin_inset Formula $j^{th}$ \end_inset column of each of the 6 generators to multiply with components of \begin_inset Formula $\hat{p}$ \end_inset \end_layout \end_inset : \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]=\left[\begin{array}{cc} \Skew{-p} & I_{3}\\ 0 & 0 \end{array}\right]\left[\begin{array}{c} \omega\\ v \end{array}\right]=H(p)\xi \] \end_inset Note how velocities are analogous to points at infinity in projective geometry: they correspond to free vectors indicating a direction and magnitude of change. According to Theorem \begin_inset CommandInset ref LatexCommand ref reference "th:Action" \end_inset , the derivative of the group action is then \begin_inset Formula \[ \deriv{\left(T\hat{p}\right)}{\xi}=TH(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]=\left[\begin{array}{cc} R\Skew{-p} & R\\ 0 & 0 \end{array}\right] \] \end_inset \begin_inset Formula \[ \deriv{\left(T\hat{p}\right)}{\hat{p}}=\left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right] \] \end_inset in homogenous coordinates. In \begin_inset Formula $\Rthree$ \end_inset this becomes \begin_inset Formula $R\left[\begin{array}{cc} -\Skew p & I_{3}\end{array}\right]$ \end_inset . \end_layout \begin_layout Standard The derivative of the inverse action \begin_inset Formula $T^{-1}p$ \end_inset is given by Theorem \begin_inset CommandInset ref LatexCommand ref reference "Th:InverseAction" \end_inset : \end_layout \begin_layout Standard \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none \begin_inset Formula \[ \deriv{\left(T^{-1}\hat{p}\right)}{\xi}=-H\left(T^{-1}\hat{p}\right)=\left[\begin{array}{cc} \Skew{T^{-1}\hat{p}} & -I_{3}\end{array}\right] \] \end_inset \begin_inset Formula \[ \deriv{\left(T^{-1}\hat{p}\right)}{\hat{p}}=\left[\begin{array}{cc} R^{T} & -R^{T}t\\ 0 & 1 \end{array}\right] \] \end_inset \end_layout \begin_layout Example Let us examine a visual SLAM example. We have 2D measurements \begin_inset Formula $z_{ij}$ \end_inset , where each measurement is predicted by \begin_inset Formula \[ z_{ij}=h(T_{i},p_{j})=\pi(T_{i}^{-1}p_{j})=\pi(q) \] \end_inset where \begin_inset Formula $T_{i}$ \end_inset is the 3D pose of the \begin_inset Formula $i^{th}$ \end_inset camera, \begin_inset Formula $p_{j}$ \end_inset is the location of the \begin_inset Formula $j^{th}$ \end_inset point, \begin_inset Formula $q=(x',y',z')=T^{-1}p$ \end_inset is the point in camera coordinates, and \begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$ \end_inset is the camera projection function from Example \begin_inset CommandInset ref LatexCommand ref reference "ex:projection" \end_inset . By the chain rule, we then have \begin_inset Formula \[ \deriv{h(T,p)}{\xi}=\deriv{\pi(q)}q\deriv{(T^{-1}p)}{\xi}=\frac{1}{z'}\left[\begin{array}{ccc} 1 & 0 & -x'/z'\\ 0 & 1 & -y'/z' \end{array}\right]\left[\begin{array}{cc} \Skew q & -I_{3}\end{array}\right]=\left[\begin{array}{cc} \pi'(q)\Skew q & -\pi'(q)\end{array}\right] \] \end_inset \begin_inset Formula \[ \deriv{h(T,p)}p=\pi'(q)R^{T} \] \end_inset \end_layout \begin_layout Subsection Derivative of Adjoint \begin_inset CommandInset label LatexCommand label name "subsec:pose3_adjoint_deriv" \end_inset \end_layout \begin_layout Standard Consider \begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ \end_inset is defined as \begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$ \end_inset . The derivative is notated (see Section \begin_inset CommandInset ref LatexCommand ref reference "sec:Derivatives-of-Actions" plural "false" caps "false" noprefix "false" \end_inset ): \end_layout \begin_layout Standard \begin_inset Formula \[ Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b}) \] \end_inset First, computing \begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$ \end_inset is easy, as its matrix is simply \begin_inset Formula $Ad_{T}$ \end_inset : \end_layout \begin_layout Standard \begin_inset Formula \[ f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b}) \] \end_inset \end_layout \begin_layout Standard \begin_inset Formula \[ D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} \] \end_inset We will derive \begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$ \end_inset using two approaches. In the first, we'll define \begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ \end_inset . From Section \begin_inset CommandInset ref LatexCommand ref reference "sec:Derivatives-of-Actions" plural "false" caps "false" noprefix "false" \end_inset , \end_layout \begin_layout Standard \begin_inset Formula \begin{align*} D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\ D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1} \end{align*} \end_inset Now we can use the definition of the Adjoint representation \begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$ \end_inset (aka conjugation by \begin_inset Formula $g$ \end_inset ) then apply product rule and simplify: \end_layout \begin_layout Standard \begin_inset Formula \begin{align*} D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\ & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ & =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\ & =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\ D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) \end{align*} \end_inset Where \begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$ \end_inset is the adjoint map of the lie algebra. \end_layout \begin_layout Standard The second, perhaps more intuitive way of deriving \begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ \end_inset , would be to use the fact that the derivative at the origin \begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ \end_inset by definition of the adjoint \begin_inset Formula $ad_{\xi}$ \end_inset . Then applying the property \begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$ \end_inset , \end_layout \begin_layout Standard \begin_inset Formula \[ D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right) \] \end_inset \end_layout \begin_layout Subsection Derivative of AdjointTranspose \end_layout \begin_layout Standard The transpose of the Adjoint, \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \xout off \uuline off \uwave off \noun off \color none \begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$ \end_inset , is useful as a way to change the reference frame of vectors in the dual space \family default \series default \shape default \size default \emph default \bar default \strikeout default \xout default \uuline default \uwave default \noun default \color inherit (note the \begin_inset Formula $^{*}$ \end_inset denoting that we are now in the dual space) \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \xout off \uuline off \uwave off \noun off \color none . To be more concrete, where \family default \series default \shape default \size default \emph default \bar default \strikeout default \xout default \uuline default \uwave default \noun default \color inherit as \begin_inset Formula $Ad_{T}\hat{\xi}_{b}$ \end_inset converts the \emph on twist \emph default \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \xout off \uuline off \uwave off \noun off \color none \begin_inset Formula $\xi_{b}$ \end_inset from the \begin_inset Formula $T$ \end_inset frame, \family default \series default \shape default \size default \emph default \bar default \strikeout default \xout default \uuline default \uwave default \noun default \color inherit \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \xout off \uuline off \uwave off \noun off \color none \begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ \end_inset converts the \family default \series default \shape default \size default \emph on \bar default \strikeout default \xout default \uuline default \uwave default \noun default \color inherit wrench \emph default \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \xout off \uuline off \uwave off \noun off \color none \begin_inset Formula $\xi_{b}^{*}$ \end_inset from the \begin_inset Formula $T$ \end_inset frame \family default \series default \shape default \size default \emph default \bar default \strikeout default \xout default \uuline default \uwave default \noun default \color inherit . It's difficult to apply a similar derivation as in Section \begin_inset CommandInset ref LatexCommand ref reference "subsec:pose3_adjoint_deriv" plural "false" caps "false" noprefix "false" \end_inset for the derivative of \begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ \end_inset because \begin_inset Formula $Ad_{T}^{T}$ \end_inset cannot be naturally defined as a conjugation, so we resort to crunching through the algebra. The details are omitted but the result is a form that vaguely resembles (but does not exactly match) \begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ \end_inset : \end_layout \begin_layout Standard \begin_inset Formula \begin{align*} \begin{bmatrix}\omega_{T}\\ v_{T} \end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\ D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\ \hat{v}_{T} & 0 \end{bmatrix} \end{align*} \end_inset \end_layout \begin_layout Subsection Instantaneous Velocity \end_layout \begin_layout Standard For rigid 3D transformations \begin_inset Formula $T_{b}^{n}$ \end_inset from a body frame \begin_inset Formula $b$ \end_inset to a navigation frame \begin_inset Formula $n$ \end_inset we have the instantaneous spatial twist \begin_inset Formula $\xi_{nb}^{n}$ \end_inset measured in the navigation frame, \begin_inset Formula \[ \hat{\xi}_{nb}^{n}\define\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1} \] \end_inset and the instantaneous body twist \begin_inset Formula $\xi_{nb}^{b}$ \end_inset measured in the body frame: \begin_inset Formula \[ \hat{\xi}_{nb}^{b}\define\left(T_{b}^{n}\right)^{T}\dot{T}_{b}^{n} \] \end_inset \end_layout \begin_layout Subsection Pushforwards of Mappings \end_layout \begin_layout Standard As we can express the Adjoint representation in terms of twist coordinates, we have \begin_inset Formula \[ \left[\begin{array}{c} \omega'\\ v' \end{array}\right]=\left[\begin{array}{cc} R & 0\\ \Skew tR & R \end{array}\right]\left[\begin{array}{c} \omega\\ v \end{array}\right] \] \end_inset Hence, as with \begin_inset Formula $\SOthree$ \end_inset , we are now in a position to simply posit the derivative of \series bold inverse \series default , \begin_inset Formula \[ \frac{\partial T^{-1}}{\partial\xi}=-\Ad T=-\left[\begin{array}{cc} R & 0\\ \Skew tR & R \end{array}\right] \] \end_inset \series bold compose \series default in its first argument, \begin_inset Formula \[ \frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}}=\Ad{T_{2}^{-1}} \] \end_inset in its second argument, \begin_inset Formula \[ \frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}}=I_{6} \] \end_inset \series bold between \series default in its first argument, \begin_inset Formula \[ \frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}}=-\Ad{T_{2}^{^{-1}}T_{1}}=\left[\begin{array}{cc} -R_{2}^{T}R_{1} & 0\\ R_{2}^{T}\left[t_{2}-t_{1}\right]_{\times}R_{1} & -R_{2}^{T}R_{1} \end{array}\right] \] \end_inset and in its second argument, \begin_inset Formula \begin{eqnarray*} \frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{2}} & = & I_{6} \end{eqnarray*} \end_inset \end_layout \begin_layout Subsection Retractions \end_layout \begin_layout Standard For \begin_inset Formula $\SEthree$ \end_inset , instead of using the true exponential map it is computationally more efficient to design other retractions. A first-order approximation to the exponential map does not quite cut it, as it yields a \begin_inset Formula $4\times4$ \end_inset matrix which is not in \begin_inset Formula $\SEthree$ \end_inset : \begin_inset Formula \begin{eqnarray*} T\exp\xihat & \approx & T(I+\xihat)\\ & = & T\left(I_{4}+\left[\begin{array}{cc} \Skew{\omega} & v\\ 0 & 0 \end{array}\right]\right)\\ & = & \left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} I_{3}+\Skew{\omega} & v\\ 0 & 1 \end{array}\right]\\ & = & \left[\begin{array}{cc} R\left(I_{3}+\Skew{\omega}\right) & t+Rv\\ 0 & 1 \end{array}\right] \end{eqnarray*} \end_inset However, we can make it into a retraction by using any retraction defined for \begin_inset Formula $\SOthree$ \end_inset , including, as below, using the exponential map \begin_inset Formula $Re^{\Skew{\omega}}$ \end_inset : \begin_inset Formula \[ \retract_{T}\left(\left[\begin{array}{c} \omega\\ v \end{array}\right]\right)=\left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} e^{\Skew{\omega}} & v\\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} Re^{\Skew{\omega}} & t+Rv\\ 0 & 1 \end{array}\right] \] \end_inset Similarly, for a second order approximation we have \begin_inset Formula \begin{eqnarray*} T\exp\xihat & \approx & T(I+\xihat+\frac{\xihat^{2}}{2})\\ & = & T\left(I_{4}+\left[\begin{array}{cc} \Skew{\omega} & v\\ 0 & 0 \end{array}\right]+\frac{1}{2}\left[\begin{array}{cc} \Skew{\omega} & v\\ 0 & 0 \end{array}\right]\left[\begin{array}{cc} \Skew{\omega} & v\\ 0 & 0 \end{array}\right]\right)\\ & = & \left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right]\left(\left[\begin{array}{cc} I_{3}+\Skew{\omega}+\frac{1}{2}\Skew{\omega}^{2} & v+\frac{1}{2}\Skew{\omega}v\\ 0 & 1 \end{array}\right]\right)\\ & = & \left[\begin{array}{cc} R\left(I_{3}+\Skew{\omega}+\frac{1}{2}\Skew{\omega}^{2}\right) & t+R\left[v+\left(\omega\times v\right)/2\right]\\ 0 & 1 \end{array}\right] \end{eqnarray*} \end_inset inspiring the retraction \begin_inset Formula \[ \retract_{T}\left(\left[\begin{array}{c} \omega\\ v \end{array}\right]\right)=\left[\begin{array}{cc} R & t\\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} e^{\Skew{\omega}} & v+\left(\omega\times v\right)/2\\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} Re^{\Skew{\omega}} & t+R\left[v+\left(\omega\times v\right)/2\right]\\ 0 & 1 \end{array}\right] \] \end_inset \end_layout \begin_layout Standard \begin_inset Newpage pagebreak \end_inset \end_layout \begin_layout Section The Sphere \begin_inset Formula $S^{2}$ \end_inset \end_layout \begin_layout Subsection Definitions \end_layout \begin_layout Standard The sphere \begin_inset Formula $S^{2}$ \end_inset is the set of all unit vectors in \begin_inset Formula $\Rthree$ \end_inset , i.e., all directions in three-space: \begin_inset Formula \[ S^{2}=\{p\in\Rthree|\left\Vert p\right\Vert =1\} \] \end_inset The tangent space \begin_inset Formula $T_{p}S^{2}$ \end_inset at a point \begin_inset Formula $p$ \end_inset consists of three-vectors \begin_inset Formula $\xihat$ \end_inset such that \begin_inset Formula $\xihat$ \end_inset is tangent to \begin_inset Formula $S^{2}$ \end_inset at \begin_inset Formula $p$ \end_inset , i.e., \begin_inset Formula \[ T_{p}S^{2}\define\left\{ \xihat\in\Rthree|p^{T}\xihat=0\right\} \] \end_inset While not a Lie group, we can define an exponential map, which is given in Ma et. al \begin_inset CommandInset citation LatexCommand cite key "Ma01ijcv" literal "true" \end_inset , as well as in this CVPR tutorial by Anuj Srivastava: \begin_inset CommandInset href LatexCommand href name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf" literal "false" \end_inset . \begin_inset Formula \[ \exp_{p}\xihat=\cos\left(\left\Vert \xihat\right\Vert \right)p+\sin\left(\left\Vert \xihat\right\Vert \right)\frac{\xihat}{\left\Vert \xihat\right\Vert } \] \end_inset The latter also gives the inverse, i.e., get the tangent vector \begin_inset Formula $z$ \end_inset to go from \begin_inset Formula $p$ \end_inset to \begin_inset Formula $q$ \end_inset : \begin_inset Formula \[ z=\log_{p}q=\frac{\theta}{\sin\theta}\left(q-p\cos\theta\right)p \] \end_inset with \begin_inset Formula $\theta=\cos^{-1}\left(p^{T}q\right)$ \end_inset . \end_layout \begin_layout Subsection Local Coordinates \end_layout \begin_layout Standard We can find a basis \begin_inset Formula $B_{p}$ \end_inset for the tangent space \begin_inset Formula $T_{p}S^{2}$ \end_inset , with \begin_inset Formula $B_{p}=\left[b_{1}|b_{2}\right]$ \end_inset a \begin_inset Formula $3\times2$ \end_inset matrix, by either \end_layout \begin_layout Enumerate Decompose \begin_inset Formula $p=QR$ \end_inset , with \begin_inset Formula $Q$ \end_inset orthonormal and \begin_inset Formula $R$ \end_inset of the form \begin_inset Formula $[1\,0\,0]^{T}$ \end_inset , and hence \begin_inset Formula $p=Q_{1}$ \end_inset . The basis \begin_inset Formula $B_{p}=\left[Q_{2}|Q_{3}\right]$ \end_inset , i.e., the last two columns of \begin_inset Formula $Q$ \end_inset . \end_layout \begin_layout Enumerate Form \begin_inset Formula $b_{1}=p\times a$ \end_inset , with \begin_inset Formula $a$ \end_inset (consistently) chosen to be non-parallel to \begin_inset Formula $p$ \end_inset , and \begin_inset Formula $b_{2}=p\times b_{1}$ \end_inset . \begin_inset Note Note status collapsed \begin_layout Plain Layout To choose \begin_inset Formula $a$ \end_inset , one way is to divide the sphere into regions, e.g., pick the axis \begin_inset Formula $e_{i}$ \end_inset such that \begin_inset Formula $e_{i}^{T}p$ \end_inset is smallest. However, that leads to discontinuous boundaries. Since \begin_inset Formula $0\leq\left|e_{i}^{T}p\right|\leq1$ \end_inset for all \begin_inset Formula $p\in S^{2}$ \end_inset , a better idea might be to use a mixture, e.g., \begin_inset Formula \[ a=\frac{1}{2(x^{2}+y^{2}+z^{2})}\left[\begin{array}{c} y^{2}+z^{2}\\ x^{2}+z^{2}\\ x^{2}+y^{2} \end{array}\right] \] \end_inset \end_layout \end_inset \end_layout \begin_layout Standard Now we can write \begin_inset Formula $\xihat=B_{p}\xi$ \end_inset with \begin_inset Formula $\xi\in R^{2}$ \end_inset the 2D coordinate in the tangent plane basis \begin_inset Formula $B_{p}$ \end_inset . \end_layout \begin_layout Subsection Retraction \end_layout \begin_layout Standard The exponential map uses \begin_inset Formula $\cos$ \end_inset and \begin_inset Formula $\sin$ \end_inset , and is more than we need for optimization. Suppose we have a point \begin_inset Formula $p\in S^{2}$ \end_inset and a 3-vector \begin_inset Formula $\xihat\in T_{p}S^{2}$ \end_inset , Absil \begin_inset CommandInset citation LatexCommand cite key "Absil07book" literal "true" \end_inset tells us we can simply add \begin_inset Formula $\xihat$ \end_inset to \begin_inset Formula $p$ \end_inset and renormalize to get a new point \begin_inset Formula $q$ \end_inset on the sphere. This is what he calls a \series bold retraction \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula $\retract_{p}(\xihat)$ \end_inset , \family default \series default \shape default \size default \emph default \bar default \strikeout default \uuline default \uwave default \noun default \color inherit \begin_inset Formula \[ q=\retract_{p}(\xihat)=\frac{p+\xihat}{\left\Vert p+z\right\Vert }=\frac{p+\xihat}{\alpha} \] \end_inset with \begin_inset Formula $\alpha$ \end_inset the norm of \begin_inset Formula $p+\xihat$ \end_inset . \end_layout \begin_layout Standard We can also define a retraction from local coordinates \begin_inset Formula $\xi\in\Rtwo$ \end_inset : \begin_inset Formula \[ q=\retract_{p}(\xi)=\frac{p+B_{p}\xi}{\left\Vert p+B_{p}\xi\right\Vert } \] \end_inset \end_layout \begin_layout Subsubsection* Inverse Retraction \end_layout \begin_layout Standard If \begin_inset Formula $\xihat=B_{p}\xi$ \end_inset with \begin_inset Formula $\xi\in R^{2}$ \end_inset the 2D coordinate in the tangent plane basis \begin_inset Formula $B_{p}$ \end_inset , we have \begin_inset Formula \[ \xi=\frac{B_{p}^{T}q}{p^{T}q} \] \end_inset \end_layout \begin_layout Proof We seek \begin_inset Formula \[ \alpha q=p+B_{p}\xi \] \end_inset If we multiply both sides with \begin_inset Formula $B_{p}^{T}$ \end_inset (project on the basis \begin_inset Formula $B_{p}$ \end_inset ) we obtain \begin_inset Formula \[ \alpha B_{p}^{T}q=B_{p}^{T}p+B_{p}^{T}B_{p}\xi \] \end_inset and because \begin_inset Formula $B_{p}^{T}p=0$ \end_inset and \begin_inset Formula $B_{p}^{T}B_{p}=I$ \end_inset we trivially obtain \begin_inset Formula $\xi$ \end_inset as the scaled projection \begin_inset Formula $B_{p}^{T}q$ \end_inset : \begin_inset Formula \[ \xi=\alpha B_{p}^{T}q \] \end_inset To recover the scale factor \begin_inset Formula $\alpha$ \end_inset we multiply with \begin_inset Formula $p^{T}$ \end_inset on both sides, and we get \begin_inset Formula \[ \alpha p^{T}q=p^{T}p+p^{T}B_{p}\xi \] \end_inset Since \begin_inset Formula $p^{T}p=1$ \end_inset and \begin_inset Formula $p^{T}B_{p}\xi=0$ \end_inset , we then obtain \begin_inset Formula $\alpha=1/(p^{T}q)$ \end_inset , which completes the proof. \end_layout \begin_layout Subsection Rotation acting on a 3D Direction \end_layout \begin_layout Standard Rotating a point \begin_inset Formula $p\in S^{2}$ \end_inset on the sphere obviously yields another point \begin_inset Formula $q=Rp\in S^{2}$ \end_inset , as rotation preserves the norm. The derivative of \begin_inset Formula $f(R,p)$ \end_inset with respect to \begin_inset Formula $R$ \end_inset can be found by equating \begin_inset Formula \[ Rp+B_{Rp}\xi=R(I+\Skew{\omega})p=Rp+R\Skew{\omega}p \] \end_inset \begin_inset Formula \[ B_{Rp}\xi=-R\Skew p\omega \] \end_inset \begin_inset Formula \[ \xi=-B_{Rp}^{T}R\Skew p\omega \] \end_inset whereas with respect to \begin_inset Formula $p$ \end_inset we have \begin_inset Formula \[ Rp+B_{Rp}\xi_{q}=R(p+B_{p}\xi_{p}) \] \end_inset \begin_inset Formula \[ \xi_{q}=B_{Rp}^{T}RB_{p}\xi_{p} \] \end_inset \end_layout \begin_layout Standard In other words, the Jacobian matrix is given by \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula \[ f'(R,p)=\left[\begin{array}{cc} -B_{Rp}^{T}R\Skew p & B_{Rp}^{T}RB_{p}\end{array}\right] \] \end_inset \end_layout \begin_layout Subsection Error between 3D Directions \end_layout \begin_layout Standard We would like to define a distance metric \begin_inset Formula $e(p,q)$ \end_inset between two directions \begin_inset Formula $p,q\in S^{2}$ \end_inset . An obvious choice is \begin_inset Formula \[ \theta=\cos^{-1}\left(p^{T}q\right) \] \end_inset which is exactly the distance along the shortest path (geodesic) on the sphere, i.e., this is the distance metric associated with the exponential. The advantage is that it is defined everywhere, but it involves \begin_inset Formula $\cos^{-1}$ \end_inset . The derivative with respect to a change in \begin_inset Formula $q$ \end_inset , via \begin_inset Formula $\xi$ \end_inset , is then \begin_inset Formula \[ \frac{\partial\theta(p,q)}{\partial\xi}=\frac{\partial\cos^{-1}\left(p^{T}q\right)}{\partial\xi}=\frac{p^{T}B_{q}}{\sqrt{1-\left(p^{T}q\right)^{2}}} \] \end_inset which is also undefined for \begin_inset Formula $p=q$ \end_inset . \end_layout \begin_layout Standard A simpler metric is derived from the retraction but only holds when \begin_inset Formula $q\approx p$ \end_inset . It simply projects \begin_inset Formula $q$ \end_inset onto the local coordinate basis \begin_inset Formula $B_{p}$ \end_inset defined by \begin_inset Formula $p$ \end_inset , and takes the norm: \begin_inset Formula \[ \theta(p,q)=\left\Vert B_{p}^{T}q\right\Vert \] \end_inset The derivative with respect to a change in \begin_inset Formula $q$ \end_inset , via \begin_inset Formula $\xi$ \end_inset , is then \begin_inset Formula \[ \frac{\partial\theta(p,q)}{\partial\xi_{q}}=\frac{\partial}{\partial\xi_{q}}\sqrt{\left(B_{p}^{T}q\right)^{2}}=\frac{1}{\sqrt{\left(B_{p}^{T}q\right)^{2}}}\left(B_{p}^{T}q\right)B_{p}^{T}B_{q}=\frac{B_{p}^{T}q}{\theta(q;p)}B_{p}^{T}B_{q} \] \end_inset Note that this again is undefined for \begin_inset Formula $\theta=0$ \end_inset . \end_layout \begin_layout Standard For use in a probabilistic factor, a signed, vector-valued error will not have the discontinuity: \begin_inset Formula \[ \theta(p,q)=B_{p}^{T}q \] \end_inset Note this is the inverse retraction up to a scale. The derivative with respect to a change in \begin_inset Formula $q$ \end_inset , via \begin_inset Formula $\xi$ \end_inset , is found by \begin_inset Formula \[ \frac{\partial\theta(p,q)}{\partial\xi_{q}}=B_{p}^{T}\frac{\partial q}{\partial\xi_{q}}=B_{p}^{T}B_{q} \] \end_inset \end_layout \begin_layout Subsubsection* Application \end_layout \begin_layout Standard We can use the above to find the unknown rotation between a camera and an IMU. If we measure the rotation between two frames as \begin_inset Formula $c_{1}Zc_{2}$ \end_inset , and the predicted rotation from the IMU is \begin_inset Formula $i_{1}Ri_{2}$ \end_inset , then we can predict \begin_inset Formula \[ c_{1}Zc_{2}=iRc^{T}\cdot i_{1}Ri_{2}\cdot iRc \] \end_inset and the axis of the incremental rotations will relate as \begin_inset Formula \[ p=iRc\cdot z \] \end_inset with \begin_inset Formula $p$ \end_inset the angular velocity axis in the IMU frame, and \begin_inset Formula $z$ \end_inset the measured axis of rotation between the two cameras. Note this only makes sense if the rotation is non-zero. So, given an initial estimate \begin_inset Formula $R$ \end_inset for the unknown rotation \begin_inset Formula $iRc$ \end_inset between IMU and camera, the derivative of the error is (using \begin_inset CommandInset ref LatexCommand ref reference "eq:Rot3action" \end_inset ) \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula \[ \frac{\partial\theta(Rz;p)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}B_{Rz}\frac{\partial\left(Rz\right)}{\partial\omega}=B_{p}^{T}\left(Rz\right)B_{p}^{T}R\Skew z \] \end_inset Here the \begin_inset Formula $2\times3$ \end_inset matrix \begin_inset Formula $B_{Rz}^{T}\Skew z$ \end_inset translates changes in \begin_inset Formula $R$ \end_inset to changes in \begin_inset Formula $Rz$ \end_inset , and the \begin_inset Formula $1\times2$ \end_inset matrix \begin_inset Formula $B_{p}^{T}\left(Rz\right)$ \end_inset describes the downstream effect on the error metric. \end_layout \begin_layout Section The Essential Matrix Manifold \end_layout \begin_layout Standard We parameterize essential matrices as a pair \begin_inset Formula $(R,t)$ \end_inset , where \begin_inset Formula $R\in\SOthree$ \end_inset and \begin_inset Formula $t\in S^{2}$ \end_inset , the unit sphere. The epipolar matrix is then given by \begin_inset Formula \[ E=\Skew tR \] \end_inset and the epipolar error given two corresponding points \begin_inset Formula $a$ \end_inset and \begin_inset Formula $b$ \end_inset is \begin_inset Formula \[ e(R,t;a,b)=a^{T}Eb \] \end_inset We are of course interested in the derivative with respect to orientation (using \begin_inset CommandInset ref LatexCommand ref reference "eq:Rot3action" \end_inset ) \begin_inset Formula \[ \frac{\partial(a^{T}[t]_{\times}Rb)}{\partial\omega}=a^{T}[t]_{\times}\frac{\partial(Rb)}{\partial\omega}=-a^{T}[t]_{\times}R\Skew b=-a^{T}E[b]_{\times} \] \end_inset and with respect to change in the direction \begin_inset Formula $t$ \end_inset \begin_inset Formula \[ \frac{\partial e(a^{T}[t]_{\times}Rb)}{\partial\xi}=a^{T}\frac{\partial(B\xi\times Rb)}{\partial v}=-a^{T}[Rb]_{\times}B \] \end_inset where we made use of the fact that the retraction can be written as \begin_inset Formula $t+B\xi$ \end_inset , with \begin_inset Formula $B$ \end_inset a local basis, and we made use of \begin_inset CommandInset ref LatexCommand eqref reference "eq:Dcross1" \end_inset : \family roman \series medium \shape up \size normal \emph off \bar no \strikeout off \uuline off \uwave off \noun off \color none \begin_inset Formula \[ \frac{\partial(a\times b)}{\partial a}=\Skew{-b} \] \end_inset \end_layout \begin_layout Section 2D Line Segments (Ocaml) \end_layout \begin_layout Standard The error between an infinite line \begin_inset Formula $(a,b,c)$ \end_inset and a 2D line segment \begin_inset Formula $((x1,y1),(x2,y2))$ \end_inset is defined in Line3.ml. \end_layout \begin_layout Section Line3vd (Ocaml) \end_layout \begin_layout Standard One representation of a line is through 2 vectors \begin_inset Formula $(v,d)$ \end_inset , where \begin_inset Formula $v$ \end_inset is the direction and the vector \begin_inset Formula $d$ \end_inset points from the origin 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 \end_layout \begin_layout Subsection Projecting Line3 \end_layout \begin_layout Standard 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 Subsection Action of \begin_inset Formula $\SEthree$ \end_inset on the line \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 $T_{c}^{w}=(R_{c}^{w},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 where \begin_inset Formula $R_{1}$ \end_inset and \begin_inset Formula $R_{2}$ \end_inset are the columns of \begin_inset Formula $R$ \end_inset , as before. \end_layout \begin_layout Standard To find the derivatives, the transformation of a line \begin_inset Formula $l^{w}=(R,a,b)$ \end_inset from world coordinates to a camera coordinate frame \begin_inset Formula $T_{c}^{w}$ \end_inset , specified in world coordinates, can be written as a function \begin_inset Formula $f:\SEthree\times L\rightarrow L$ \end_inset , as given above, i.e., \begin_inset Formula \[ f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right). \] \end_inset Let us find the Jacobian \begin_inset Formula $J_{1}$ \end_inset of \begin_inset Formula $f$ \end_inset with respect to the first argument \begin_inset Formula $T_{c}^{w}$ \end_inset , which should obey \begin_inset Formula \begin{align*} f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi \end{align*} \end_inset Note that \begin_inset Formula \[ T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc} R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\ 0 & 1 \end{array}\right] \] \end_inset Let's write this out separately for each of \begin_inset Formula $R,a,b$ \end_inset : \begin_inset Formula \begin{align*} \left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\ a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\ b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v \end{align*} \end_inset Simplifying, we get: \begin_inset Formula \begin{align*} -\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\ -R_{1}^{T}R_{c}^{w} & \approx J_{av}\\ -R_{2}^{T}R_{c}^{w} & \approx J_{bv} \end{align*} \end_inset which gives the expressions for \begin_inset Formula $J_{av}$ \end_inset and \begin_inset Formula $J_{bv}$ \end_inset . The top line can be further simplified: \begin_inset Formula \begin{align*} -\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\ -R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\ -\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\ -R'^{T} & \approx J_{R\omega} \end{align*} \end_inset For the second argument \begin_inset Formula $R$ \end_inset we now simply have: \begin_inset Formula \begin{eqnarray*} AB(I+\Omega') & = & AB(I+\Omega)\\ \Omega' & = & \Omega\\ \omega' & = & \omega \end{eqnarray*} \end_inset The scalar derivatives can be found by realizing that \begin_inset Formula \[ \left(\begin{array}{c} a'\\ b'\\ ... \end{array}\right)=\left(\begin{array}{c} a\\ b\\ 0 \end{array}\right)-R^{T}t^{w} \] \end_inset where we don't care about the third row. Hence \begin_inset Formula \[ \frac{\partial(\left(R(I+\Omega_{2})\right)^{T}t^{w})}{\partial\omega}=-\frac{\partial(\Omega_{2}R^{T}t^{w})}{\partial\omega}=-\Skew{R^{T}t^{w}}=\left[\begin{array}{ccc} 0 & R_{3}^{T}t^{w} & -R_{2}^{T}t^{w}\\ -R_{3}^{T}t^{w} & 0 & R_{1}^{T}t^{w}\\ ... & ... & 0 \end{array}\right] \] \end_inset \end_layout \begin_layout Section \series bold Aligning 3D Scans \end_layout \begin_layout Standard Below is the explanation 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 Section* Appendix \end_layout \begin_layout Subsection* Differentiation Rules \end_layout \begin_layout Standard Spivak \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" literal "true" \end_inset also notes some multivariate derivative rules defined component-wise, but they are not that useful in practice: \end_layout \begin_layout Itemize Since \begin_inset Formula $f:\Multi nm$ \end_inset is defined in terms of \begin_inset Formula $m$ \end_inset component functions \begin_inset Formula $f^{i}$ \end_inset , then \begin_inset Formula $f$ \end_inset is differentiable at \begin_inset Formula $a$ \end_inset iff each \begin_inset Formula $f^{i}$ \end_inset is, and the Jacobian matrix \begin_inset Formula $F_{a}$ \end_inset is the \begin_inset Formula $m\times n$ \end_inset matrix whose \begin_inset Formula $i^{th}$ \end_inset row is \begin_inset Formula $\left(f^{i}\right)'(a)$ \end_inset : \begin_inset Formula \[ F_{a}\define f'(a)=\left[\begin{array}{c} \left(f^{1}\right)'(a)\\ \vdots\\ \left(f^{m}\right)'(a) \end{array}\right] \] \end_inset \end_layout \begin_layout Itemize Scalar differentiation rules: if \begin_inset Formula $f,g:\OneD n$ \end_inset are differentiable at \begin_inset Formula $a$ \end_inset , then \begin_inset Formula \[ (f+g)'(a)=F_{a}+G_{a} \] \end_inset \begin_inset Formula \[ (f\cdot g)'(a)=g(a)F_{a}+f(a)G_{a} \] \end_inset \begin_inset Formula \[ (f/g)'(a)=\frac{1}{g(a)^{2}}\left[g(a)F_{a}-f(a)G_{a}\right] \] \end_inset \end_layout \begin_layout Subsection* Tangent Spaces and the Tangent Bundle \end_layout \begin_layout Standard The following is adapted from Appendix A in \begin_inset CommandInset citation LatexCommand cite key "Murray94book" literal "true" \end_inset . \end_layout \begin_layout Standard The \series bold tangent space \series default \begin_inset Formula $T_{p}M$ \end_inset of a manifold \begin_inset Formula $M$ \end_inset at a point \begin_inset Formula $p\in M$ \end_inset is the vector space of \series bold tangent vectors \series default at \begin_inset Formula $p$ \end_inset . The \series bold tangent bundle \series default \begin_inset Formula $TM$ \end_inset is the set of all tangent vectors \begin_inset Formula \[ TM\define\bigcup_{p\in M}T_{p}M \] \end_inset A \series bold vector field \series default \begin_inset Formula $X:M\rightarrow TM$ \end_inset assigns a single tangent vector \begin_inset Formula $x\in T_{p}M$ \end_inset to each point \begin_inset Formula $p$ \end_inset . \end_layout \begin_layout Standard If \begin_inset Formula $F:M\rightarrow N$ \end_inset is a smooth map from a manifold \begin_inset Formula $M$ \end_inset to a manifold \begin_inset Formula $N$ \end_inset , then we can define the \series bold tangent map \series default of \begin_inset Formula $F$ \end_inset at \begin_inset Formula $p$ \end_inset as the linear map \begin_inset Formula $F_{*p}:T_{p}M\rightarrow T_{F(p)}N$ \end_inset that maps tangent vectors in \begin_inset Formula $T_{p}M$ \end_inset at \begin_inset Formula $p$ \end_inset to tangent vectors in \begin_inset Formula $T_{F(p)}N$ \end_inset at the image \begin_inset Formula $F(p)$ \end_inset . \end_layout \begin_layout Subsection* Homomorphisms \end_layout \begin_layout Standard The following \emph on might be \emph default relevant \begin_inset CommandInset citation LatexCommand cite after "page 45" key "Hall00book" literal "true" \end_inset : suppose that \begin_inset Formula $\Phi:G\rightarrow H$ \end_inset is a mapping (Lie group homomorphism). Then there exists a unique linear map \begin_inset Formula $\phi:\gg\rightarrow\mathfrak{h}$ \end_inset \begin_inset Formula \[ \phi(\xhat)\define\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(e^{t\xhat}\right) \] \end_inset such that \end_layout \begin_layout Enumerate \begin_inset Formula $\Phi\left(e^{\xhat}\right)=e^{\phi\left(\xhat\right)}$ \end_inset \end_layout \begin_layout Enumerate \begin_inset Formula $\phi\left(T\xhat T^{-1}\right)=\Phi(T)\phi(\xhat)\Phi(T^{-1})$ \end_inset \end_layout \begin_layout Enumerate \begin_inset Formula $\phi\left([\xhat,\yhat]\right)=\left[\phi(\xhat),\phi(\yhat)\right]$ \end_inset \end_layout \begin_layout Standard In other words, the map \begin_inset Formula $\phi$ \end_inset is the derivative of \begin_inset Formula $\Phi$ \end_inset at the identity. As an example, suppose \begin_inset Formula $\Phi(g)=g^{-1}$ \end_inset , then the corresponding derivative \emph on at the identity \emph default is \begin_inset Formula \[ \phi(\xhat)\define\lim_{t\rightarrow0}\frac{d}{dt}\left(e^{t\xhat}\right)^{-1}=\lim_{t\rightarrow0}\frac{d}{dt}e^{-t\xhat}=-\xhat\lim_{t\rightarrow0}e^{-t\xhat}=-\xhat \] \end_inset In general it suffices to compute \begin_inset Formula $\phi$ \end_inset for a basis of \begin_inset Formula $\gg$ \end_inset . \end_layout \begin_layout Standard \begin_inset Note Note status collapsed \begin_layout Plain Layout Undercooked: What if we want the derivative of \begin_inset Formula $\Phi$ \end_inset at some other element \begin_inset Formula $g$ \end_inset ? In other words, if we apply \begin_inset Formula $\Phi$ \end_inset at \begin_inset Formula $g$ \end_inset incremented by some Lie algebra element \begin_inset Formula $e^{\xhat}$ \end_inset , then we are looking for a \begin_inset Formula $\yhat\in\gg$ \end_inset will yield the same result: \begin_inset Formula \[ \Phi\left(g\right)\lim_{t\rightarrow0}\frac{d}{dt}e^{t\yhat}=\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(ge^{t\xhat}\right) \] \end_inset \begin_inset Formula \[ \lim_{t\rightarrow0}\frac{d}{dt}e^{t\yhat}=\Phi\left(g\right)^{-1}\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(ge^{t\xhat}\right) \] \end_inset \end_layout \end_inset \end_layout \begin_layout Standard \begin_inset Note Note status collapsed \begin_layout Plain Layout Let us define two mappings \begin_inset Formula \[ \Phi_{1}(A)=AB\mbox{ and }\Phi_{2}(B)=AB \] \end_inset Then \begin_inset Formula \[ \phi_{1}(\xhat)=\lim_{t\rightarrow0}\frac{d}{dt}\Phi_{1}\left(e^{t\xhat}B\right)= \] \end_inset \end_layout \end_inset \end_layout \begin_layout Standard \begin_inset CommandInset bibtex LatexCommand bibtex bibfiles "refs" options "plain" \end_inset \end_layout \end_body \end_document