1270 lines
26 KiB
Plaintext
1270 lines
26 KiB
Plaintext
#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
|