#LyX 1.6.5 created this file. For more info see http://www.lyx.org/ \lyxformat 345 \begin_document \begin_header \textclass article \use_default_options false \language english \inputencoding auto \font_roman times \font_sans default \font_typewriter default \font_default_family rmdefault \font_sc false \font_osf false \font_sf_scale 100 \font_tt_scale 100 \graphics default \paperfontsize 12 \spacing single \use_hyperref false \papersize default \use_geometry false \use_amsmath 1 \use_esint 0 \cite_engine basic \use_bibtopic false \paperorientation portrait \secnumdepth 3 \tocdepth 3 \paragraph_separation indent \defskip medskip \quotes_language english \papercolumns 1 \papersides 1 \paperpagestyle default \tracking_changes false \output_changes false \author "" \author "" \end_header \begin_body \begin_layout Title Geometry Derivatives and Other Hairy Math \end_layout \begin_layout Author Frank Dellaert \end_layout \begin_layout Standard \begin_inset FormulaMacro \newcommand{\Skew}[1]{[#1]_{\times}} {[#1]_{\times}} \end_inset \end_layout \begin_layout Standard This document should be kept up to date and specify how each of the derivatives in the geometry modules are computed. \end_layout \begin_layout Section General Lie group derivations \end_layout \begin_layout Standard The derivatives for \emph on compose \emph default , \emph on inverse \emph default , and \emph on between \emph default can be derived from Lie group principals to work with any transformation type. To find the derivatives of these functions, we look for the necessary \begin_inset Quotes eld \end_inset delta \begin_inset Quotes erd \end_inset in the tangent space of the function \emph on output \emph default that corresponds to a \begin_inset Quotes eld \end_inset delta \begin_inset Quotes erd \end_inset in the tangent space of the function \emph on input \emph default . For example, to find the derivative of a function \begin_inset Formula $f\left(X\right)$ \end_inset , we include the differential changes in the tangent space: \begin_inset Formula \[ f\left(X\right)\exp\partial f=f\left(X\exp\partial x\right)\] \end_inset and then taking the partial derivatives \begin_inset Formula $\frac{\partial y}{\partial x}$ \end_inset . Calculating these derivatives requires that we know the form of the function \begin_inset Formula $f$ \end_inset . \end_layout \begin_layout Standard \series bold \emph on This section is not correct - math doesn't make sense and need to fix. \end_layout \begin_layout Standard Starting with inverse: \begin_inset Formula \begin{align*} X^{-1}\exp\partial i & =\left(X\exp\left[\partial x\right]\right)^{-1}\\ & =\left(\exp-\left[\partial x\right]\right)X^{-1}\\ \exp\partial i & =X\left(\exp-\left[\partial x\right]\right)X^{-1}\\ & =\exp-X\left[\partial x\right]X^{-1}\\ \partial i & =-X\left[\partial x\right]X^{-1}\end{align*} \end_inset \end_layout \begin_layout Standard Compose can be derived similarly: \begin_inset Formula \begin{align*} AB\exp\partial c & =A\left(\exp\left[\partial a\right]\right)B\\ \exp\partial c & =B^{-1}\left(\exp\left[\partial a\right]\right)B\\ \partial c & =B^{-1}\left[\partial a\right]B\end{align*} \end_inset \end_layout \begin_layout Section Rot2 (in gtsam) \end_layout \begin_layout Standard A rotation is stored as \begin_inset Formula $(\cos\theta,\sin\theta)$ \end_inset . An incremental rotation is applied using the trigonometric sum rule: \begin_inset Formula \[ \cos\theta'=\cos\theta\cos\delta-\sin\theta\sin\delta\] \end_inset \begin_inset Formula \[ \sin\theta'=\sin\theta\cos\delta+\cos\theta\sin\delta\] \end_inset where \begin_inset Formula $\delta$ \end_inset is an incremental rotation angle. The derivatives of \series bold \emph on rotate \series default \emph default are then found easily, using \begin_inset Formula \begin{eqnarray*} \frac{\partial x'}{\partial\delta} & = & \frac{\partial(x\cos\theta'-y\sin\theta')}{\partial\delta}\\ & = & \frac{\partial(x(\cos\theta\cos\delta-\sin\theta\sin\delta)-y(\sin\theta\cos\delta+\cos\theta\sin\delta))}{\partial\delta}\\ & = & x(-\cos\theta\sin\delta-\sin\theta\cos\delta)-y(-\sin\theta\sin\delta+\cos\theta\cos\delta)\\ & = & -x\sin\theta-y\cos\theta=-y'\end{eqnarray*} \end_inset \end_layout \begin_layout Standard \begin_inset Formula \begin{eqnarray*} \frac{\partial y'}{\partial\delta} & = & \frac{\partial(x\sin\theta'+y\cos\theta')}{\partial\delta}\\ & = & \frac{\partial(x(\sin\theta\cos\delta+\cos\theta\sin\delta)+y(\cos\theta\cos\delta-\sin\theta\sin\delta))}{\partial\delta}\\ & = & x(-\sin\theta\sin\delta+\cos\theta\cos\delta)+y(-\cos\theta\sin\delta-\sin\theta\cos\delta)\\ & = & x\cos\theta-y\sin\theta=x'\end{eqnarray*} \end_inset \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial p'}{\partial p}=\frac{\partial(Rp)}{\partial p}=R\] \end_inset Similarly, unrotate \end_layout \begin_layout Standard \begin_inset Formula \begin{eqnarray*} \frac{\partial x'}{\partial\delta} & = & \frac{\partial(x\cos\theta'+y\sin\theta')}{\partial\delta}\\ & = & \frac{\partial(x(\cos\theta\cos\delta-\sin\theta\sin\delta)+y(\sin\theta\cos\delta+\cos\theta\sin\delta))}{\partial\delta}\\ & = & x(-\cos\theta\sin\delta-\sin\theta\cos\delta)+y(-\sin\theta\sin\delta+\cos\theta\cos\delta)\\ & = & -x\sin\theta+y\cos\theta=y'\end{eqnarray*} \end_inset \end_layout \begin_layout Standard \begin_inset Formula \begin{eqnarray*} \frac{\partial y'}{\partial\delta} & = & \frac{\partial(-x\sin\theta'+y\cos\theta')}{\partial\delta}\\ & = & \frac{\partial(-x(\sin\theta\cos\delta+\cos\theta\sin\delta)+y(\cos\theta\cos\delta-\sin\theta\sin\delta))}{\partial\delta}\\ & = & -x(-\sin\theta\sin\delta+\cos\theta\cos\delta)+y(-\cos\theta\sin\delta-\sin\theta\cos\delta)\\ & = & -x\cos\theta-y\sin\theta=-x'\end{eqnarray*} \end_inset \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial p'}{\partial p}=\frac{\partial(Rp)}{\partial p}=R\] \end_inset \end_layout \begin_layout Section Point3 \end_layout \begin_layout Standard A cross product \begin_inset Formula $a\times b$ \end_inset can be written as a matrix multiplication \begin_inset Formula \[ a\times b=\Skew ab\] \end_inset where \begin_inset Formula $\Skew a$ \end_inset is a skew-symmetric matrix defined as \begin_inset Formula \[ \Skew{x,y,z}=\left[\begin{array}{ccc} 0 & -z & y\\ z & 0 & -x\\ -y & x & 0\end{array}\right]\] \end_inset We also have \begin_inset Formula \[ a^{T}\Skew b=-(\Skew ba)^{T}=-(a\times b)^{T}\] \end_inset The derivative of a cross product \begin_inset Formula \begin{equation} \frac{\partial(a\times b)}{\partial a}=\Skew{-b}\label{eq:Dcross1}\end{equation} \end_inset \begin_inset Formula \begin{equation} \frac{\partial(a\times b)}{\partial b}=\Skew a\label{eq:Dcross2}\end{equation} \end_inset \end_layout \begin_layout Section Rot3 \end_layout \begin_layout Standard An incremental rotation is applied as (switched to right-multiply Jan 25 2010) \begin_inset Formula \[ R'=R(I+\Omega)\] \end_inset where \begin_inset Formula $\Omega=\Skew{\omega}$ \end_inset is the skew symmetric matrix corresponding to the incremental rotation angles \begin_inset Formula $\omega=(\omega_{x},\omega_{y},\omega_{z})$ \end_inset . The derivatives of \series bold \emph on rotate \series default \emph default are then found easily, using \begin_inset CommandInset ref LatexCommand eqref reference "eq:Dcross1" \end_inset : \begin_inset Formula \[ \frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=\frac{\partial(R\left(\omega\times x\right))}{\partial\omega}=R\frac{\partial\left(\omega\times x\right)}{\partial\omega}=R\Skew{-x}\] \end_inset \begin_inset Formula \[ \frac{\partial(Rx)}{\partial x}=R\] \end_inset \end_layout \begin_layout Standard For composition and transposing of rotation matrices the situation is a bit more complex. We want to figure out what incremental rotation \begin_inset Formula $\Omega'$ \end_inset on the composed matrix, will yield the same change as \begin_inset Formula $\Omega$ \end_inset applied to either the first ( \begin_inset Formula $A$ \end_inset ) or second argument \begin_inset Formula $(B$ \end_inset ). Hence, the derivative with respect to the second argument is now easy: \begin_inset Formula \begin{eqnarray*} (AB)(I+\Omega') & = & A\left[B(I+\Omega)\right]\\ AB+AB\Omega' & = & AB+AB\Omega\\ \Omega' & = & \Omega\\ \omega' & = & \omega\end{eqnarray*} \end_inset i.e. the derivative is the identity matrix. \end_layout \begin_layout Standard For the first argument of \series bold \emph on compose \series default \emph default , we will make use of useful property of rotation matrices \begin_inset Formula $A$ \end_inset : \begin_inset Formula \begin{eqnarray} A\Omega A^{T} & = & A\Omega\left[\begin{array}{ccc} a_{1} & a_{2} & a_{3}\end{array}\right]\nonumber \\ & = & A\left[\begin{array}{ccc} \omega\times a_{1} & \omega\times a_{2} & \omega\times a_{3}\end{array}\right]\nonumber \\ & = & \left[\begin{array}{ccc} a_{1}(\omega\times a_{1}) & a_{1}(\omega\times a_{2}) & a_{1}(\omega\times a_{3})\\ a_{2}(\omega\times a_{1}) & a_{2}(\omega\times a_{2}) & a_{2}(\omega\times a_{3})\\ a_{3}(\omega\times a_{1}) & a_{3}(\omega\times a_{2}) & a_{3}(\omega\times a_{3})\end{array}\right]\nonumber \\ & = & \left[\begin{array}{ccc} \omega(a_{1}\times a_{1}) & \omega(a_{2}\times a_{1}) & \omega(a_{3}\times a_{1})\\ \omega(a_{1}\times a_{2}) & \omega(a_{2}\times a_{2}) & \omega(a_{3}\times a_{2})\\ \omega(a_{1}\times a_{3}) & \omega(a_{2}\times a_{3}) & \omega(a_{3}\times a_{3})\end{array}\right]\nonumber \\ & = & \left[\begin{array}{ccc} 0 & -\omega a_{3} & \omega a_{2}\\ \omega a_{3} & 0 & -\omega a_{1}\\ -\omega a_{2} & \omega a_{1} & 0\end{array}\right]\nonumber \\ & = & \Skew{A\omega}\label{eq:property1}\end{eqnarray} \end_inset where \begin_inset Formula $a_{x}$ \end_inset are the \emph on rows \emph default of \begin_inset Formula $A$ \end_inset , we made use of the orthogonality of rotation matrices, and the triple product rule: \begin_inset Formula \[ a(b\times c)=b(c\times a)=c(a\times b)\] \end_inset The derivative in the first argument of \series bold \emph on compose \series default \emph default can then be found using \begin_inset CommandInset ref LatexCommand eqref reference "eq:property1" \end_inset : \end_layout \begin_layout Standard \begin_inset Formula \begin{eqnarray*} (AB)(I+\Omega') & = & A(I+\Omega)B\\ AB+AB\Omega' & = & AB+A\Omega B\\ B\Omega' & = & \Omega B\\ \Omega' & = & B^{T}\Omega B=\Skew{B^{T}\omega}\\ \omega' & = & B^{T}\omega\end{eqnarray*} \end_inset The derivative of \series bold \emph on transpose \series default \emph default can be found similarly: \begin_inset Formula \begin{eqnarray*} A^{T}(I+\Omega') & = & \left[A(I+\Omega)\right]^{T}\\ A^{T}+A^{T}\Omega' & = & (I-\Omega)A^{T}\\ A^{T}\Omega' & = & -\Omega A^{T}\\ \Omega' & = & -A\Omega A^{T}\\ & = & -\Skew{A\omega}\\ \omega' & = & -A\omega\end{eqnarray*} \end_inset \end_layout \begin_layout Standard Let's find the derivative of \begin_inset Formula $between(A,B)=compose(inverse(A),B)$ \end_inset , so \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial c(i(A),B)}{\partial a}=dc1(i(A),B)di(A)=-B^{T}A=-between(B,A)\] \end_inset \begin_inset Formula \[ \frac{\partial c(i(A),B)}{\partial b}=dc2(i(A),B)=I_{3}\] \end_inset Similarly, the derivative of \begin_inset Formula $unrotate(R,x)=rotate(inverse(R),x)$ \end_inset , so \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial r(i(R),x)}{\partial\omega}=dr1(i(R),x)di(R)=R^{T}\Skew xR=\Skew{R^{T}x}\] \end_inset \begin_inset Formula \[ \frac{\partial r(i(R),x)}{\partial x}=i(R)=R^{T}\] \end_inset \end_layout \begin_layout Section Pose3 (gtsam, old-style exmap) \end_layout \begin_layout Standard In the old-style, we have \end_layout \begin_layout Standard \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none \begin_inset Formula $R'=R(I+\Omega)$ \end_inset \end_layout \begin_layout Standard \begin_inset Formula $t'=t+dt$ \end_inset \end_layout \begin_layout Standard In this case, the derivative of \series bold \emph on transform_from \series default \emph default , \begin_inset Formula $Rx+t$ \end_inset : \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial(R(I+\Omega)x+t)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=\frac{\partial(R\left(\omega\times x\right))}{\partial\omega}=R\Skew{-x}\] \end_inset and with respect to \begin_inset Formula $dt$ \end_inset is easy: \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial(Rx+t+dt)}{\partial dt}=I\] \end_inset The derivative of \series bold \emph on transform_to \series default \emph default , \begin_inset Formula $inv(R)(x-t)$ \end_inset we can obtain using the chain rule: \begin_inset Formula \[ \frac{\partial(inv(R)(x-t))}{\partial\omega}=\frac{\partial unrot(R,(x-t))}{\partial\omega}=skew(R^{T}\left(x-t\right))\] \end_inset \end_layout \begin_layout Standard and with respect to \begin_inset Formula $dt$ \end_inset is easy: \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial(R^{T}(x-t-dt))}{\partial dt}=-R^{T}\] \end_inset \end_layout \begin_layout Standard The derivative of \begin_inset Formula $inverse=R^{T},-R^{T}t=R^{T}(I,-t)$ \end_inset , first derivative of rotation in rotation argument: \end_layout \begin_layout Standard The partials \begin_inset Formula \[ \frac{\partial\omega^{\prime}}{\partial\omega}=\frac{\partial inv(R)}{\partial\omega}=-R\] \end_inset \begin_inset Formula \[ \frac{\partial t^{\prime}}{\partial\omega}=\frac{-\partial unrot(R,t)}{\partial\omega}=-skew(R^{T}t)\] \end_inset \begin_inset Formula \[ \frac{\partial\omega^{\prime}}{\partial t}=\mathbf{0}\] \end_inset \begin_inset Formula \[ \frac{\partial t^{\prime}}{\partial t}=\frac{-\partial unrot(R,t)}{\partial t}=-R^{T}\] \end_inset \series bold old stuff: \series default \begin_inset Formula \begin{eqnarray*} (I+\Omega')R^{T} & = & \left((I+\Omega)R\right)^{T}\\ R^{T}+\Omega'R^{T} & = & R^{T}(I-\Omega)\\ \Omega'R^{T} & = & -R^{T}\Omega\\ \Omega' & = & -R^{T}\Omega R=-\Skew{R^{T}\omega}\\ \omega' & = & -R^{T}\omega\end{eqnarray*} \end_inset \end_layout \begin_layout Standard Now \series bold \emph on compose \series default \emph default , first w.r.t. a change in rotation in the first argument: \begin_inset Formula \begin{align*} AB & =\left(T_{A}R_{A}T_{B}\right)\left(R_{A}R_{B}\right)\\ \left(T_{A}R_{A}T_{B}\left(I+T^{\prime}\right)\right)\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(T_{A}R_{A}\left(I+\Omega\right)T_{B}\right)\left(R_{A}\left(I+\Omega\right)R_{B}\right)\\ \textrm{translation only:}\\ T_{A}R_{A}T_{B}\left(I+T^{\prime}\right) & =T_{A}R_{A}\left(I+\Omega\right)T_{B}\\ T_{B}\left(I+T^{\prime}\right) & =\left(I+\Omega\right)T_{B}\\ T_{B}+T_{B}T^{\prime} & =T_{B}+\Omega T_{B}\\ T^{\prime} & =T_{B}^{-1}skew(\omega)T_{B}\\ T^{\prime} & =skew(T_{B}\omega)\,???\\ \textrm{rotation only:}\\ R_{A}R_{B}\left(I+\Omega^{\prime}\right) & =R_{A}\left(I+\Omega\right)R_{B}\\ R_{B}\Omega^{\prime} & =\Omega R_{B}\\ \Omega^{\prime} & =R_{B}^{T}\Omega R_{B}\\ & =skew(R_{B}^{T}\omega)\\ \omega^{\prime} & =R_{B}^{T}\omega\end{align*} \end_inset \end_layout \begin_layout Standard And w.r.t. a rotation in the second argument: \begin_inset Formula \begin{align*} \left(T_{A}R_{A}T_{B}\left(I+T^{\prime}\right)\right)\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(T_{A}R_{A}T_{B}\right)\left(R_{A}R_{B}\left(I+\Omega\right)\right)\\ \left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(R_{A}R_{B}\left(I+\Omega\right)\right)\\ \omega^{\prime} & =\omega\\ t^{\prime} & =0\end{align*} \end_inset w.r.t. a translation in the second argument: \begin_inset Formula \begin{align*} \left(T_{A}R_{A}T_{B}\left(I+T^{\prime}\right)\right)\left(R_{A}R_{B}\left(I+\Omega^{\prime}\right)\right) & =\left(T_{A}R_{A}T_{B}\left(I+T\right)\right)\left(R_{A}R_{B}\right)\\ \omega^{\prime} & =0\\ t^{\prime} & =t\end{align*} \end_inset \end_layout \begin_layout Standard Finally, \series bold \emph on between \series default \emph default in the first argument: \begin_inset Formula \begin{align*} \frac{\partial A^{-1}B}{\partial A} & =\frac{\partial c\left(A^{-1},B\right)}{\partial A^{-1}}\frac{\partial inv(A)}{A}\\ \frac{\partial A^{-1}B}{B} & =\frac{\partial c\left(A^{-1},B\right)}{\partial B}\end{align*} \end_inset \end_layout \begin_layout Section Pose3 (gtsam, new-style exmap) \end_layout \begin_layout Standard In the new-style exponential map, Pose3 is composed with a delta pose as follows \end_layout \begin_layout Standard \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none \begin_inset Formula $R'=(I+\Omega)R$ \end_inset \end_layout \begin_layout Standard \begin_inset Formula $t'=(I+\Omega)t+dt$ \end_inset \end_layout \begin_layout Standard The derivative of transform_from, \begin_inset Formula $Rx+t$ \end_inset : \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial((I+\Omega)Rx+(I+\Omega)t)}{\partial\omega}=\frac{\partial(\Omega(Rx+t))}{\partial\omega}=\frac{\partial(\omega\times(Rx+t))}{\partial\omega}=-\Skew{Rx+t}\] \end_inset and with respect to \begin_inset Formula $dt$ \end_inset is easy: \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial(Rx+t+dt)}{\partial dt}=I\] \end_inset The derivative of transform_to, \begin_inset Formula $R^{T}(x-t)$ \end_inset , eludes me. The calculation below is just an attempt: \end_layout \begin_layout Standard Noting that \family roman \series medium \shape up \size normal \emph off \bar no \noun off \color none \begin_inset Formula $R'^{T}=R^{T}(I-\Omega)$ \end_inset , and \begin_inset Formula $(I-\Omega)(x-(I+\Omega)t)=(I-\Omega)(x-t-\Omega t)=x-t-dt-\Omega x+\Omega^{2}t$ \end_inset \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial(R'^{T}(x-t'))}{\partial\omega}=\frac{\partial(R^{T}(I-\Omega)(x-(I+\Omega)t))}{\partial\omega}=-\frac{\partial(R^{T}(\Omega(x-\Omega t)))}{\partial\omega}\] \end_inset \begin_inset Formula \[ -\frac{\partial(\Skew{R^{T}\omega}R^{T}x)}{\partial\omega}=\Skew{R^{T}x}\frac{\partial(R^{T}\omega)}{\partial\omega}=\Skew{R^{T}x}R^{T}\] \end_inset \begin_inset Formula \[ =\frac{\partial(R^{T}\Omega^{2}t)}{\partial\omega}+\Skew{R^{T}x}R^{T}\] \end_inset and with respect to \begin_inset Formula $dt$ \end_inset is easy: \end_layout \begin_layout Standard \begin_inset Formula \[ \frac{\partial(R^{T}(x-t-dt))}{\partial dt}=-R^{T}\] \end_inset \end_layout \begin_layout Section Line3vd \end_layout \begin_layout Standard One representation of a line is through 2 vectors \begin_inset Formula $(v,d)$ \end_inset , where \begin_inset Formula $v$ \end_inset is the direction and the vector \begin_inset Formula $d$ \end_inset points from the orgin to the closest point on the line. \end_layout \begin_layout Standard In this representation, transforming a 3D line from a world coordinate frame to a camera at \begin_inset Formula $(R_{w}^{c},t^{w})$ \end_inset is done by \begin_inset Formula \[ v^{c}=R_{w}^{c}v^{w}\] \end_inset \begin_inset Formula \[ d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)\] \end_inset \end_layout \begin_layout Section Line3 \end_layout \begin_layout Standard For 3D lines, we use a parameterization due to C.J. Taylor, using a rotation matrix \begin_inset Formula $R$ \end_inset and 2 scalars \begin_inset Formula $a$ \end_inset and \begin_inset Formula $b$ \end_inset . The line direction \begin_inset Formula $v$ \end_inset is simply the Z-axis of the rotated frame, i.e., \begin_inset Formula $v=R_{3}$ \end_inset , while the vector \begin_inset Formula $d$ \end_inset is given by \begin_inset Formula $d=aR_{1}+bR_{2}$ \end_inset . \end_layout \begin_layout Standard Now, we will \emph on not \emph default use the incremental rotation scheme we used for rotations: because the matrix R translates from the line coordinate frame to the world frame, we need to apply the incremental rotation on the right-side: \begin_inset Formula \[ R'=R(I+\Omega)\] \end_inset Projecting a line to 2D can be done easily, as both \begin_inset Formula $v$ \end_inset and \begin_inset Formula $d$ \end_inset are also the 2D homogenous coordinates of two points on the projected line, and hence we have \begin_inset Formula \begin{eqnarray*} l & = & v\times d\\ & = & R_{3}\times\left(aR_{1}+bR_{2}\right)\\ & = & a\left(R_{3}\times R_{1}\right)+b\left(R_{3}\times R_{2}\right)\\ & = & aR_{2}-bR_{1}\end{eqnarray*} \end_inset This can be written as a rotation of a point, \begin_inset Formula \[ l=R\left(\begin{array}{c} -b\\ a\\ 0\end{array}\right)\] \end_inset but because the incremental rotation is now done on the right, we need to figure out the derivatives again: \begin_inset Formula \begin{equation} \frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=R\frac{\partial(\Omega x)}{\partial\omega}=R\Skew{-x}\label{eq:rotateRight}\end{equation} \end_inset and hence the derivative of the projection \begin_inset Formula $l$ \end_inset with respect to the rotation matrix \begin_inset Formula $R$ \end_inset of the 3D line is \begin_inset Formula \begin{equation} \frac{\partial(l)}{\partial\omega}=R\Skew{\left(\begin{array}{c} b\\ -a\\ 0\end{array}\right)}=\left[\begin{array}{ccc} aR_{3} & bR_{3} & -(aR_{1}+bR_{2})\end{array}\right]\end{equation} \end_inset or the \begin_inset Formula $a,b$ \end_inset scalars: \begin_inset Formula \[ \frac{\partial(l)}{\partial a}=R_{2}\] \end_inset \begin_inset Formula \[ \frac{\partial(l)}{\partial b}=-R_{1}\] \end_inset \end_layout \begin_layout Standard Transforming a 3D line \begin_inset Formula $(R,(a,b))$ \end_inset from a world coordinate frame to a camera frame \begin_inset Formula $(R_{w}^{c},t^{w})$ \end_inset is done by \end_layout \begin_layout Standard \begin_inset Formula \[ R'=R_{w}^{c}R\] \end_inset \begin_inset Formula \[ a'=a-R_{1}^{T}t^{w}\] \end_inset \begin_inset Formula \[ b'=b-R_{2}^{T}t^{w}\] \end_inset Again, we need to redo the derivatives, as R is incremented from the right. The first argument is incremented from the left, but the result is incremented on the right: \begin_inset Formula \begin{eqnarray*} R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\ I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\ \Omega' & = & R'^{T}\Skew{S\omega}R'\\ \Omega' & = & \Skew{R'^{T}S\omega}\\ \omega' & = & R'^{T}S\omega\end{eqnarray*} \end_inset For the second argument \begin_inset Formula $R$ \end_inset we now simply have: \begin_inset Formula \begin{eqnarray*} AB(I+\Omega') & = & AB(I+\Omega)\\ \Omega' & = & \Omega\\ \omega' & = & \omega\end{eqnarray*} \end_inset The scalar derivatives can be found by realizing that \begin_inset Formula \[ \left(\begin{array}{c} a'\\ b'\\ ...\end{array}\right)=\left(\begin{array}{c} a\\ b\\ 0\end{array}\right)-R^{T}t^{w}\] \end_inset where we don't care about the third row. Hence \begin_inset Formula \[ \frac{\partial(\left(R(I+\Omega_{2})\right)^{T}t^{w})}{\partial\omega}=-\frac{\partial(\Omega_{2}R^{T}t^{w})}{\partial\omega}=-\Skew{R^{T}t^{w}}=\left[\begin{array}{ccc} 0 & R_{3}^{T}t^{w} & -R_{2}^{T}t^{w}\\ -R_{3}^{T}t^{w} & 0 & R_{1}^{T}t^{w}\\ ... & ... & 0\end{array}\right]\] \end_inset \end_layout \begin_layout Section 2D Line Segments \end_layout \begin_layout Standard The error between an infinite line \begin_inset Formula $(a,b,c)$ \end_inset and a 2D line segment \begin_inset Formula $((x1,y1),(x2,y2))$ \end_inset is defined in Line3.ml. \end_layout \begin_layout Section \series bold Recovering Pose \end_layout \begin_layout Standard Below is the explanaition underlying Pose3.align, i.e. aligning two point clouds using SVD. Inspired but modified from CVOnline... \end_layout \begin_layout Standard \emph on Our \emph default model is \begin_inset Formula \[ p^{c}=R\left(p^{w}-t\right)\] \end_inset i.e., \begin_inset Formula $R$ \end_inset is from camera to world, and \begin_inset Formula $t$ \end_inset is the camera location in world coordinates. The objective function is \begin_inset Formula \begin{equation} \frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}+Rt\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}-t'\right)^{2}\label{eq:J}\end{equation} \end_inset where \begin_inset Formula $t'=-Rt$ \end_inset is the location of the origin in the camera frame. Taking the derivative with respect to \begin_inset Formula $t'$ \end_inset and setting to zero we have \begin_inset Formula \[ \sum\left(p^{c}-Rp^{w}-t'\right)=0\] \end_inset or \begin_inset Formula \begin{equation} t'=\frac{1}{n}\sum\left(p^{c}-Rp^{w}\right)=\bar{p}^{c}-R\bar{p}^{w}\label{eq:t}\end{equation} \end_inset here \begin_inset Formula $\bar{p}^{c}$ \end_inset and \begin_inset Formula $\bar{p}^{w}$ \end_inset are the point cloud centroids. Substituting back into \begin_inset CommandInset ref LatexCommand eqref reference "eq:J" \end_inset , we get \begin_inset Formula \[ \frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(\left(p^{c}-\bar{p}^{c}\right)-R\left(p^{w}-\bar{p}^{w}\right)\right)^{2}=\frac{1}{2}\sum\left(\hat{p}^{c}-R\hat{p}^{w}\right)^{2}\] \end_inset Now, to minimize the above it suffices to maximize (see CVOnline) \begin_inset Formula \[ \mathop{trace}\left(R^{T}C\right)\] \end_inset where \begin_inset Formula $C=\sum\hat{p}^{c}\left(\hat{p}^{w}\right)^{T}$ \end_inset is the correlation matrix. Intuitively, the cloud of points is rotated to align with the principal axes. This can be achieved by SVD decomposition on \begin_inset Formula $C$ \end_inset \begin_inset Formula \[ C=USV^{T}\] \end_inset and setting \begin_inset Formula \[ R=UV^{T}\] \end_inset Clearly, from \begin_inset CommandInset ref LatexCommand eqref reference "eq:t" \end_inset we then also recover the optimal \begin_inset Formula $t$ \end_inset as \begin_inset Formula \[ t=\bar{p}^{w}-R^{T}\bar{p}^{c}\] \end_inset \end_layout \end_body \end_document