gtsam/doc/gtsam-coordinate-frames.lyx

#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