2519 lines
41 KiB
Plaintext
2519 lines
41 KiB
Plaintext
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
|
\lyxformat 413
|
|
\begin_document
|
|
\begin_header
|
|
\textclass article
|
|
\use_default_options true
|
|
\maintain_unincluded_children false
|
|
\language english
|
|
\language_package default
|
|
\inputencoding auto
|
|
\fontencoding global
|
|
\font_roman times
|
|
\font_sans helvet
|
|
\font_typewriter lmtt
|
|
\font_default_family default
|
|
\use_non_tex_fonts false
|
|
\font_sc false
|
|
\font_osf false
|
|
\font_sf_scale 100
|
|
\font_tt_scale 100
|
|
|
|
\graphics default
|
|
\default_output_format default
|
|
\output_sync 0
|
|
\bibtex_command default
|
|
\index_command default
|
|
\paperfontsize default
|
|
\spacing single
|
|
\use_hyperref false
|
|
\papersize default
|
|
\use_geometry false
|
|
\use_amsmath 1
|
|
\use_esint 1
|
|
\use_mhchem 1
|
|
\use_mathdots 1
|
|
\cite_engine basic
|
|
\use_bibtopic false
|
|
\use_indices false
|
|
\paperorientation portrait
|
|
\suppress_date false
|
|
\use_refstyle 1
|
|
\index Index
|
|
\shortcut idx
|
|
\color #008000
|
|
\end_index
|
|
\secnumdepth 3
|
|
\tocdepth 3
|
|
\paragraph_separation indent
|
|
\paragraph_indentation default
|
|
\quotes_language english
|
|
\papercolumns 1
|
|
\papersides 1
|
|
\paperpagestyle default
|
|
\tracking_changes false
|
|
\output_changes false
|
|
\html_math_output 0
|
|
\html_css_as_file 0
|
|
\html_be_strict false
|
|
\end_header
|
|
|
|
\begin_body
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\SE}[1]{\mathbb{SE}\left(#1\right)}
|
|
{\mathbb{SE}\left(#1\right)}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\se}[1]{\mathfrak{se}\left(#1\right)}
|
|
{\mathfrak{se}\left(#1\right)}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\R}[1]{\mathbb{R}^{#1}}
|
|
{\mathbb{R}^{#1}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\norm}[1]{\left\Vert #1\right\Vert }
|
|
{\left\Vert #1\right\Vert }
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\t}{\mathsf{T}}
|
|
{\mathsf{T}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset FormulaMacro
|
|
\newcommand{\lin}[1]{\overset{{\scriptscriptstyle \circ}}{#1}}
|
|
{\overset{{\scriptscriptstyle \circ}}{#1}}
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Introduction
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
This document describes the coordinate frame conventions in which GTSAM
|
|
inputs and represents states and uncertainties.
|
|
When specifying initial conditions, measurements and their uncertainties,
|
|
and interpreting estimated uncertainties and the results of geometry operations
|
|
, the coordinate frame convention comes into play.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
GTSAM as consistently as possible represents all states and uncertainties
|
|
in the body frame.
|
|
In cases where several frames are used simultaneously, a good rule of thumb
|
|
is that measurements and uncertainties will be represented in the
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
last
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
frame of the series.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Frame Conventions in Geometry, Lie Group, and Manifold Operations
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Float table
|
|
wide false
|
|
sideways false
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
\align center
|
|
|
|
\size footnotesize
|
|
\begin_inset Tabular
|
|
<lyxtabular version="3" rows="10" columns="4">
|
|
<features booktabs="true" tabularvalignment="middle">
|
|
<column alignment="left" valignment="top" width="4cm">
|
|
<column alignment="left" valignment="top" width="1.25cm">
|
|
<column alignment="left" valignment="top" width="1.75cm">
|
|
<column alignment="left" valignment="top" width="3.5cm">
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Syntax
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Input
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Output
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Identities
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell multicolumn="1" alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset space \quad{}
|
|
\end_inset
|
|
|
|
|
|
\series bold
|
|
Lie group operations
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell multicolumn="2" alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell multicolumn="2" alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell multicolumn="2" alignment="center" valignment="top" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $c=a.\mathbf{compose}\left(b\right)$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $c=a\mathbf{*}b$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $c=b$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $c=ab$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $b=a.\mathbf{inverse}()$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $b=g$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $b=a^{-1}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a.\mathbf{compose}($
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\quad a.\mathbf{inverse}())==\mathbf{I}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $c=a.\mathbf{between}\left(b\right)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $c=b$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $c=a^{-1}b$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a.\mathbf{inverse}().$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\quad\mathbf{compose}(b)==c$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $\delta=a.\mathbf{logmap}()$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\hat{\delta}=\log a$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $\mathrm{X}::\mathbf{Expmap}(\delta)==a$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a=\mathrm{X}::\mathbf{Expmap}(\delta)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $a=\exp\hat{\delta}$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a.\mathbf{logmap}()==\delta$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell multicolumn="1" alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
\begin_inset space \quad{}
|
|
\end_inset
|
|
|
|
Lie group actions
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell multicolumn="2" alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell multicolumn="2" alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell multicolumn="2" alignment="center" valignment="top" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $q=a.\mathbf{transform\_to}\left(p\right)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $q=p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $q=a^{-1}p$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $q=a.\mathbf{transform\_from}\left(p\right)$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $q=a\mathbf{*}p$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $q=p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $q=ap$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
</lyxtabular>
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Caption
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "tab:Coordinate-frame-transformations"
|
|
|
|
\end_inset
|
|
|
|
Coordinate frame transformations performed by GTSAM geometry operations.
|
|
Here,
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $c$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
are Lie group elements (Pose2, Pose3, Rot2, Rot3, Point2, Point3,
|
|
\emph on
|
|
etc
|
|
\emph default
|
|
).
|
|
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
is a set of Lie algebra coordinates (i.e.
|
|
linear update, linear delta, tangent space coordinates), and
|
|
\begin_inset Formula $\mathrm{X}$
|
|
\end_inset
|
|
|
|
is a Lie group type (e.g.
|
|
Pose2).
|
|
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $q$
|
|
\end_inset
|
|
|
|
are the objects of Lie group actions (Point2, Point3,
|
|
\emph on
|
|
etc
|
|
\emph default
|
|
).
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
At the core of most coordinate frame usage in GTSAM are geometry and Lie
|
|
group operations.
|
|
We explain the geometry and Lie group operations in GTSAM in terms of the
|
|
coordinate frame transformations they perform, detailed in Table
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "tab:Coordinate-frame-transformations"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Float table
|
|
wide false
|
|
sideways false
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
\align center
|
|
|
|
\size footnotesize
|
|
\begin_inset Tabular
|
|
<lyxtabular version="3" rows="3" columns="4">
|
|
<features booktabs="true" tabularvalignment="middle">
|
|
<column alignment="left" valignment="top" width="4.5cm">
|
|
<column alignment="left" valignment="top" width="1cm">
|
|
<column alignment="left" valignment="top" width="1.25cm">
|
|
<column alignment="left" valignment="top" width="3.75cm">
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Syntax
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Input
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Output
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
Identities
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $\delta=a.\mathbf{localCoordinates}\left(b\right)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a.\mathbf{retract}\left(\delta\right)==b$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\mathbf{I}.\mathbf{localCoordinates}($
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\quad a.\mathbf{between}\left(b\right))==\delta$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $b=a.\mathbf{retract}\left(\delta\right)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" bottomline="true" leftline="true" rightline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $a.\mathbf{compose}($
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\quad\mathbf{I}.\mathbf{retract}\left(\delta\right))==b$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
</lyxtabular>
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset Caption
|
|
|
|
\begin_layout Plain Layout
|
|
\begin_inset CommandInset label
|
|
LatexCommand label
|
|
name "tab:Coordinate-frames-manifold"
|
|
|
|
\end_inset
|
|
|
|
Coordinate frames for manifold tangent space operations.
|
|
Here,
|
|
\begin_inset Formula $a$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula $b$
|
|
\end_inset
|
|
|
|
, and
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
are manifold elements,
|
|
\begin_inset Formula $\delta$
|
|
\end_inset
|
|
|
|
is a tangent space element, and
|
|
\begin_inset Formula $\mathrm{X}$
|
|
\end_inset
|
|
|
|
is a Lie group type (e.g.
|
|
Pose2).
|
|
For the identities column, we assume the elements are also Lie group elements
|
|
with identity
|
|
\begin_inset Formula $\mathbf{I}$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
The manifold tangent space operations
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
retract
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
local coordinates
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
also work in the body frame for Lie group elements.
|
|
The tangent space coordinates given to
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
retract
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
should be in the body frame, not the global frame.
|
|
Similarly, the tangent space coordinates returned by
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
local coordinates
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
will be in the same body frame.
|
|
This is detailed in Table
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "tab:Coordinate-frames-manifold"
|
|
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Frame and Uncertainty Conventions For Built-in Factors
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
\begin_inset Float table
|
|
wide false
|
|
sideways false
|
|
status open
|
|
|
|
\begin_layout Plain Layout
|
|
\align center
|
|
|
|
\size footnotesize
|
|
\begin_inset Tabular
|
|
<lyxtabular version="3" rows="7" columns="5">
|
|
<features tabularvalignment="middle">
|
|
<column alignment="left" valignment="top" width="2.5cm">
|
|
<column alignment="left" valignment="top" width="2.75cm">
|
|
<column alignment="left" valignment="top" width="1.25cm">
|
|
<column alignment="left" valignment="top" width="1.75cm">
|
|
<column alignment="left" valignment="top" width="1.75cm">
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Name
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Residual
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Variables
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Measurement
|
|
\end_layout
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
(
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
)
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Measurement Uncertainty
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
PriorFactor
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $z.\mathrm{localCoordinates}\left(x\right)$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Ideal
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
In
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
/ In
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
BetweenFactor
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $z.\mathrm{localCoordinates}($
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\; x.\mathrm{between}\left(y\right))$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Ideal
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
In
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
/ In
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
RangeFactor
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x.\mathrm{range}\left(y\right)-z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="center" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Euclidean distance
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
In
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
BearingFactor
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $z.\mathrm{localCoordinates}($
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $\; x.\mathrm{bearing}\left(y\right))$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="center" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Bearing of
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
position in frame
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
In
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
GenericProjection
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
Factor
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $K^{-1}\left(P\left(x^{-1}p\right)\right)-z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="center" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Perspective projection of
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
In
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
<row>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\series bold
|
|
\size footnotesize
|
|
GeneralSFM
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
Factor
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $K^{-1}\left(P\left(x^{-1}p\right)\right)-z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="center" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $g$
|
|
\end_inset
|
|
|
|
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
Parameters
|
|
\begin_inset Newline newline
|
|
\end_inset
|
|
|
|
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
of
|
|
\begin_inset Formula $K$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
Perspective projection of
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
<cell alignment="left" valignment="top" topline="true" bottomline="true" usebox="none">
|
|
\begin_inset Text
|
|
|
|
\begin_layout Plain Layout
|
|
|
|
\size footnotesize
|
|
In
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
</cell>
|
|
</row>
|
|
</lyxtabular>
|
|
|
|
\end_inset
|
|
|
|
|
|
\size default
|
|
|
|
\begin_inset Caption
|
|
|
|
\begin_layout Plain Layout
|
|
Measurement functions and coordinate frames of factors provided with GTSAM.
|
|
To simplify notation,
|
|
\begin_inset Formula $K$
|
|
\end_inset
|
|
|
|
is a camera calibration function converting pixels to normalized image
|
|
coordinates, and
|
|
\begin_inset Formula $P$
|
|
\end_inset
|
|
|
|
is the pinhole projection function.
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_inset
|
|
|
|
All built-in GTSAM factors follow a consistent coordinate frame convention
|
|
(though fundamentally how a measurement and its uncertainty are specified
|
|
depends on the measurement model described by a factor).
|
|
In all built-in GTSAM factors, the
|
|
\emph on
|
|
noise model
|
|
\emph default
|
|
, i.e.
|
|
the measurement uncertainty, should be specified in the coordinate frame
|
|
of the measurement itself.
|
|
This is part of a convention in GTSAM that tangent-space quantities (like
|
|
Gaussian noise models and update delta vectors) are always in the coordinate
|
|
frame of the element owning the tangent space.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
PriorFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A PriorFactor is a simple unary prior.
|
|
It encodes a direct measurement of the value of a variable
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
, with the specified mean
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
and uncertainty, such that
|
|
\begin_inset Formula $z.\mathrm{between}\left(x\right)$
|
|
\end_inset
|
|
|
|
is distributed according to the specified noise model.
|
|
From this definition and the definition of
|
|
\series bold
|
|
between
|
|
\series default
|
|
in Table
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "tab:Coordinate-frame-transformations"
|
|
|
|
\end_inset
|
|
|
|
, the measurement itself should be specified in the frame with respect to
|
|
which
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
is specified, while the uncertainty is specified in the coordinate frame
|
|
of the measurement, or equivalently, in frame
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
BetweenFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A BetweenFactor is a measurement on the relative transformation between
|
|
two variables.
|
|
A BetweenFactor on variables
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
with measurement
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
implies that
|
|
\begin_inset Formula $z.\mathrm{between}\left(x.\mathrm{between}\left(y\right)\right)$
|
|
\end_inset
|
|
|
|
is distributed according to the specified noise model.
|
|
This definition, along with that of
|
|
\series bold
|
|
between
|
|
\series default
|
|
in Table
|
|
\begin_inset space ~
|
|
\end_inset
|
|
|
|
|
|
\begin_inset CommandInset ref
|
|
LatexCommand ref
|
|
reference "tab:Coordinate-frame-transformations"
|
|
|
|
\end_inset
|
|
|
|
, implies that the measurement is in frame
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
, i.e.
|
|
it measures
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
in
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
, and that the uncertainty is in the frame of the measurement, or equivalently,
|
|
in frame
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
RangeFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A RangeFactor measures the Euclidean distance either between two poses,
|
|
a pose and a point, or two points.
|
|
The range is a scalar, specified to be distributed according to the specified
|
|
noise model.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
BearingFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A BearingFactor measures the bearing (angle) of the
|
|
\emph on
|
|
position
|
|
\emph default
|
|
of a pose or point
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
as observed from a pose
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
.
|
|
The orientation of
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
affects the measurement prediction.
|
|
Though, if
|
|
\begin_inset Formula $y$
|
|
\end_inset
|
|
|
|
is a pose, it's orientation does not matter.
|
|
The noise model specifies the distribution of the bearing, in radians.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
GenericProjectionFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A GenericProjectionFactor measures the pixel coordinates of a landmark
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
projected into a camera
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
with the calibration function
|
|
\begin_inset Formula $K$
|
|
\end_inset
|
|
|
|
that converts pixels to normalized image coordinates.
|
|
The measurement
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
is specified in real pixel coordinates (thanks to the
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
uncalibration
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
function
|
|
\begin_inset Formula $K^{-1}$
|
|
\end_inset
|
|
|
|
used in the residual).
|
|
In a GenericProjectionFactor, the calibration is fixed.
|
|
On the other hand, GeneralSFMFactor allows the calibration parameters to
|
|
be optimized as variables.
|
|
\end_layout
|
|
|
|
\begin_layout Subsection
|
|
GeneralSFMFactor
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
A GeneralSFMFactor is the same as a GenericProjectionFactor except that
|
|
a GeneralSFMFactor also allows the parameters of the calibration function
|
|
|
|
\begin_inset Formula $K$
|
|
\end_inset
|
|
|
|
to be optimized as variables, instead of having them fixed.
|
|
A GeneralSFMFactor measures the pixel coordinates of a landmark
|
|
\begin_inset Formula $p$
|
|
\end_inset
|
|
|
|
projected into a camera
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
with the calibration function
|
|
\begin_inset Formula $K$
|
|
\end_inset
|
|
|
|
that converts pixels to normalized image coordinates.
|
|
The measurement
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
is specified in real pixel coordinates (thanks to the
|
|
\begin_inset Quotes eld
|
|
\end_inset
|
|
|
|
uncalibration
|
|
\begin_inset Quotes erd
|
|
\end_inset
|
|
|
|
function
|
|
\begin_inset Formula $K^{-1}$
|
|
\end_inset
|
|
|
|
used in the residual).
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Noise models of prior factors
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The simplest way to describe noise models is by an example.
|
|
Let's take a prior factor on a 3D pose
|
|
\begin_inset Formula $x\in\SE 3$
|
|
\end_inset
|
|
|
|
,
|
|
\family typewriter
|
|
Pose3
|
|
\family default
|
|
in GTSAM.
|
|
Let
|
|
\begin_inset Formula $z\in\SE 3$
|
|
\end_inset
|
|
|
|
be the expected pose, i.e.
|
|
the zero-error solution for the prior factor.
|
|
The
|
|
\emph on
|
|
unwhitened error
|
|
\emph default
|
|
(the error vector not accounting for the noise model) is
|
|
\begin_inset Formula
|
|
\[
|
|
h\left(x\right)=\log\left(z^{-1}x\right)\text{,}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $\cdot^{-1}$
|
|
\end_inset
|
|
|
|
is the Lie group inverse and
|
|
\begin_inset Formula $\log\cdot$
|
|
\end_inset
|
|
|
|
is the logarithm map on
|
|
\begin_inset Formula $\SE 3$
|
|
\end_inset
|
|
|
|
.
|
|
The full factor error, including the noise model, is
|
|
\begin_inset Formula
|
|
\[
|
|
e\left(x\right)=\norm{h\left(x\right)}_{\Sigma}^{2}=h\left(x\right)^{\t}\Sigma^{-1}h\left(x\right)\text{.}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
[ Skipping details of the derivation for now, for lack of time to get a
|
|
useful answer out quickly ]
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The density induced by a noise model on the prior factor is Gaussian in
|
|
the tangent space about the linearization point.
|
|
Suppose that the pose is linearized at
|
|
\begin_inset Formula $\lin x\in\SE 3$
|
|
\end_inset
|
|
|
|
, which we assume is near to
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
.
|
|
Let
|
|
\begin_inset Formula $\delta x\in\R 6$
|
|
\end_inset
|
|
|
|
be an update vector in local coordinates (a twist).
|
|
Then, the factor error in terms of the update vector
|
|
\begin_inset Formula $\delta x$
|
|
\end_inset
|
|
|
|
is
|
|
\begin_inset Formula
|
|
\[
|
|
e\left(\delta x\right)=\norm{h\left(\lin x\exp\delta x\right)}_{\Sigma}^{2}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
We can see why the covariance
|
|
\begin_inset Formula $\Sigma$
|
|
\end_inset
|
|
|
|
is in the body frame of
|
|
\begin_inset Formula $x$
|
|
\end_inset
|
|
|
|
by looking at the linearized error function,
|
|
\begin_inset Formula
|
|
\begin{align*}
|
|
e\left(\delta x\right) & \approx\norm{\log\left(z^{-1}\lin x\exp\delta x\right)}_{\Sigma}^{2}\\
|
|
& \approx\norm{\log\left(z^{-1}\lin x\right)+\delta x}_{\Sigma}^{2}
|
|
\end{align*}
|
|
|
|
\end_inset
|
|
|
|
Here we see that the update
|
|
\begin_inset Formula $\exp\delta x$
|
|
\end_inset
|
|
|
|
from the linear step
|
|
\begin_inset Formula $\delta x$
|
|
\end_inset
|
|
|
|
is applied in the body frame of
|
|
\begin_inset Formula $\lin x$
|
|
\end_inset
|
|
|
|
, because of the ordering
|
|
\begin_inset Formula $\lin x\exp\delta x$
|
|
\end_inset
|
|
|
|
.
|
|
Furthermore,
|
|
\begin_inset Formula $z^{-1}\lin x$
|
|
\end_inset
|
|
|
|
is a constant term, so we can also see that the covariance
|
|
\begin_inset Formula $\Sigma$
|
|
\end_inset
|
|
|
|
is actually applied to the linear update vector
|
|
\begin_inset Formula $\delta x$
|
|
\end_inset
|
|
|
|
.
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
This means that to draw random pose samples, we actually draw random samples
|
|
of
|
|
\begin_inset Formula $\delta x$
|
|
\end_inset
|
|
|
|
with zero mean and covariance
|
|
\begin_inset Formula $\Sigma$
|
|
\end_inset
|
|
|
|
, i.e.
|
|
\begin_inset Formula
|
|
\[
|
|
\delta x\sim\mathcal{N}\left(0,\:\Sigma\right)\text{.}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\begin_layout Section
|
|
Noise models of between factors
|
|
\end_layout
|
|
|
|
\begin_layout Standard
|
|
The noise model of a BetweenFactor is a bit more complicated.
|
|
The unwhitened error is
|
|
\begin_inset Formula
|
|
\[
|
|
h\left(x_{1},x_{2}\right)=\log\left(z^{-1}x_{1}^{-1}x_{2}\right)\text{,}
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
where
|
|
\begin_inset Formula $z$
|
|
\end_inset
|
|
|
|
is the expected relative pose between
|
|
\begin_inset Formula $x_{1}$
|
|
\end_inset
|
|
|
|
and
|
|
\begin_inset Formula $x_{2}$
|
|
\end_inset
|
|
|
|
, i.e.
|
|
the factor has zero error when
|
|
\begin_inset Formula $x_{1}z=x_{2}$
|
|
\end_inset
|
|
|
|
.
|
|
If we consider the density on the second pose
|
|
\begin_inset Formula $x_{2}$
|
|
\end_inset
|
|
|
|
induced by holding the first pose
|
|
\begin_inset Formula $x_{1}$
|
|
\end_inset
|
|
|
|
fixed, we can see that the covariance is applied to the linear update in
|
|
the body frame of the second pose
|
|
\begin_inset Formula $x_{2}$
|
|
\end_inset
|
|
|
|
,
|
|
\begin_inset Formula
|
|
\[
|
|
e\left(\delta x_{2}\right)\approx\norm{\log\left(z^{-1}x_{1}^{-1}x_{2}\exp\delta x_{2}\right)}_{\Sigma}^{2}.
|
|
\]
|
|
|
|
\end_inset
|
|
|
|
If we hold the second pose fixed, the covariance is applied as follows (actually
|
|
, what frame is it in now??)
|
|
\begin_inset Formula
|
|
\begin{align*}
|
|
e\left(\delta x_{1}\right) & \approx\norm{\log\left(z^{-1}\left(x_{1}\exp\delta x_{1}\right)^{-1}x_{2}\right)}_{\Sigma}^{2}\\
|
|
& =\norm{\log\left(z^{-1}\exp-\delta x_{1}x_{1}^{-1}x_{2}\right)}_{\Sigma}^{2}
|
|
\end{align*}
|
|
|
|
\end_inset
|
|
|
|
|
|
\end_layout
|
|
|
|
\end_body
|
|
\end_document
|