2772 lines
52 KiB
Plaintext
2772 lines
52 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 true
|
|
\use_amsmath 1
|
|
\use_esint 0
|
|
\cite_engine basic
|
|
\use_bibtopic false
|
|
\paperorientation portrait
|
|
\leftmargin 1in
|
|
\topmargin 1in
|
|
\rightmargin 1in
|
|
\bottommargin 1in
|
|
\secnumdepth 3
|
|
\tocdepth 3
|
|
\paragraph_separation indent
|
|
\defskip medskip
|
|
\quotes_language english
|
|
\papercolumns 1
|
|
\papersides 1
|
|
\paperpagestyle default
|
|
\tracking_changes false
|
|
\output_changes false
|
|
\author ""
|
|
\author ""
|
|
\end_header
|
|
|
|
\begin_body
|
|
|
|
\begin_layout Title
|
|
Geometry Derivatives and Other Hairy Math
|
|
\end_layout
|
|
|
|
\begin_layout Author
|
|
Frank Dellaert
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Review of Lie Groups
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\xhat}{\hat{x}}
|
|
{\hat{x}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\yhat}{\hat{y}}
|
|
{\hat{y}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Ad}[1]{Ad_{#1}}
|
|
{Ad_{#1}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\define}{\stackrel{\Delta}{=}}
|
|
{\stackrel{\Delta}{=}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\gg}{\mathfrak{g}}
|
|
{\mathfrak{g}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rn}{\mathbb{R}^{n}}
|
|
{\mathbb{R}^{n}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A Lie group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
is a manifold that possesses a smooth group operation.
|
|
Associated with it is a Lie Algebra
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
which, loosely speaking, can be identified with the tangent space at the
|
|
identity and completely defines how the groups behaves around the identity.
|
|
There is a mapping from
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
back to
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
, called the exponential map
|
|
\begin_inset Formula \[
|
|
\exp:\gg\rightarrow G\]
|
|
|
|
\end_inset
|
|
|
|
and a corresponding inverse
|
|
\begin_inset Formula \[
|
|
\log:G\rightarrow\gg\]
|
|
|
|
\end_inset
|
|
|
|
that maps elements in G to an element in
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
.
|
|
For
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional matrix Lie groups, the Lie algebra
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
is isomorphic to
|
|
\begin_inset Formula $\mathbb{R}^{n}$
|
|
\end_inset
|
|
|
|
, and we can define the map
|
|
\begin_inset Formula \[
|
|
\hat{}:\mathbb{R}^{n}\rightarrow\gg\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\hat{}:x\rightarrow\xhat\]
|
|
|
|
\end_inset
|
|
|
|
which maps
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-vectors
|
|
\begin_inset Formula $x\in$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\Rn$
|
|
\end_inset
|
|
|
|
to elements of
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
.
|
|
In the case of matrix Lie groups, the elements
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $\gg$
|
|
\end_inset
|
|
|
|
are
|
|
\begin_inset Formula $n\times n$
|
|
\end_inset
|
|
|
|
matrices.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Below we frequently make use of the equality
|
|
\begin_inset Foot
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
http://en.wikipedia.org/wiki/Exponential_map
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
ge^{\xhat}g^{-1}=e^{\Ad g{\xhat}}\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Ad g:\gg\rightarrow\mathfrak{\gg}$
|
|
\end_inset
|
|
|
|
is a map parameterized by a group element
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
.
|
|
The intuitive explanation is that a change
|
|
\begin_inset Formula $\exp\left(\xhat\right)$
|
|
\end_inset
|
|
|
|
defined around the orgin, but applied at the group element
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
, can be written in one step by taking the adjoint
|
|
\begin_inset Formula $\Ad g{\xhat}$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
.
|
|
In the case of a matrix group the ajoint can be written as
|
|
\begin_inset Foot
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
http://en.wikipedia.org/wiki/Adjoint_representation_of_a_Lie_group
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\Ad T{\xhat}\define Te^{\xhat}T^{-1}\]
|
|
|
|
\end_inset
|
|
|
|
and hence we have
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
Te^{\xhat}T^{-1}=e^{T\xhat T^{-1}}\]
|
|
|
|
\end_inset
|
|
|
|
where both
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
are
|
|
\begin_inset Formula $n\times n$
|
|
\end_inset
|
|
|
|
matrices for an
|
|
\begin_inset Formula $n$
|
|
\end_inset
|
|
|
|
-dimensional Lie group.
|
|
Below we introduce the most important Lie groups that we deal with.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Derivatives of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The derivatives for
|
|
\emph on
|
|
inverse, compose
|
|
\emph default
|
|
, and
|
|
\emph on
|
|
between
|
|
\emph default
|
|
can be derived from Lie group principles.
|
|
|
|
\begin_inset Note Note
|
|
status collapsed
|
|
|
|
\begin_layout Plain Layout
|
|
To find the derivatives of these functions, we look for the necessary
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
delta
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
in the tangent space of the function
|
|
\emph on
|
|
output
|
|
\emph default
|
|
|
|
\begin_inset Formula $f(g)$
|
|
\end_inset
|
|
|
|
that corresponds to a
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
delta
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
in the tangent space of the function
|
|
\emph on
|
|
input
|
|
\emph default
|
|
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
Specifically, to find the derivative of a function
|
|
\begin_inset Formula $f\left(g\right)$
|
|
\end_inset
|
|
|
|
, we want to find the Lie algebra element
|
|
\begin_inset Formula $\yhat\in\gg$
|
|
\end_inset
|
|
|
|
, that will result from changing
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
using
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
, also in exponential coordinates:
|
|
\begin_inset Formula \[
|
|
f\left(g\right)e^{\yhat}=f\left(ge^{\xhat}\right)\]
|
|
|
|
\end_inset
|
|
|
|
Calculating these derivatives requires that we know the form of the function
|
|
|
|
\begin_inset Formula $f$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Starting with
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
, i.e.,
|
|
\begin_inset Formula $f(g)=g^{-1}$
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula \begin{align}
|
|
g^{-1}e^{\yhat} & =\left(ge^{\xhat}\right)^{-1}=e^{-\xhat}g^{-1}\nonumber \\
|
|
e^{\yhat} & =ge^{-\xhat}g^{-1}=e^{\Ad g\left(-\xhat\right)}\nonumber \\
|
|
\yhat & =\Ad g\left(-\xhat\right)\label{eq:Dinverse}\end{align}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In other words, and this is very intuitive in hindsight, the inverse is
|
|
just negation of
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
, along with an adjoint to make sure it is applied in the right frame!
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\series bold
|
|
Compose
|
|
\series default
|
|
can be derived similarly.
|
|
Let us define two functions to find the derivatives in first and second
|
|
arguments:
|
|
\begin_inset Formula \[
|
|
f_{1}(g)=gh\mbox{ and }f_{2}(h)=gh\]
|
|
|
|
\end_inset
|
|
|
|
The latter is easiest, as a change
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
in the second argument
|
|
\begin_inset Formula $h$
|
|
\end_inset
|
|
|
|
simply gets applied to the result
|
|
\begin_inset Formula $gh$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula \begin{align}
|
|
f_{2}(h)e^{\yhat} & =f_{2}\left(he^{\xhat}\right)\nonumber \\
|
|
ghe^{\yhat} & =ghe^{\xhat}\nonumber \\
|
|
\yhat & =\xhat\label{eq:Dcompose2}\end{align}
|
|
|
|
\end_inset
|
|
|
|
The derivative for the first argument is a bit trickier:
|
|
\begin_inset Formula \begin{align}
|
|
f_{1}(g)e^{\yhat} & =f_{1}\left(ge^{\xhat}\right)\nonumber \\
|
|
ghe^{\yhat} & =ge^{\xhat}h\nonumber \\
|
|
e^{\yhat} & =h^{-1}e^{\xhat}h=e^{\Ad{h^{-1}}\xhat}\nonumber \\
|
|
\yhat & =\Ad{h^{-1}}\xhat\label{eq:Dcompose1}\end{align}
|
|
|
|
\end_inset
|
|
|
|
In other words, to apply a change
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
we first need to undo
|
|
\begin_inset Formula $h$
|
|
\end_inset
|
|
|
|
, then apply
|
|
\begin_inset Formula $\xhat$
|
|
\end_inset
|
|
|
|
, and then apply
|
|
\begin_inset Formula $h$
|
|
\end_inset
|
|
|
|
again.
|
|
All can be done in one step by simply applying
|
|
\begin_inset Formula $\Ad{h^{-1}}\xhat$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Finally, let us find the derivative of
|
|
\series bold
|
|
between
|
|
\series default
|
|
, defined as
|
|
\begin_inset Formula $between(g,h)=compose(inverse(g),h)$
|
|
\end_inset
|
|
|
|
.
|
|
The derivative in the second argument
|
|
\begin_inset Formula $h$
|
|
\end_inset
|
|
|
|
is similarly trivial:
|
|
\begin_inset Formula $\yhat=\xhat$
|
|
\end_inset
|
|
|
|
.
|
|
The first argument goes as follows:
|
|
\begin_inset Formula \begin{align}
|
|
f_{1}(g)e^{\yhat} & =f_{1}\left(ge^{\xhat}\right)\nonumber \\
|
|
g^{-1}he^{\yhat} & =\left(ge^{\xhat}\right)^{-1}h=e^{\left(-\xhat\right)}g^{-1}h\nonumber \\
|
|
e^{\yhat} & =\left(h^{-1}g\right)e^{\left(-\xhat\right)}\left(h^{-1}g\right)^{-1}=e^{\Ad{\left(h^{-1}g\right)}\left(-\xhat\right)}\nonumber \\
|
|
\yhat & =\Ad{\left(h^{-1}g\right)}\left(-\xhat\right)=\Ad{between\left(h,g\right)}\left(-\xhat\right)\label{eq:Dbetween1}\end{align}
|
|
|
|
\end_inset
|
|
|
|
Hence, now we undo
|
|
\begin_inset Formula $h$
|
|
\end_inset
|
|
|
|
and then apply the inverse
|
|
\begin_inset Formula $\left(-\xhat\right)$
|
|
\end_inset
|
|
|
|
in the
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
frame.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
|
|
{\frac{\partial#1}{\partial#2}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
|
|
{#1\biggr\rvert_{#2}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
|
|
{\at{\deriv{#1}{#2}}{#3}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
When a Lie group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
acts on a vector space
|
|
\begin_inset Formula $V$
|
|
\end_inset
|
|
|
|
, we are interested in the derivatives of
|
|
\begin_inset Formula \[
|
|
f_{1}\left(g\right)=gv\mbox{ and }f_{2}(v)=gv\]
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula $f_{1}:G\rightarrow V$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $f_{2}:V\rightarrow V$
|
|
\end_inset
|
|
|
|
.
|
|
The brilliance of Lie group theory is that we only need to know how the
|
|
generators of the group act around the group's identity element
|
|
\begin_inset Formula $g=id$
|
|
\end_inset
|
|
|
|
, and then we can use the Adjoint map to effectuate that action in the correct
|
|
frame of reference.
|
|
Specifically, if
|
|
\begin_inset Formula \[
|
|
H_{v}=\left[\begin{array}{ccc}
|
|
\frac{\partial f_{1}}{\partial x_{1}} & \ldots & \frac{\partial f_{1}}{\partial x_{n}}\end{array}\right]\rvert_{g=id}\]
|
|
|
|
\end_inset
|
|
|
|
is the
|
|
\begin_inset Formula $m\times n$
|
|
\end_inset
|
|
|
|
Jacobian of the group action on
|
|
\begin_inset Formula $\mbox{v\in}V$
|
|
\end_inset
|
|
|
|
with respect to an incremental change
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula \[
|
|
\Jac{f_{1}}xg=H_{v}\Ad g\]
|
|
|
|
\end_inset
|
|
|
|
The meaning of
|
|
\begin_inset Formula $H$
|
|
\end_inset
|
|
|
|
will depend on the group
|
|
\begin_inset Formula $G$
|
|
\end_inset
|
|
|
|
and will be very intuitive!
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
3D Rotations
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Basics
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}}
|
|
{\mathfrak{\mathbb{R}^{3}}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SOthree}{SO(3)}
|
|
{SO(3)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\sothree}{\mathfrak{so(3)}}
|
|
{\mathfrak{so(3)}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\what}{\hat{\omega}}
|
|
{\hat{\omega}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Skew}[1]{[#1]_{\times}}
|
|
{[#1]_{\times}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The Lie group
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
is a subgroup of the general linear group
|
|
\begin_inset Formula $GL(3)$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
invertible matrices.
|
|
Its Lie algebra
|
|
\begin_inset Formula $\sothree$
|
|
\end_inset
|
|
|
|
is the vector space of
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
skew-symmetric matrices.
|
|
The exponential map can be computed in closed form using Rodrigues' formula.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Since
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
is a three-dimensional manifold,
|
|
\begin_inset Formula $\sothree$
|
|
\end_inset
|
|
|
|
is isomorphic to
|
|
\begin_inset Formula $\Rthree$
|
|
\end_inset
|
|
|
|
and we define the map
|
|
\begin_inset Formula \[
|
|
\hat{}:\Rthree\rightarrow\sothree\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\hat{}:\omega\rightarrow\what=\Skew{\omega}\]
|
|
|
|
\end_inset
|
|
|
|
which maps 3-vectors
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
to skew-symmetric matrices
|
|
\begin_inset Formula $\Skew{\omega}$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula \[
|
|
\Skew{\omega}=\left[\begin{array}{ccc}
|
|
0 & -\omega_{z} & \omega_{y}\\
|
|
\omega_{z} & 0 & -\omega_{x}\\
|
|
-\omega_{y} & \omega_{x} & 0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
For every
|
|
\begin_inset Formula $3-$
|
|
\end_inset
|
|
|
|
vector
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
there is a corresponding rotation matrix
|
|
\begin_inset Formula \[
|
|
R=e^{\Skew{\omega}}\]
|
|
|
|
\end_inset
|
|
|
|
and this is defines the canonical parameterization of
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
, with
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
known as the canonical or exponential coordinates.
|
|
It is equivalent to the axis-angle representation for rotations, where
|
|
the unit vector
|
|
\begin_inset Formula $\omega/\left\Vert \omega\right\Vert $
|
|
\end_inset
|
|
|
|
defines the rotation axis, and its magnitude the amount of rotation
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
The Adjoint Map
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can prove the following identity for rotation matrices
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula \begin{eqnarray}
|
|
R\Skew{\omega}R^{T} & = & R\Skew{\omega}\left[\begin{array}{ccc}
|
|
a_{1} & a_{2} & a_{3}\end{array}\right]\nonumber \\
|
|
& = & R\left[\begin{array}{ccc}
|
|
\omega\times a_{1} & \omega\times a_{2} & \omega\times a_{3}\end{array}\right]\nonumber \\
|
|
& = & \left[\begin{array}{ccc}
|
|
a_{1}(\omega\times a_{1}) & a_{1}(\omega\times a_{2}) & a_{1}(\omega\times a_{3})\\
|
|
a_{2}(\omega\times a_{1}) & a_{2}(\omega\times a_{2}) & a_{2}(\omega\times a_{3})\\
|
|
a_{3}(\omega\times a_{1}) & a_{3}(\omega\times a_{2}) & a_{3}(\omega\times a_{3})\end{array}\right]\nonumber \\
|
|
& = & \left[\begin{array}{ccc}
|
|
\omega(a_{1}\times a_{1}) & \omega(a_{2}\times a_{1}) & \omega(a_{3}\times a_{1})\\
|
|
\omega(a_{1}\times a_{2}) & \omega(a_{2}\times a_{2}) & \omega(a_{3}\times a_{2})\\
|
|
\omega(a_{1}\times a_{3}) & \omega(a_{2}\times a_{3}) & \omega(a_{3}\times a_{3})\end{array}\right]\nonumber \\
|
|
& = & \left[\begin{array}{ccc}
|
|
0 & -\omega a_{3} & \omega a_{2}\\
|
|
\omega a_{3} & 0 & -\omega a_{1}\\
|
|
-\omega a_{2} & \omega a_{1} & 0\end{array}\right]\nonumber \\
|
|
& = & \Skew{R\omega}\label{eq:property1}\end{eqnarray}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $a_{1}$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $a_{2}$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $a_{3}$
|
|
\end_inset
|
|
|
|
are the
|
|
\emph on
|
|
rows
|
|
\emph default
|
|
of
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
.
|
|
Above we made use of the orthogonality of rotation matrices and the triple
|
|
product rule:
|
|
\begin_inset Formula \[
|
|
a(b\times c)=b(c\times a)=c(a\times b)\]
|
|
|
|
\end_inset
|
|
|
|
Hence, given property
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:property1"
|
|
|
|
\end_inset
|
|
|
|
, the adjoint map for
|
|
\begin_inset Formula $\sothree$
|
|
\end_inset
|
|
|
|
simplifies to
|
|
\begin_inset Formula \[
|
|
\Ad R{\Skew{\omega}}=R\Skew{\omega}R^{T}=\Skew{R\omega}\]
|
|
|
|
\end_inset
|
|
|
|
and this can be expressed in exponential coordinates simply by rotating
|
|
the axis
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $R\omega$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
As an example, to apply an axis-angle rotation
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
to a point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in the frame
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
, we could:
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
First transform
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
back to the world frame, apply
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
, and then rotate back:
|
|
\begin_inset Formula \[
|
|
q=Re^{\Skew{\omega}}R^{T}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Enumerate
|
|
Immediately apply the transformed axis-angle transformation
|
|
\begin_inset Formula $\Ad R{\Skew{\omega}}=\Skew{R\omega}$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula \[
|
|
q=e^{\Skew{R\omega}}p\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Hence, we are now in a position to simply posit the derivative of
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\Skew{\omega'} & = & \Ad R\left(\Skew{-\omega}\right)=\Skew{R(-\omega)}\\
|
|
\frac{\partial R^{T}}{\partial\omega} & = & -R\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
compose
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\Skew{\omega'} & = & \Ad{R_{2}^{T}}\left(\Skew{\omega}\right)=\Skew{R_{2}^{T}\omega}\\
|
|
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}} & = & R_{2}^{T}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
compose in its second argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}} & = & I_{3}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
between
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\Skew{\omega'} & = & \Ad{R_{2}^{T}R_{1}}\left(\Skew{-\omega}\right)=\Skew{R_{2}^{T}R_{1}(-\omega)}\\
|
|
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}} & = & -R_{2}^{T}R_{1}=-between(R_{2},R_{1})\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
and between in its second argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{2}} & = & I_{3}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The beauty of Lie group theory comes in play when we talk about the derivative
|
|
of a group action.
|
|
In the case of
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
the vector space is
|
|
\begin_inset Formula $\Rthree$
|
|
\end_inset
|
|
|
|
, and the group action corresponds to rotating a point
|
|
\begin_inset Formula \[
|
|
q=Rp\]
|
|
|
|
\end_inset
|
|
|
|
We would now like to know what an incremental rotation parameterized by
|
|
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
would do
|
|
\begin_inset Formula \[
|
|
\deriv q{\omega}=\deriv{}{\omega}\left(Rp\right)=\deriv{}{\omega}\left(e^{\Skew{\omega}}p\right)\]
|
|
|
|
\end_inset
|
|
|
|
Since
|
|
\begin_inset Formula \[
|
|
e^{A}=I+A+\frac{A^{2}}{2!}+\frac{A^{3}}{3!}+\ldots\]
|
|
|
|
\end_inset
|
|
|
|
and derivative is linear and we are only interested in first order we have
|
|
\begin_inset Formula \[
|
|
\deriv{e^{\Skew{\omega}}}{\omega}=\deriv{\Skew{\omega}}{\omega}=\omega_{x}G_{1}+\omega_{y}G_{2}+\omega_{z}G_{3}\]
|
|
|
|
\end_inset
|
|
|
|
Specifically, the generators for
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
are
|
|
\begin_inset Formula \[
|
|
G_{1}=\left(\begin{array}{ccc}
|
|
0 & 0 & 0\\
|
|
0 & 0 & -1\\
|
|
0 & 1 & 0\end{array}\right)\mbox{}G_{2}=\left(\begin{array}{ccc}
|
|
0 & 0 & 1\\
|
|
0 & 0 & 0\\
|
|
-1 & 0 & 0\end{array}\right)\mbox{ }G_{1}=\left(\begin{array}{ccc}
|
|
0 & -1 & 0\\
|
|
1 & 0 & 0\\
|
|
0 & 0 & 0\end{array}\right)\]
|
|
|
|
\end_inset
|
|
|
|
corresponding to a rotation around
|
|
\begin_inset Formula $X$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $Y$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $Z$
|
|
\end_inset
|
|
|
|
, respectively.
|
|
When given an incremental angular velocity
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
, we obtain the effect of the group action around the identity:
|
|
\begin_inset Formula \[
|
|
H\omega=\omega_{x}G_{1}+\omega_{y}G_{2}+\omega_{z}G_{3}=\left[\begin{array}{ccc}
|
|
0 & -\omega_{z} & \omega_{y}\\
|
|
\omega_{z} & 0 & -\omega_{x}\\
|
|
-\omega_{y} & \omega_{x} & 0\end{array}\right]=\Skew{\omega}\]
|
|
|
|
\end_inset
|
|
|
|
Hence, at the origin, the effect of an incremental action
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
on a point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
is a velocity
|
|
\begin_inset Formula \[
|
|
\Skew{\omega}p=\omega\times p\]
|
|
|
|
\end_inset
|
|
|
|
We can write this as a
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
Jacobian
|
|
\begin_inset Formula $H_{p}$
|
|
\end_inset
|
|
|
|
that is multipled with
|
|
\begin_inset Formula $\omega$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula \[
|
|
\omega\times p=-p\times\omega=-\Skew p\omega=H_{p}\omega\]
|
|
|
|
\end_inset
|
|
|
|
Now, if we want to apply this in a frame
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
, we need to do something quite similar to the Adjoint map: (a) transform
|
|
the point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
to the origin using
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
, apply the action
|
|
\begin_inset Formula $\Skew{\omega}$
|
|
\end_inset
|
|
|
|
, and transform back with
|
|
\begin_inset Formula $R^{T}$
|
|
\end_inset
|
|
|
|
.
|
|
In short
|
|
\begin_inset Formula \[
|
|
q=R^{T}e^{\Skew{\omega}}Rp=\exp\left(\Ad{R^{T}}\Skew{\omega}\right)p=\exp\left(\Skew{R^{T}\omega}\right)p\]
|
|
|
|
\end_inset
|
|
|
|
and hence the velocity becomes
|
|
\begin_inset Formula \[
|
|
\Skew{R^{T}\omega}\times p=-\Skew pR^{T}\omega=-R^{T}R\Skew pR^{T}\omega=-R^{T}\Skew{Rp}\omega=R^{T}H_{Rp}\omega\]
|
|
|
|
\end_inset
|
|
|
|
This is quite intuitive in hindsight: we transform
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
to
|
|
\begin_inset Formula $Rp$
|
|
\end_inset
|
|
|
|
, calculate the velocity by
|
|
\begin_inset Formula $H_{Rp}$
|
|
\end_inset
|
|
|
|
, and transform back by the rotation
|
|
\begin_inset Formula $R^{T}$
|
|
\end_inset
|
|
|
|
.
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
3D Rigid Transformations
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}}
|
|
{\mathfrak{\mathbb{R}^{6}}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SEthree}{SE(3)}
|
|
{SE(3)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\sethree}{\mathfrak{se(3)}}
|
|
{\mathfrak{se(3)}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\xihat}{\hat{\xi}}
|
|
{\hat{\xi}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The Lie group
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
is a subgroup of the general linear group
|
|
\begin_inset Formula $GL(4)$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $4\times4$
|
|
\end_inset
|
|
|
|
invertible matrices of the form
|
|
\begin_inset Formula \[
|
|
T\define\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $R\in\SOthree$
|
|
\end_inset
|
|
|
|
is a rotation matrix and
|
|
\begin_inset Formula $t\in\Rthree$
|
|
\end_inset
|
|
|
|
is a translation vector.
|
|
Its Lie algebra
|
|
\begin_inset Formula $\sethree$
|
|
\end_inset
|
|
|
|
is the vector space of
|
|
\begin_inset Formula $4\times4$
|
|
\end_inset
|
|
|
|
twists
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
parameterized by the
|
|
\emph on
|
|
twist coordinates
|
|
\emph default
|
|
|
|
\begin_inset Formula $\xi\in\Rsix$
|
|
\end_inset
|
|
|
|
, with the mapping
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Murray94book"
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\xi\define\left[\begin{array}{c}
|
|
\omega\\
|
|
v\end{array}\right]\rightarrow\xihat\define\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
Note we follow Frank Park's convention and reserve the first three components
|
|
for rotation, and the last three for translation.
|
|
Applying the exponential map to a twist
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
yields a screw motion yielding an element in
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula \[
|
|
T=\exp\xihat\]
|
|
|
|
\end_inset
|
|
|
|
A closed form solution for the exponential map is given in
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
after "page 42"
|
|
key "Murray94book"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
The Adjoint Map
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The adjoint is
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\Ad T{\xihat} & = & T\xihat T^{-1}\\
|
|
& = & \left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1\end{array}\right]\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0\end{array}\right]\left[\begin{array}{cc}
|
|
R^{T} & -R^{T}t\\
|
|
0 & 1\end{array}\right]\\
|
|
& = & \left[\begin{array}{cc}
|
|
\Skew{R\omega} & -\Skew{R\omega}t+Rv\\
|
|
0 & 0\end{array}\right]\\
|
|
& = & \left[\begin{array}{cc}
|
|
\Skew{R\omega} & t\times R\omega+Rv\\
|
|
0 & 0\end{array}\right]\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
From this we can express the Adjoint map in terms of twist coordinates (see
|
|
also
|
|
\begin_inset CommandInset citation
|
|
LatexCommand cite
|
|
key "Murray94book"
|
|
|
|
\end_inset
|
|
|
|
and FP):
|
|
\begin_inset Formula \[
|
|
\left[\begin{array}{c}
|
|
\omega'\\
|
|
v'\end{array}\right]=\left[\begin{array}{cc}
|
|
R & 0\\
|
|
\Skew tR & R\end{array}\right]\left[\begin{array}{c}
|
|
\omega\\
|
|
v\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Hence, as with
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
, we are now in a position to simply posit the derivative of
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial T^{-1}}{\partial\xi} & = & -\left[\begin{array}{cc}
|
|
R & 0\\
|
|
\Skew tR & R\end{array}\right]\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
(but unit test on the above fails !!!),
|
|
\series bold
|
|
compose
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}} & = & \left[\begin{array}{cc}
|
|
R_{2}^{T} & 0\\
|
|
\Skew{-R_{2}^{T}t}R_{2}^{T} & R_{2}^{T}\end{array}\right]\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
compose in its second argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}} & = & I_{6}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
between
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & -\left[\begin{array}{cc}
|
|
R & 0\\
|
|
\Skew tR & R\end{array}\right]\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
with
|
|
\begin_inset Formula \[
|
|
\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1\end{array}\right]=T_{1}^{^{-1}}T_{2}=between(T_{2},T_{1})\]
|
|
|
|
\end_inset
|
|
|
|
and between in its second argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
The derivatives of Actions
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The action of
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
on 3D points is done by embedding the points in
|
|
\begin_inset Formula $\mathbb{R}^{4}$
|
|
\end_inset
|
|
|
|
by using homogeneous coordinates
|
|
\begin_inset Formula \[
|
|
q=\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1\end{array}\right]\left[\begin{array}{c}
|
|
p\\
|
|
1\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The generators for
|
|
\begin_inset Formula $\SEthree$
|
|
\end_inset
|
|
|
|
are
|
|
\begin_inset Formula \[
|
|
G_{1}=\left(\begin{array}{cccc}
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & -1 & 0\\
|
|
0 & 1 & 0 & 0\\
|
|
0 & 0 & 0 & 0\end{array}\right)\mbox{}G_{2}=\left(\begin{array}{cccc}
|
|
0 & 0 & 1 & 0\\
|
|
0 & 0 & 0 & 0\\
|
|
-1 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\end{array}\right)\mbox{ }G_{1}=\left(\begin{array}{cccc}
|
|
0 & -1 & 0 & 0\\
|
|
1 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\end{array}\right)\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
G_{4}=\left(\begin{array}{cccc}
|
|
0 & 0 & 0 & 1\\
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\end{array}\right)\mbox{}G_{5}=\left(\begin{array}{cccc}
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 1\\
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\end{array}\right)\mbox{ }G_{6}=\left(\begin{array}{cccc}
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 0\\
|
|
0 & 0 & 0 & 1\\
|
|
0 & 0 & 0 & 0\end{array}\right)\]
|
|
|
|
\end_inset
|
|
|
|
and hence a twist around the origin applies to homogeneous coordinates
|
|
\begin_inset Formula $\hat{p}\in\mathbb{R}^{4}$
|
|
\end_inset
|
|
|
|
as
|
|
\begin_inset Formula \[
|
|
\sum_{i}\xi_{i}G_{i}=\left(\begin{array}{cccc}
|
|
0 & -\omega_{z} & \omega_{y} & v_{x}\\
|
|
\omega_{z} & 0 & -\omega_{x} & v_{y}\\
|
|
-\omega_{y} & \omega_{x} & 0 & v_{z}\\
|
|
0 & 0 & 0 & 0\end{array}\right)=\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0\end{array}\right]=\xihat\]
|
|
|
|
\end_inset
|
|
|
|
Hence, at the origin, an incremental action
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
on a point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
induces the velocity
|
|
\begin_inset Formula \[
|
|
\left[\begin{array}{cc}
|
|
\Skew{\omega} & v\\
|
|
0 & 0\end{array}\right]\left[\begin{array}{c}
|
|
p\\
|
|
1\end{array}\right]=\left[\begin{array}{c}
|
|
\omega\times p+v\\
|
|
0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
We can write this as a velocity in
|
|
\begin_inset Formula $\Rthree$
|
|
\end_inset
|
|
|
|
as the product of a
|
|
\begin_inset Formula $3\times6$
|
|
\end_inset
|
|
|
|
matrix
|
|
\begin_inset Formula $H(p)$
|
|
\end_inset
|
|
|
|
that acts upon the exponential coordinates
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
directly:
|
|
\begin_inset Formula \[
|
|
\omega\times p+v=-p\times\omega+v=\left[\begin{array}{cc}
|
|
-\Skew p & I_{3}\end{array}\right]\left[\begin{array}{c}
|
|
\omega\\
|
|
v\end{array}\right]=H(p)\xi\]
|
|
|
|
\end_inset
|
|
|
|
Now, if we want to apply this in a frame
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
, we can (1) transform the point
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
to the origin using
|
|
\begin_inset Formula $T^{-1}$
|
|
\end_inset
|
|
|
|
, apply the action
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
, and transform back to
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
.
|
|
In short
|
|
\begin_inset Formula \[
|
|
\left[\begin{array}{c}
|
|
q\\
|
|
1\end{array}\right]=Te^{\xihat}T^{-1}\left[\begin{array}{c}
|
|
p\\
|
|
1\end{array}\right]=\exp\left(\Ad T\xihat\right)\left[\begin{array}{c}
|
|
p\\
|
|
1\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
To get the velocity of the point in frame
|
|
\begin_inset Formula $T$
|
|
\end_inset
|
|
|
|
, we have
|
|
\begin_inset Formula \[
|
|
H(p)\Ad T\xi=\left[\begin{array}{cc}
|
|
-\Skew p & I_{3}\end{array}\right]\left[\begin{array}{cc}
|
|
R & 0\\
|
|
\Skew tR & R\end{array}\right]\xi=R\left[\begin{array}{cc}
|
|
-\Skew{T^{-1}p} & I_{3}\end{array}\right]\xi=H(T^{-1}p)\xi\]
|
|
|
|
\end_inset
|
|
|
|
where I made use of
|
|
\begin_inset Formula $\Skew{t-p}R=-RR^{T}\Skew{p-t}R=-R\Skew{R^{T}(p-t)}=-R\Skew{T^{-1}p}$
|
|
\end_inset
|
|
|
|
.
|
|
This is intuitive in hindsight: we transform
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
back to the orgin by
|
|
\begin_inset Formula $T^{-1}p$
|
|
\end_inset
|
|
|
|
, apply
|
|
\begin_inset Formula $H(.)$
|
|
\end_inset
|
|
|
|
to get a velocity, and only need the rotation
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
to transform it back to the orginal frame (as velocities are not affected
|
|
by translation).
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
2D Rotations
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
|
|
{\mathfrak{\mathbb{R}^{2}}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SOtwo}{SO(2)}
|
|
{SO(2)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\sotwo}{\mathfrak{so(2)}}
|
|
{\mathfrak{so(2)}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\that}{\hat{\theta}}
|
|
{\hat{\theta}}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\skew}[1]{[#1]_{+}}
|
|
{[#1]_{+}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The Lie group
|
|
\begin_inset Formula $\SOtwo$
|
|
\end_inset
|
|
|
|
is a subgroup of the general linear group
|
|
\begin_inset Formula $GL(2)$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $2\times2$
|
|
\end_inset
|
|
|
|
invertible matrices.
|
|
Its Lie algebra
|
|
\begin_inset Formula $\sotwo$
|
|
\end_inset
|
|
|
|
is the vector space of
|
|
\begin_inset Formula $2\times2$
|
|
\end_inset
|
|
|
|
skew-symmetric matrices.
|
|
Though simpler than
|
|
\begin_inset Formula $\SOthree$
|
|
\end_inset
|
|
|
|
it is
|
|
\emph on
|
|
commutative
|
|
\emph default
|
|
and hence things simplify in ways that do not generalize well, so we treat
|
|
it only now.
|
|
Since
|
|
\begin_inset Formula $\SOtwo$
|
|
\end_inset
|
|
|
|
is a one-dimensional manifold,
|
|
\begin_inset Formula $\sotwo$
|
|
\end_inset
|
|
|
|
is isomorphic to
|
|
\begin_inset Formula $\mathbb{R}$
|
|
\end_inset
|
|
|
|
and we define
|
|
\begin_inset Formula \[
|
|
\hat{}:\mathbb{R}\rightarrow\sotwo\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\hat{}:\theta\rightarrow\that=\skew{\theta}\]
|
|
|
|
\end_inset
|
|
|
|
which maps the angle
|
|
\begin_inset Formula $\theta$
|
|
\end_inset
|
|
|
|
to the
|
|
\begin_inset Formula $2\times2$
|
|
\end_inset
|
|
|
|
skew-symmetric matrix
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $\skew{\theta}$
|
|
\end_inset
|
|
|
|
:
|
|
\family default
|
|
\series default
|
|
\shape default
|
|
\size default
|
|
\emph default
|
|
\bar default
|
|
\noun default
|
|
\color inherit
|
|
|
|
\begin_inset Formula \[
|
|
\skew{\theta}=\left[\begin{array}{cc}
|
|
0 & -\theta\\
|
|
\theta & 0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
Note that
|
|
\begin_inset Formula \begin{equation}
|
|
\skew{\theta}\left[\begin{array}{c}
|
|
x\\
|
|
y\end{array}\right]=\theta R_{\pi/2}\left[\begin{array}{c}
|
|
x\\
|
|
y\end{array}\right]=\theta\left[\begin{array}{c}
|
|
-y\\
|
|
x\end{array}\right]\label{eq:RestrictedCross}\end{equation}
|
|
|
|
\end_inset
|
|
|
|
which acts like a restricted
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
cross product
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
in the plane.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The exponential map can be computed in closed form as
|
|
\begin_inset Formula \[
|
|
R=e^{\skew{\theta}}=\left[\begin{array}{cc}
|
|
\cos\theta & -\sin\theta\\
|
|
\sin\theta & \cos\theta\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The adjoint map for
|
|
\begin_inset Formula $\sotwo$
|
|
\end_inset
|
|
|
|
is trivially equal to the identity, as is the case for
|
|
\emph on
|
|
all
|
|
\emph default
|
|
commutative groups, and we have the derivative of
|
|
\series bold
|
|
inverse
|
|
\series default
|
|
,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial R^{T}}{\partial\theta} & = & -\Ad R=-1\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
compose
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\theta_{1}} & = & \Ad{R_{2}^{T}}=1\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
compose in its second argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(R_{1}R_{2}\right)}{\partial\theta_{2}} & = & 1\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
between
|
|
\series default
|
|
in its first argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\theta_{1}} & = & -\Ad{R_{2}^{T}R_{1}}=-1\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
and between in its second argument,
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\theta_{2}} & = & 1\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
2D Rigid Transformations
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SEtwo}{SE(2)}
|
|
{SE(2)}
|
|
\end_inset
|
|
|
|
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\setwo}{\mathfrak{se(2)}}
|
|
{\mathfrak{se(2)}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The Lie group
|
|
\begin_inset Formula $\SEtwo$
|
|
\end_inset
|
|
|
|
is a subgroup of the general linear group
|
|
\begin_inset Formula $GL(3)$
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
invertible matrices of the form
|
|
\begin_inset Formula \[
|
|
T\define\left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $R\in\SOtwo$
|
|
\end_inset
|
|
|
|
is a rotation matrix and
|
|
\begin_inset Formula $t\in\Rtwo$
|
|
\end_inset
|
|
|
|
is a translation vector.
|
|
Its Lie algebra
|
|
\begin_inset Formula $\setwo$
|
|
\end_inset
|
|
|
|
is the vector space of
|
|
\begin_inset Formula $3\times3$
|
|
\end_inset
|
|
|
|
twists
|
|
\begin_inset Formula $\xihat$
|
|
\end_inset
|
|
|
|
parameterized by the
|
|
\emph on
|
|
twist coordinates
|
|
\emph default
|
|
|
|
\begin_inset Formula $\xi\in\Rthree$
|
|
\end_inset
|
|
|
|
, with the mapping
|
|
\begin_inset Formula \[
|
|
\xi\define\left[\begin{array}{c}
|
|
v\\
|
|
\omega\end{array}\right]\rightarrow\xihat\define\left[\begin{array}{cc}
|
|
\skew{\omega} & v\\
|
|
0 & 0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
Note we think of robots as having a pose
|
|
\begin_inset Formula $(x,y,\theta)$
|
|
\end_inset
|
|
|
|
and hence I switched the order above, reserving the first two components
|
|
for translation and the last for rotation.
|
|
Applying the exponential map to a twist
|
|
\begin_inset Formula $\xi$
|
|
\end_inset
|
|
|
|
yields a screw motion yielding an element in
|
|
\begin_inset Formula $\SEtwo$
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula \[
|
|
T=\exp\xihat\]
|
|
|
|
\end_inset
|
|
|
|
A closed form solution for the exponential map is in the works...
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
The Adjoint Map
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The adjoint is
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\Ad T{\xihat} & = & T\xihat T^{-1}\\
|
|
& = & \left[\begin{array}{cc}
|
|
R & t\\
|
|
0 & 1\end{array}\right]\left[\begin{array}{cc}
|
|
\skew{\omega} & v\\
|
|
0 & 0\end{array}\right]\left[\begin{array}{cc}
|
|
R^{T} & -R^{T}t\\
|
|
0 & 1\end{array}\right]\\
|
|
& = & \left[\begin{array}{cc}
|
|
\skew{\omega} & -\skew{\omega}t+Rv\\
|
|
0 & 0\end{array}\right]\\
|
|
& = & \left[\begin{array}{cc}
|
|
\skew{\omega} & Rv-\omega R_{\pi/2}t\\
|
|
0 & 0\end{array}\right]\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
From this we can express the Adjoint map in terms of plane twist coordinates:
|
|
\begin_inset Formula \[
|
|
\left[\begin{array}{c}
|
|
v'\\
|
|
\omega'\end{array}\right]=\left[\begin{array}{cc}
|
|
R & -R_{\pi/2}t\\
|
|
0 & 1\end{array}\right]\left[\begin{array}{c}
|
|
v\\
|
|
\omega\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
Derivatives of Mappings
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
We can just define all derivatives in terms of the above adjoint map:
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial T^{^{-1}}}{\partial\xi} & = & -\Ad T\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}} & = & \Ad{T_{2}^{^{-1}}}=1\mbox{ and }\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}}=I_{3}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial\left(T_{1}^{-1}T_{2}\right)}{\partial\xi_{1}} & = & -\Ad{T_{2}^{^{-1}}T_{1}}=-\Ad{between(T_{2},T_{1})}\mbox{ and }\frac{\partial\left(T_{1}^{-1}T_{2}\right)}{\partial\xi_{2}}=I_{3}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Part
|
|
Old Stuff
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Rot2 (in gtsam)
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A rotation is stored as
|
|
\begin_inset Formula $(\cos\theta,\sin\theta)$
|
|
\end_inset
|
|
|
|
.
|
|
An incremental rotation is applied using the trigonometric sum rule:
|
|
\begin_inset Formula \[
|
|
\cos\theta'=\cos\theta\cos\delta-\sin\theta\sin\delta\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\sin\theta'=\sin\theta\cos\delta+\cos\theta\sin\delta\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
is an incremental rotation angle.
|
|
The derivatives of
|
|
\series bold
|
|
\emph on
|
|
rotate
|
|
\series default
|
|
\emph default
|
|
are then found easily, using
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial x'}{\partial\delta} & = & \frac{\partial(x\cos\theta'-y\sin\theta')}{\partial\delta}\\
|
|
& = & \frac{\partial(x(\cos\theta\cos\delta-\sin\theta\sin\delta)-y(\sin\theta\cos\delta+\cos\theta\sin\delta))}{\partial\delta}\\
|
|
& = & x(-\cos\theta\sin\delta-\sin\theta\cos\delta)-y(-\sin\theta\sin\delta+\cos\theta\cos\delta)\\
|
|
& = & -x\sin\theta-y\cos\theta=-y'\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial y'}{\partial\delta} & = & \frac{\partial(x\sin\theta'+y\cos\theta')}{\partial\delta}\\
|
|
& = & \frac{\partial(x(\sin\theta\cos\delta+\cos\theta\sin\delta)+y(\cos\theta\cos\delta-\sin\theta\sin\delta))}{\partial\delta}\\
|
|
& = & x(-\sin\theta\sin\delta+\cos\theta\cos\delta)+y(-\cos\theta\sin\delta-\sin\theta\cos\delta)\\
|
|
& = & x\cos\theta-y\sin\theta=x'\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial p'}{\partial p}=\frac{\partial(Rp)}{\partial p}=R\]
|
|
|
|
\end_inset
|
|
|
|
Similarly, unrotate
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial x'}{\partial\delta} & = & \frac{\partial(x\cos\theta'+y\sin\theta')}{\partial\delta}\\
|
|
& = & \frac{\partial(x(\cos\theta\cos\delta-\sin\theta\sin\delta)+y(\sin\theta\cos\delta+\cos\theta\sin\delta))}{\partial\delta}\\
|
|
& = & x(-\cos\theta\sin\delta-\sin\theta\cos\delta)+y(-\sin\theta\sin\delta+\cos\theta\cos\delta)\\
|
|
& = & -x\sin\theta+y\cos\theta=y'\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
\frac{\partial y'}{\partial\delta} & = & \frac{\partial(-x\sin\theta'+y\cos\theta')}{\partial\delta}\\
|
|
& = & \frac{\partial(-x(\sin\theta\cos\delta+\cos\theta\sin\delta)+y(\cos\theta\cos\delta-\sin\theta\sin\delta))}{\partial\delta}\\
|
|
& = & -x(-\sin\theta\sin\delta+\cos\theta\cos\delta)+y(-\cos\theta\sin\delta-\sin\theta\cos\delta)\\
|
|
& = & -x\cos\theta-y\sin\theta=-x'\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial p'}{\partial p}=\frac{\partial(Rp)}{\partial p}=R\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Point3
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A cross product
|
|
\begin_inset Formula $a\times b$
|
|
\end_inset
|
|
|
|
can be written as a matrix multiplication
|
|
\begin_inset Formula \[
|
|
a\times b=\Skew ab\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Skew a$
|
|
\end_inset
|
|
|
|
is a skew-symmetric matrix defined as
|
|
\begin_inset Formula \[
|
|
\Skew{x,y,z}=\left[\begin{array}{ccc}
|
|
0 & -z & y\\
|
|
z & 0 & -x\\
|
|
-y & x & 0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
We also have
|
|
\begin_inset Formula \[
|
|
a^{T}\Skew b=-(\Skew ba)^{T}=-(a\times b)^{T}\]
|
|
|
|
\end_inset
|
|
|
|
The derivative of a cross product
|
|
\begin_inset Formula \begin{equation}
|
|
\frac{\partial(a\times b)}{\partial a}=\Skew{-b}\label{eq:Dcross1}\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \begin{equation}
|
|
\frac{\partial(a\times b)}{\partial b}=\Skew a\label{eq:Dcross2}\end{equation}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Rot3
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
An incremental rotation is applied as (switched to right-multiply Jan 25
|
|
2010)
|
|
\begin_inset Formula \[
|
|
R'=R(I+\Omega)\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\Omega=\Skew{\omega}$
|
|
\end_inset
|
|
|
|
is the skew symmetric matrix corresponding to the incremental rotation
|
|
angles
|
|
\begin_inset Formula $\omega=(\omega_{x},\omega_{y},\omega_{z})$
|
|
\end_inset
|
|
|
|
.
|
|
The derivatives of
|
|
\series bold
|
|
\emph on
|
|
rotate
|
|
\series default
|
|
\emph default
|
|
are then found easily, using
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:Dcross1"
|
|
|
|
\end_inset
|
|
|
|
:
|
|
\begin_inset Formula \[
|
|
\frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=\frac{\partial(R\left(\omega\times x\right))}{\partial\omega}=R\frac{\partial\left(\omega\times x\right)}{\partial\omega}=R\Skew{-x}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\frac{\partial(Rx)}{\partial x}=R\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Pose3 (gtsam, old-style exmap)
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In the old-style, we have
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
\begin_inset Formula $R'=R(I+\Omega)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula $t'=t+dt$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In this case, the derivative of
|
|
\series bold
|
|
\emph on
|
|
transform_from
|
|
\series default
|
|
\emph default
|
|
,
|
|
\begin_inset Formula $Rx+t$
|
|
\end_inset
|
|
|
|
:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial(R(I+\Omega)x+t)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=\frac{\partial(R\left(\omega\times x\right))}{\partial\omega}=R\Skew{-x}\]
|
|
|
|
\end_inset
|
|
|
|
and with respect to
|
|
\begin_inset Formula $dt$
|
|
\end_inset
|
|
|
|
is easy:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial(Rx+t+dt)}{\partial dt}=I\]
|
|
|
|
\end_inset
|
|
|
|
The derivative of
|
|
\series bold
|
|
\emph on
|
|
transform_to
|
|
\series default
|
|
\emph default
|
|
,
|
|
\begin_inset Formula $inv(R)(x-t)$
|
|
\end_inset
|
|
|
|
we can obtain using the chain rule:
|
|
\begin_inset Formula \[
|
|
\frac{\partial(inv(R)(x-t))}{\partial\omega}=\frac{\partial unrot(R,(x-t))}{\partial\omega}=skew(R^{T}\left(x-t\right))\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
and with respect to
|
|
\begin_inset Formula $dt$
|
|
\end_inset
|
|
|
|
is easy:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial(R^{T}(x-t-dt))}{\partial dt}=-R^{T}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Pose3 (gtsam, new-style exmap)
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In the new-style exponential map, Pose3 is composed with a delta pose as
|
|
follows
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
\begin_inset Formula $R'=(I+\Omega)R$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula $t'=(I+\Omega)t+dt$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The derivative of transform_from,
|
|
\begin_inset Formula $Rx+t$
|
|
\end_inset
|
|
|
|
:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial((I+\Omega)Rx+(I+\Omega)t)}{\partial\omega}=\frac{\partial(\Omega(Rx+t))}{\partial\omega}=\frac{\partial(\omega\times(Rx+t))}{\partial\omega}=-\Skew{Rx+t}\]
|
|
|
|
\end_inset
|
|
|
|
and with respect to
|
|
\begin_inset Formula $dt$
|
|
\end_inset
|
|
|
|
is easy:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial(Rx+t+dt)}{\partial dt}=I\]
|
|
|
|
\end_inset
|
|
|
|
The derivative of transform_to,
|
|
\begin_inset Formula $R^{T}(x-t)$
|
|
\end_inset
|
|
|
|
, eludes me.
|
|
The calculation below is just an attempt:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Noting that
|
|
\family roman
|
|
\series medium
|
|
\shape up
|
|
\size normal
|
|
\emph off
|
|
\bar no
|
|
\noun off
|
|
\color none
|
|
|
|
\begin_inset Formula $R'^{T}=R^{T}(I-\Omega)$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $(I-\Omega)(x-(I+\Omega)t)=(I-\Omega)(x-t-\Omega t)=x-t-dt-\Omega x+\Omega^{2}t$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial(R'^{T}(x-t'))}{\partial\omega}=\frac{\partial(R^{T}(I-\Omega)(x-(I+\Omega)t))}{\partial\omega}=-\frac{\partial(R^{T}(\Omega(x-\Omega t)))}{\partial\omega}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
-\frac{\partial(\Skew{R^{T}\omega}R^{T}x)}{\partial\omega}=\Skew{R^{T}x}\frac{\partial(R^{T}\omega)}{\partial\omega}=\Skew{R^{T}x}R^{T}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
=\frac{\partial(R^{T}\Omega^{2}t)}{\partial\omega}+\Skew{R^{T}x}R^{T}\]
|
|
|
|
\end_inset
|
|
|
|
and with respect to
|
|
\begin_inset Formula $dt$
|
|
\end_inset
|
|
|
|
is easy:
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
\frac{\partial(R^{T}(x-t-dt))}{\partial dt}=-R^{T}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Line3vd
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
One representation of a line is through 2 vectors
|
|
\begin_inset Formula $(v,d)$
|
|
\end_inset
|
|
|
|
, where
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
is the direction and the vector
|
|
\begin_inset Formula $d$
|
|
\end_inset
|
|
|
|
points from the orgin to the closest point on the line.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
In this representation, transforming a 3D line from a world coordinate frame
|
|
to a camera at
|
|
\begin_inset Formula $(R_{w}^{c},t^{w})$
|
|
\end_inset
|
|
|
|
is done by
|
|
\begin_inset Formula \[
|
|
v^{c}=R_{w}^{c}v^{w}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right)\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Line3
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
For 3D lines, we use a parameterization due to C.J.
|
|
Taylor, using a rotation matrix
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
and 2 scalars
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
.
|
|
The line direction
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
is simply the Z-axis of the rotated frame, i.e.,
|
|
\begin_inset Formula $v=R_{3}$
|
|
\end_inset
|
|
|
|
, while the vector
|
|
\begin_inset Formula $d$
|
|
\end_inset
|
|
|
|
is given by
|
|
\begin_inset Formula $d=aR_{1}+bR_{2}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Now, we will
|
|
\emph on
|
|
not
|
|
\emph default
|
|
use the incremental rotation scheme we used for rotations: because the
|
|
matrix R translates from the line coordinate frame to the world frame,
|
|
we need to apply the incremental rotation on the right-side:
|
|
\begin_inset Formula \[
|
|
R'=R(I+\Omega)\]
|
|
|
|
\end_inset
|
|
|
|
Projecting a line to 2D can be done easily, as both
|
|
\begin_inset Formula $v$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $d$
|
|
\end_inset
|
|
|
|
are also the 2D homogenous coordinates of two points on the projected line,
|
|
and hence we have
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
l & = & v\times d\\
|
|
& = & R_{3}\times\left(aR_{1}+bR_{2}\right)\\
|
|
& = & a\left(R_{3}\times R_{1}\right)+b\left(R_{3}\times R_{2}\right)\\
|
|
& = & aR_{2}-bR_{1}\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
This can be written as a rotation of a point,
|
|
\begin_inset Formula \[
|
|
l=R\left(\begin{array}{c}
|
|
-b\\
|
|
a\\
|
|
0\end{array}\right)\]
|
|
|
|
\end_inset
|
|
|
|
but because the incremental rotation is now done on the right, we need to
|
|
figure out the derivatives again:
|
|
\begin_inset Formula \begin{equation}
|
|
\frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=R\frac{\partial(\Omega x)}{\partial\omega}=R\Skew{-x}\label{eq:rotateRight}\end{equation}
|
|
|
|
\end_inset
|
|
|
|
and hence the derivative of the projection
|
|
\begin_inset Formula $l$
|
|
\end_inset
|
|
|
|
with respect to the rotation matrix
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
of the 3D line is
|
|
\begin_inset Formula \begin{equation}
|
|
\frac{\partial(l)}{\partial\omega}=R\Skew{\left(\begin{array}{c}
|
|
b\\
|
|
-a\\
|
|
0\end{array}\right)}=\left[\begin{array}{ccc}
|
|
aR_{3} & bR_{3} & -(aR_{1}+bR_{2})\end{array}\right]\end{equation}
|
|
|
|
\end_inset
|
|
|
|
or the
|
|
\begin_inset Formula $a,b$
|
|
\end_inset
|
|
|
|
scalars:
|
|
\begin_inset Formula \[
|
|
\frac{\partial(l)}{\partial a}=R_{2}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
\frac{\partial(l)}{\partial b}=-R_{1}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Transforming a 3D line
|
|
\begin_inset Formula $(R,(a,b))$
|
|
\end_inset
|
|
|
|
from a world coordinate frame to a camera frame
|
|
\begin_inset Formula $(R_{w}^{c},t^{w})$
|
|
\end_inset
|
|
|
|
is done by
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Formula \[
|
|
R'=R_{w}^{c}R\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
a'=a-R_{1}^{T}t^{w}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
b'=b-R_{2}^{T}t^{w}\]
|
|
|
|
\end_inset
|
|
|
|
Again, we need to redo the derivatives, as R is incremented from the right.
|
|
The first argument is incremented from the left, but the result is incremented
|
|
on the right:
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\
|
|
I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
|
|
\Omega' & = & R'^{T}\Skew{S\omega}R'\\
|
|
\Omega' & = & \Skew{R'^{T}S\omega}\\
|
|
\omega' & = & R'^{T}S\omega\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
For the second argument
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
we now simply have:
|
|
\begin_inset Formula \begin{eqnarray*}
|
|
AB(I+\Omega') & = & AB(I+\Omega)\\
|
|
\Omega' & = & \Omega\\
|
|
\omega' & = & \omega\end{eqnarray*}
|
|
|
|
\end_inset
|
|
|
|
The scalar derivatives can be found by realizing that
|
|
\begin_inset Formula \[
|
|
\left(\begin{array}{c}
|
|
a'\\
|
|
b'\\
|
|
...\end{array}\right)=\left(\begin{array}{c}
|
|
a\\
|
|
b\\
|
|
0\end{array}\right)-R^{T}t^{w}\]
|
|
|
|
\end_inset
|
|
|
|
where we don't care about the third row.
|
|
Hence
|
|
\begin_inset Formula \[
|
|
\frac{\partial(\left(R(I+\Omega_{2})\right)^{T}t^{w})}{\partial\omega}=-\frac{\partial(\Omega_{2}R^{T}t^{w})}{\partial\omega}=-\Skew{R^{T}t^{w}}=\left[\begin{array}{ccc}
|
|
0 & R_{3}^{T}t^{w} & -R_{2}^{T}t^{w}\\
|
|
-R_{3}^{T}t^{w} & 0 & R_{1}^{T}t^{w}\\
|
|
... & ... & 0\end{array}\right]\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
2D Line Segments
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The error between an infinite line
|
|
\begin_inset Formula $(a,b,c)$
|
|
\end_inset
|
|
|
|
and a 2D line segment
|
|
\begin_inset Formula $((x1,y1),(x2,y2))$
|
|
\end_inset
|
|
|
|
is defined in Line3.ml.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
|
|
\series bold
|
|
Recovering Pose
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
Below is the explanaition underlying Pose3.align, i.e.
|
|
aligning two point clouds using SVD.
|
|
Inspired but modified from CVOnline...
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
|
|
\emph on
|
|
Our
|
|
\emph default
|
|
model is
|
|
\begin_inset Formula \[
|
|
p^{c}=R\left(p^{w}-t\right)\]
|
|
|
|
\end_inset
|
|
|
|
i.e.,
|
|
\begin_inset Formula $R$
|
|
\end_inset
|
|
|
|
is from camera to world, and
|
|
\begin_inset Formula $t$
|
|
\end_inset
|
|
|
|
is the camera location in world coordinates.
|
|
The objective function is
|
|
\begin_inset Formula \begin{equation}
|
|
\frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}+Rt\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}-t'\right)^{2}\label{eq:J}\end{equation}
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $t'=-Rt$
|
|
\end_inset
|
|
|
|
is the location of the origin in the camera frame.
|
|
Taking the derivative with respect to
|
|
\begin_inset Formula $t'$
|
|
\end_inset
|
|
|
|
and setting to zero we have
|
|
\begin_inset Formula \[
|
|
\sum\left(p^{c}-Rp^{w}-t'\right)=0\]
|
|
|
|
\end_inset
|
|
|
|
or
|
|
\begin_inset Formula \begin{equation}
|
|
t'=\frac{1}{n}\sum\left(p^{c}-Rp^{w}\right)=\bar{p}^{c}-R\bar{p}^{w}\label{eq:t}\end{equation}
|
|
|
|
\end_inset
|
|
|
|
here
|
|
\begin_inset Formula $\bar{p}^{c}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $\bar{p}^{w}$
|
|
\end_inset
|
|
|
|
are the point cloud centroids.
|
|
Substituting back into
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:J"
|
|
|
|
\end_inset
|
|
|
|
, we get
|
|
\begin_inset Formula \[
|
|
\frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(\left(p^{c}-\bar{p}^{c}\right)-R\left(p^{w}-\bar{p}^{w}\right)\right)^{2}=\frac{1}{2}\sum\left(\hat{p}^{c}-R\hat{p}^{w}\right)^{2}\]
|
|
|
|
\end_inset
|
|
|
|
Now, to minimize the above it suffices to maximize (see CVOnline)
|
|
\begin_inset Formula \[
|
|
\mathop{trace}\left(R^{T}C\right)\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $C=\sum\hat{p}^{c}\left(\hat{p}^{w}\right)^{T}$
|
|
\end_inset
|
|
|
|
is the correlation matrix.
|
|
Intuitively, the cloud of points is rotated to align with the principal
|
|
axes.
|
|
This can be achieved by SVD decomposition on
|
|
\begin_inset Formula $C$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula \[
|
|
C=USV^{T}\]
|
|
|
|
\end_inset
|
|
|
|
and setting
|
|
\begin_inset Formula \[
|
|
R=UV^{T}\]
|
|
|
|
\end_inset
|
|
|
|
Clearly, from
|
|
\begin_inset CommandInset ref
|
|
LatexCommand eqref
|
|
reference "eq:t"
|
|
|
|
\end_inset
|
|
|
|
we then also recover the optimal
|
|
\begin_inset Formula $t$
|
|
\end_inset
|
|
|
|
as
|
|
\begin_inset Formula \[
|
|
t=\bar{p}^{w}-R^{T}\bar{p}^{c}\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset CommandInset bibtex
|
|
LatexCommand bibtex
|
|
bibfiles "/Users/dellaert/papers/refs"
|
|
options "plain"
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_body
|
|
\end_document
|