Merged in feature/ImuFactorPush2 (pull request #200)
ImuFactor with NavState manifold pre-integrationrelease/4.3a0
Frank Dellaert
commit
6da59d73bd
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
|
@ -76,335 +76,10 @@ Frank Dellaert
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
\begin_inset Note Comment
|
\begin_inset CommandInset include
|
||||||
status open
|
LatexCommand include
|
||||||
|
filename "macros.lyx"
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
Derivatives
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
|
|
||||||
{\frac{\partial#1}{\partial#2}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
|
|
||||||
{#1\biggr\rvert_{#2}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
|
|
||||||
{\at{\deriv{#1}{#2}}{#3}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status open
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
Lie Groups
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\xhat}{\hat{x}}
|
|
||||||
{\hat{x}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\yhat}{\hat{y}}
|
|
||||||
{\hat{y}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Ad}[1]{Ad_{#1}}
|
|
||||||
{Ad_{#1}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
|
|
||||||
{\mathbf{\mathop{Ad}}{}_{#1}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\define}{\stackrel{\Delta}{=}}
|
|
||||||
{\stackrel{\Delta}{=}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\gg}{\mathfrak{g}}
|
|
||||||
{\mathfrak{g}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Rn}{\mathbb{R}^{n}}
|
|
||||||
{\mathbb{R}^{n}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status open
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
SO(2), 1
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
|
|
||||||
{\mathfrak{\mathbb{R}^{2}}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\SOtwo}{SO(2)}
|
|
||||||
{SO(2)}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\sotwo}{\mathfrak{so(2)}}
|
|
||||||
{\mathfrak{so(2)}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\that}{\hat{\theta}}
|
|
||||||
{\hat{\theta}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\skew}[1]{[#1]_{+}}
|
|
||||||
{[#1]_{+}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status open
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
SE(2), 3
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\SEtwo}{SE(2)}
|
|
||||||
{SE(2)}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\setwo}{\mathfrak{se(2)}}
|
|
||||||
{\mathfrak{se(2)}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Skew}[1]{[#1]_{\times}}
|
|
||||||
{[#1]_{\times}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status open
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
SO(3), 3
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}}
|
|
||||||
{\mathfrak{\mathbb{R}^{3}}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\SOthree}{SO(3)}
|
|
||||||
{SO(3)}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\sothree}{\mathfrak{so(3)}}
|
|
||||||
{\mathfrak{so(3)}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\what}{\hat{\omega}}
|
|
||||||
{\hat{\omega}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status open
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
SE(3),6
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}}
|
|
||||||
{\mathfrak{\mathbb{R}^{6}}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\SEthree}{SE(3)}
|
|
||||||
{SE(3)}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\sethree}{\mathfrak{se(3)}}
|
|
||||||
{\mathfrak{se(3)}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\xihat}{\hat{\xi}}
|
|
||||||
{\hat{\xi}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status open
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
Aff(2),6
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\Afftwo}{Aff(2)}
|
|
||||||
{Aff(2)}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\afftwo}{\mathfrak{aff(2)}}
|
|
||||||
{\mathfrak{aff(2)}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\aa}{a}
|
|
||||||
{a}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\ahat}{\hat{a}}
|
|
||||||
{\hat{a}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset Note Comment
|
|
||||||
status collapsed
|
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
|
||||||
SL(3),8
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\SLthree}{SL(3)}
|
|
||||||
{SL(3)}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\slthree}{\mathfrak{sl(3)}}
|
|
||||||
{\mathfrak{sl(3)}}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\hh}{h}
|
|
||||||
{h}
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
|
||||||
\newcommand{\hhat}{\hat{h}}
|
|
||||||
{\hat{h}}
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
137
doc/macros.lyx
137
doc/macros.lyx
|
|
@ -1,42 +1,60 @@
|
||||||
#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
|
#LyX 2.0 created this file. For more info see http://www.lyx.org/
|
||||||
\lyxformat 345
|
\lyxformat 413
|
||||||
\begin_document
|
\begin_document
|
||||||
\begin_header
|
\begin_header
|
||||||
\textclass article
|
\textclass article
|
||||||
\use_default_options true
|
\use_default_options true
|
||||||
|
\maintain_unincluded_children false
|
||||||
\language english
|
\language english
|
||||||
|
\language_package default
|
||||||
\inputencoding auto
|
\inputencoding auto
|
||||||
|
\fontencoding global
|
||||||
\font_roman default
|
\font_roman default
|
||||||
\font_sans default
|
\font_sans default
|
||||||
\font_typewriter default
|
\font_typewriter default
|
||||||
\font_default_family default
|
\font_default_family default
|
||||||
|
\use_non_tex_fonts false
|
||||||
\font_sc false
|
\font_sc false
|
||||||
\font_osf false
|
\font_osf false
|
||||||
\font_sf_scale 100
|
\font_sf_scale 100
|
||||||
\font_tt_scale 100
|
\font_tt_scale 100
|
||||||
|
|
||||||
\graphics default
|
\graphics default
|
||||||
|
\default_output_format default
|
||||||
|
\output_sync 0
|
||||||
|
\bibtex_command default
|
||||||
|
\index_command default
|
||||||
\paperfontsize default
|
\paperfontsize default
|
||||||
\use_hyperref false
|
\use_hyperref false
|
||||||
\papersize default
|
\papersize default
|
||||||
\use_geometry false
|
\use_geometry false
|
||||||
\use_amsmath 1
|
\use_amsmath 1
|
||||||
\use_esint 1
|
\use_esint 1
|
||||||
|
\use_mhchem 1
|
||||||
|
\use_mathdots 0
|
||||||
\cite_engine basic
|
\cite_engine basic
|
||||||
\use_bibtopic false
|
\use_bibtopic false
|
||||||
|
\use_indices false
|
||||||
\paperorientation portrait
|
\paperorientation portrait
|
||||||
|
\suppress_date false
|
||||||
|
\use_refstyle 0
|
||||||
|
\index Index
|
||||||
|
\shortcut idx
|
||||||
|
\color #008000
|
||||||
|
\end_index
|
||||||
\secnumdepth 3
|
\secnumdepth 3
|
||||||
\tocdepth 3
|
\tocdepth 3
|
||||||
\paragraph_separation indent
|
\paragraph_separation indent
|
||||||
\defskip medskip
|
\paragraph_indentation default
|
||||||
\quotes_language english
|
\quotes_language english
|
||||||
\papercolumns 1
|
\papercolumns 1
|
||||||
\papersides 1
|
\papersides 1
|
||||||
\paperpagestyle default
|
\paperpagestyle default
|
||||||
\tracking_changes false
|
\tracking_changes false
|
||||||
\output_changes false
|
\output_changes false
|
||||||
\author ""
|
\html_math_output 0
|
||||||
\author ""
|
\html_css_as_file 0
|
||||||
|
\html_be_strict false
|
||||||
\end_header
|
\end_header
|
||||||
|
|
||||||
\begin_body
|
\begin_body
|
||||||
|
|
@ -62,14 +80,14 @@ Derivatives
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
\begin_inset FormulaMacro
|
||||||
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
|
\newcommand{\at}[1]{#1\biggr\vert_{\#2}}
|
||||||
{#1\biggr\rvert_{#2}}
|
{#1\biggr\vert_{\#2}}
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
\begin_inset FormulaMacro
|
||||||
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
|
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
|
||||||
{\at{\deriv{#1}{#2}}{#3}}
|
{\at{\deriv{#1}{#2}}#3}
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -107,6 +125,15 @@ Lie Groups
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Standard
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
|
||||||
|
{\mathbf{\mathop{Ad}}{}_{#1}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
|
|
@ -144,6 +171,12 @@ SO(2)
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\Rone}{\mathfrak{\mathbb{R}}}
|
||||||
|
{\mathfrak{\mathbb{R}}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
\begin_inset FormulaMacro
|
||||||
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
|
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
|
||||||
{\mathfrak{\mathbb{R}^{2}}}
|
{\mathfrak{\mathbb{R}^{2}}}
|
||||||
|
|
@ -202,6 +235,12 @@ SE(2)
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\Skew}[1]{[#1]_{\times}}
|
||||||
|
{[#1]_{\times}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
|
|
@ -243,7 +282,7 @@ SO(3)
|
||||||
|
|
||||||
|
|
||||||
\begin_inset FormulaMacro
|
\begin_inset FormulaMacro
|
||||||
\newcommand{\Skew}[1]{[#1]_{\times}}
|
\renewcommand{\Skew}[1]{[#1]_{\times}}
|
||||||
{[#1]_{\times}}
|
{[#1]_{\times}}
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -288,6 +327,86 @@ SE(3)
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Standard
|
||||||
|
\begin_inset Note Comment
|
||||||
|
status open
|
||||||
|
|
||||||
|
\begin_layout Plain Layout
|
||||||
|
Aff(2),6
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Standard
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\Afftwo}{Aff(2)}
|
||||||
|
{Aff(2)}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\afftwo}{\mathfrak{aff(2)}}
|
||||||
|
{\mathfrak{aff(2)}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\aa}{a}
|
||||||
|
{a}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\ahat}{\hat{a}}
|
||||||
|
{\hat{a}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Standard
|
||||||
|
\begin_inset Note Comment
|
||||||
|
status collapsed
|
||||||
|
|
||||||
|
\begin_layout Plain Layout
|
||||||
|
SL(3),8
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Standard
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\SLthree}{SL(3)}
|
||||||
|
{SL(3)}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\slthree}{\mathfrak{sl(3)}}
|
||||||
|
{\mathfrak{sl(3)}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\hh}{h}
|
||||||
|
{h}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset FormulaMacro
|
||||||
|
\newcommand{\hhat}{\hat{h}}
|
||||||
|
{\hat{h}}
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\end_body
|
\end_body
|
||||||
|
|
|
||||||
396
doc/math.lyx
396
doc/math.lyx
|
|
@ -1237,21 +1237,28 @@ reference "eq:ApproximateObjective"
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
.
|
.
|
||||||
In particular, the notion of an exponential map allows us to define an
|
In particular, the notion of an exponential map allows us to define a mapping
|
||||||
incremental transformation as tracing out a geodesic curve on the group
|
from
|
||||||
manifold along a certain
|
|
||||||
\series bold
|
\series bold
|
||||||
tangent vector
|
local coordinates
|
||||||
\series default
|
\series default
|
||||||
|
|
||||||
\begin_inset Formula $\xi$
|
\begin_inset Formula $\xi$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
back to a neighborhood in
|
||||||
|
\begin_inset Formula $G$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
around
|
||||||
|
\begin_inset Formula $a$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
,
|
,
|
||||||
\begin_inset Formula
|
\begin_inset Formula
|
||||||
\[
|
\begin{equation}
|
||||||
a\oplus\xi\define a\exp\left(\hat{\xi}\right)
|
a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap}
|
||||||
\]
|
\end{equation}
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -1263,11 +1270,12 @@ with
|
||||||
\begin_inset Formula $n$
|
\begin_inset Formula $n$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
-dimensional Lie group,
|
-dimensional Lie group.
|
||||||
|
Above,
|
||||||
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
|
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
the Lie algebra element corresponding to the vector
|
is the Lie algebra element corresponding to the vector
|
||||||
\begin_inset Formula $\xi$
|
\begin_inset Formula $\xi$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -1305,7 +1313,7 @@ For the Lie group
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
is denoted as
|
is denoted as
|
||||||
\begin_inset Formula $\omega$
|
\begin_inset Formula $\omega t$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
and represents an angular displacement.
|
and represents an angular displacement.
|
||||||
|
|
@ -1314,17 +1322,17 @@ For the Lie group
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
is a skew symmetric matrix denoted as
|
is a skew symmetric matrix denoted as
|
||||||
\begin_inset Formula $\Skew{\omega}\in\sothree$
|
\begin_inset Formula $\Skew{\omega t}\in\sothree$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
, and is given by
|
, and is given by
|
||||||
\begin_inset Formula
|
\begin_inset Formula
|
||||||
\[
|
\[
|
||||||
\Skew{\omega}=\left[\begin{array}{ccc}
|
\Skew{\omega t}=\left[\begin{array}{ccc}
|
||||||
0 & -\omega_{z} & \omega_{y}\\
|
0 & -\omega_{z} & \omega_{y}\\
|
||||||
\omega_{z} & 0 & -\omega_{x}\\
|
\omega_{z} & 0 & -\omega_{x}\\
|
||||||
-\omega_{y} & \omega_{x} & 0
|
-\omega_{y} & \omega_{x} & 0
|
||||||
\end{array}\right]
|
\end{array}\right]t
|
||||||
\]
|
\]
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
@ -1334,12 +1342,136 @@ Finally, the increment
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
corresponds to an incremental rotation
|
corresponds to an incremental rotation
|
||||||
\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$
|
\begin_inset Formula $R\oplus\omega t=Re^{\Skew{\omega t}}$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
.
|
.
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Subsection
|
||||||
|
Local Coordinates vs.
|
||||||
|
Tangent Vectors
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Standard
|
||||||
|
In differential geometry,
|
||||||
|
\series bold
|
||||||
|
tangent vectors
|
||||||
|
\series default
|
||||||
|
|
||||||
|
\begin_inset Formula $v\in T_{a}G$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
at
|
||||||
|
\begin_inset Formula $a$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
are elements of the Lie algebra
|
||||||
|
\begin_inset Formula $\mathfrak{g}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, and are defined as
|
||||||
|
\begin_inset Formula
|
||||||
|
\[
|
||||||
|
v\define\Jac{\gamma(t)}t{t=0}
|
||||||
|
\]
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
where
|
||||||
|
\begin_inset Formula $\gamma$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
is some curve that passes through
|
||||||
|
\begin_inset Formula $a$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
at
|
||||||
|
\begin_inset Formula $t=0$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, i.e.
|
||||||
|
|
||||||
|
\begin_inset Formula $\gamma(0)=a$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
.
|
||||||
|
In particular, for any fixed local coordinate
|
||||||
|
\begin_inset Formula $\xi$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
the map
|
||||||
|
\begin_inset CommandInset ref
|
||||||
|
LatexCommand eqref
|
||||||
|
reference "eq:expmap"
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
can be used to define a
|
||||||
|
\series bold
|
||||||
|
geodesic curve
|
||||||
|
\series default
|
||||||
|
on the group manifold defined by
|
||||||
|
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, and the corresponding tangent vector is given by
|
||||||
|
\begin_inset Formula
|
||||||
|
\begin{equation}
|
||||||
|
\Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector}
|
||||||
|
\end{equation}
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
This defines the mapping between local coordinates
|
||||||
|
\begin_inset Formula $\xi\in\Rn$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
and actual tangent vectors
|
||||||
|
\begin_inset Formula $a\xihat\in g$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
: the vector
|
||||||
|
\begin_inset Formula $\xi$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
defines a direction of travel on the manifold, but does so in the local
|
||||||
|
coordinate frame
|
||||||
|
\begin_inset Formula $a$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
.
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Example
|
||||||
|
Assume a rigid body's attitude is described by
|
||||||
|
\begin_inset Formula $R_{b}^{n}(t)$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, where the indices denote the navigation frame
|
||||||
|
\begin_inset Formula $N$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
and body frame
|
||||||
|
\begin_inset Formula $B$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, respectively.
|
||||||
|
An extrinsically calibrated gyroscope measures the angular velocity
|
||||||
|
\begin_inset Formula $\omega^{b}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, in the body frame, and the corresponding tangent vector is
|
||||||
|
\begin_inset Formula
|
||||||
|
\[
|
||||||
|
\dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}}
|
||||||
|
\]
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Subsection
|
\begin_layout Subsection
|
||||||
Derivatives
|
Derivatives
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
@ -1352,7 +1484,7 @@ reference "def:differentiable"
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
to map exponential coordinates
|
to map local coordinates
|
||||||
\begin_inset Formula $\xi$
|
\begin_inset Formula $\xi$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -1368,7 +1500,7 @@ reference "def:differentiable"
|
||||||
\begin_inset Formula $Df_{a}$
|
\begin_inset Formula $Df_{a}$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
locally approximates a function
|
approximates the function
|
||||||
\begin_inset Formula $f$
|
\begin_inset Formula $f$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -1378,6 +1510,10 @@ reference "def:differentiable"
|
||||||
|
|
||||||
to
|
to
|
||||||
\begin_inset Formula $\Reals m$
|
\begin_inset Formula $\Reals m$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
in a neighborhood around
|
||||||
|
\begin_inset Formula $a$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
:
|
:
|
||||||
|
|
@ -1455,41 +1591,6 @@ derivative
|
||||||
.
|
.
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Standard
|
|
||||||
Note that the vectors
|
|
||||||
\begin_inset Formula $\xi$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
can be viewed as lying in the tangent space to
|
|
||||||
\begin_inset Formula $G$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
at
|
|
||||||
\begin_inset Formula $a$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
, but defining this rigorously would take us on a longer tour of differential
|
|
||||||
geometry.
|
|
||||||
Informally,
|
|
||||||
\begin_inset Formula $\xi$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
is simply the direction, in a local coordinate frame, that is locally tangent
|
|
||||||
at
|
|
||||||
\begin_inset Formula $a$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
to a geodesic curve
|
|
||||||
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
traced out by the exponential map, with
|
|
||||||
\begin_inset Formula $\gamma(0)=a$
|
|
||||||
\end_inset
|
|
||||||
|
|
||||||
.
|
|
||||||
\end_layout
|
|
||||||
|
|
||||||
\begin_layout Subsection
|
\begin_layout Subsection
|
||||||
Derivative of an Action
|
Derivative of an Action
|
||||||
\begin_inset CommandInset label
|
\begin_inset CommandInset label
|
||||||
|
|
@ -3000,7 +3101,7 @@ f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g)
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Subsection
|
\begin_layout Subsection
|
||||||
Derivative of the Exponential and Logarithm Map
|
Derivative of the Exponential Map
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
\begin_layout Theorem
|
\begin_layout Theorem
|
||||||
|
|
@ -3064,17 +3165,196 @@ For
|
||||||
\begin_inset Formula $\xi\neq0$
|
\begin_inset Formula $\xi\neq0$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
, things are not simple, see .
|
, things are not simple.
|
||||||
|
As with pushforwards above, we will be looking for an
|
||||||
|
\begin_inset Formula $n\times n$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
\begin_inset Flex URL
|
\family roman
|
||||||
|
\series medium
|
||||||
|
\shape up
|
||||||
|
\size normal
|
||||||
|
\emph off
|
||||||
|
\bar no
|
||||||
|
\strikeout off
|
||||||
|
\uuline off
|
||||||
|
\uwave off
|
||||||
|
\noun off
|
||||||
|
\color none
|
||||||
|
Jacobian
|
||||||
|
\begin_inset Formula $f'(\xi)$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
such that
|
||||||
|
\family default
|
||||||
|
\series default
|
||||||
|
\shape default
|
||||||
|
\size default
|
||||||
|
\emph default
|
||||||
|
\bar default
|
||||||
|
\strikeout default
|
||||||
|
\uuline default
|
||||||
|
\uwave default
|
||||||
|
\noun default
|
||||||
|
\color inherit
|
||||||
|
|
||||||
|
\begin_inset Formula
|
||||||
|
\begin{equation}
|
||||||
|
f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp}
|
||||||
|
\end{equation}
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
Differential geometry tells us that for any Lie algebra element
|
||||||
|
\begin_inset Formula $\xihat\in\mathfrak{g}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
there exists a
|
||||||
|
\emph on
|
||||||
|
linear
|
||||||
|
\emph default
|
||||||
|
map
|
||||||
|
\begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, which is given by
|
||||||
|
\begin_inset Foot
|
||||||
status collapsed
|
status collapsed
|
||||||
|
|
||||||
\begin_layout Plain Layout
|
\begin_layout Plain Layout
|
||||||
|
See
|
||||||
|
\begin_inset Flex URL
|
||||||
|
status open
|
||||||
|
|
||||||
http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti
|
\begin_layout Plain Layout
|
||||||
al-of-the-exponential/
|
|
||||||
|
http://deltaepsilons.wordpress.com/2009/11/06/
|
||||||
\end_layout
|
\end_layout
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
or
|
||||||
|
\begin_inset Flex URL
|
||||||
|
status open
|
||||||
|
|
||||||
|
\begin_layout Plain Layout
|
||||||
|
|
||||||
|
https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
.
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
|
||||||
|
\begin_inset Formula
|
||||||
|
\begin{equation}
|
||||||
|
d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp}
|
||||||
|
\end{equation}
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
with
|
||||||
|
\begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
and
|
||||||
|
\begin_inset Formula $ad_{\xihat}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
itself a linear map taking
|
||||||
|
\begin_inset Formula $\hat{x}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
to
|
||||||
|
\begin_inset Formula $[\xihat,\hat{x}]$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, the Lie bracket.
|
||||||
|
The actual formula above is not really as important as the fact that the
|
||||||
|
linear map exists, although it is expressed directly in terms of tangent
|
||||||
|
vectors to
|
||||||
|
\begin_inset Formula $\mathfrak{g}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
and
|
||||||
|
\begin_inset Formula $G$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
.
|
||||||
|
Equation
|
||||||
|
\begin_inset CommandInset ref
|
||||||
|
LatexCommand eqref
|
||||||
|
reference "eq:dexp"
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
is a tangent vector, and comparing with
|
||||||
|
\begin_inset CommandInset ref
|
||||||
|
LatexCommand eqref
|
||||||
|
reference "eq:tangent-vector"
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
we see that it maps to local coordinates
|
||||||
|
\begin_inset Formula $y$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
as follows:
|
||||||
|
\begin_inset Formula
|
||||||
|
\[
|
||||||
|
\yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}
|
||||||
|
\]
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
which can be used to construct the Jacobian
|
||||||
|
\begin_inset Formula $f'(\xi)$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
.
|
||||||
|
\end_layout
|
||||||
|
|
||||||
|
\begin_layout Example
|
||||||
|
For
|
||||||
|
\begin_inset Formula $\SOthree$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, the operator
|
||||||
|
\begin_inset Formula $ad_{\xihat}$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
is simply a matrix multiplication when representing
|
||||||
|
\begin_inset Formula $\sothree$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
using 3-vectors, i.e.,
|
||||||
|
\begin_inset Formula $ad_{\xihat}x=\xihat x$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
, and the
|
||||||
|
\begin_inset Formula $3\times3$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
Jacobian corresponding to
|
||||||
|
\begin_inset Formula $d\exp$
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
is
|
||||||
|
\begin_inset Formula
|
||||||
|
\[
|
||||||
|
f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k}
|
||||||
|
\]
|
||||||
|
|
||||||
|
\end_inset
|
||||||
|
|
||||||
|
which, similar to the exponential map, has a simple closed form expression
|
||||||
|
for
|
||||||
|
\begin_inset Formula $\SOthree$
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
.
|
.
|
||||||
|
|
@ -3097,7 +3377,7 @@ Retractions
|
||||||
|
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
\begin_inset FormulaMacro
|
\begin_inset FormulaMacro
|
||||||
\newcommand{\retract}{\mathcal{R}}
|
\renewcommand{\retract}{\mathcal{R}}
|
||||||
{\mathcal{R}}
|
{\mathcal{R}}
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
||||||
|
|
@ -6797,7 +7077,7 @@ Then
|
||||||
\begin_layout Standard
|
\begin_layout Standard
|
||||||
\begin_inset CommandInset bibtex
|
\begin_inset CommandInset bibtex
|
||||||
LatexCommand bibtex
|
LatexCommand bibtex
|
||||||
bibfiles "/Users/dellaert/papers/refs"
|
bibfiles "refs"
|
||||||
options "plain"
|
options "plain"
|
||||||
|
|
||||||
\end_inset
|
\end_inset
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
@article{Iserles00an,
|
||||||
|
title = {Lie-group methods},
|
||||||
|
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
|
||||||
|
N{\o}rsett, Syvert P and Zanna, Antonella},
|
||||||
|
journal = {Acta Numerica 2000},
|
||||||
|
volume = {9},
|
||||||
|
pages = {215--365},
|
||||||
|
year = {2000},
|
||||||
|
publisher = {Cambridge Univ Press}
|
||||||
|
}
|
||||||
|
|
||||||
|
@book{Murray94book,
|
||||||
|
title = {A mathematical introduction to robotic manipulation},
|
||||||
|
author = {Murray, Richard M and Li, Zexiang and Sastry, S
|
||||||
|
Shankar and Sastry, S Shankara},
|
||||||
|
year = {1994},
|
||||||
|
publisher = {CRC press}
|
||||||
|
}
|
||||||
|
|
||||||
|
@book{Spivak65book,
|
||||||
|
title = {Calculus on manifolds},
|
||||||
|
author = {Spivak, Michael},
|
||||||
|
volume = {1},
|
||||||
|
year = {1965},
|
||||||
|
publisher = {WA Benjamin New York}
|
||||||
|
}
|
||||||
150
gtsam.h
150
gtsam.h
|
|
@ -2447,7 +2447,7 @@ namespace imuBias {
|
||||||
#include <gtsam/navigation/ImuBias.h>
|
#include <gtsam/navigation/ImuBias.h>
|
||||||
|
|
||||||
class ConstantBias {
|
class ConstantBias {
|
||||||
// Standard Constructor
|
// Constructors
|
||||||
ConstantBias();
|
ConstantBias();
|
||||||
ConstantBias(Vector biasAcc, Vector biasGyro);
|
ConstantBias(Vector biasAcc, Vector biasGyro);
|
||||||
|
|
||||||
|
|
@ -2479,97 +2479,107 @@ class ConstantBias {
|
||||||
|
|
||||||
}///\namespace imuBias
|
}///\namespace imuBias
|
||||||
|
|
||||||
|
#include <gtsam/navigation/NavState.h>
|
||||||
|
class NavState {
|
||||||
|
// Constructors
|
||||||
|
NavState();
|
||||||
|
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
|
||||||
|
NavState(const gtsam::Pose3& pose, Vector v);
|
||||||
|
|
||||||
|
// Testable
|
||||||
|
void print(string s) const;
|
||||||
|
bool equals(const gtsam::NavState& expected, double tol) const;
|
||||||
|
|
||||||
|
// Access
|
||||||
|
gtsam::Rot3 attitude() const;
|
||||||
|
gtsam::Point3 position() const;
|
||||||
|
Vector velocity() const;
|
||||||
|
gtsam::Pose3 pose() const;
|
||||||
|
};
|
||||||
|
|
||||||
|
#include <gtsam/navigation/PreintegrationParams.h>
|
||||||
|
class PreintegrationParams {
|
||||||
|
PreintegrationParams(Vector n_gravity);
|
||||||
|
// TODO(frank): add setters/getters or make this MATLAB wrapper feature
|
||||||
|
};
|
||||||
|
|
||||||
|
#include <gtsam/navigation/PreintegrationBase.h>
|
||||||
|
virtual class PreintegrationBase {
|
||||||
|
// Constructors
|
||||||
|
PreintegrationBase(const gtsam::PreintegrationParams* params);
|
||||||
|
PreintegrationBase(const gtsam::PreintegrationParams* params,
|
||||||
|
const gtsam::imuBias::ConstantBias& bias);
|
||||||
|
|
||||||
|
// Testable
|
||||||
|
void print(string s) const;
|
||||||
|
bool equals(const gtsam::PreintegrationBase& expected, double tol);
|
||||||
|
|
||||||
|
double deltaTij() const;
|
||||||
|
gtsam::Rot3 deltaRij() const;
|
||||||
|
Vector deltaPij() const;
|
||||||
|
Vector deltaVij() const;
|
||||||
|
Vector biasHatVector() const;
|
||||||
|
|
||||||
|
// Standard Interface
|
||||||
|
gtsam::NavState predict(const gtsam::NavState& state_i,
|
||||||
|
const gtsam::imuBias::ConstantBias& bias) const;
|
||||||
|
};
|
||||||
|
|
||||||
#include <gtsam/navigation/ImuFactor.h>
|
#include <gtsam/navigation/ImuFactor.h>
|
||||||
class PoseVelocityBias{
|
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
|
||||||
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
|
// Constructors
|
||||||
};
|
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
|
||||||
class PreintegratedImuMeasurements {
|
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
|
||||||
// Standard Constructor
|
const gtsam::imuBias::ConstantBias& bias);
|
||||||
PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
|
|
||||||
PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
|
|
||||||
// PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs);
|
|
||||||
|
|
||||||
// Testable
|
// Testable
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
|
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
|
||||||
|
|
||||||
double deltaTij() const;
|
|
||||||
gtsam::Rot3 deltaRij() const;
|
|
||||||
Vector deltaPij() const;
|
|
||||||
Vector deltaVij() const;
|
|
||||||
Vector biasHatVector() const;
|
|
||||||
Matrix delPdelBiasAcc() const;
|
|
||||||
Matrix delPdelBiasOmega() const;
|
|
||||||
Matrix delVdelBiasAcc() const;
|
|
||||||
Matrix delVdelBiasOmega() const;
|
|
||||||
Matrix delRdelBiasOmega() const;
|
|
||||||
Matrix preintMeasCov() const;
|
|
||||||
|
|
||||||
// Standard Interface
|
// Standard Interface
|
||||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
|
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
|
||||||
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
|
double deltaT);
|
||||||
Vector gravity, Vector omegaCoriolis) const;
|
void resetIntegration();
|
||||||
|
Matrix preintMeasCov() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
virtual class ImuFactor : gtsam::NonlinearFactor {
|
virtual class ImuFactor: gtsam::NonlinearFactor {
|
||||||
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
|
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
|
||||||
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
|
size_t bias,
|
||||||
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
|
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
|
||||||
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
|
|
||||||
const gtsam::Pose3& body_P_sensor);
|
|
||||||
// Standard Interface
|
// Standard Interface
|
||||||
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
|
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
|
||||||
|
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
|
||||||
|
const gtsam::Pose3& pose_j, Vector vel_j,
|
||||||
|
const gtsam::imuBias::ConstantBias& bias);
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||||
class PreintegratedCombinedMeasurements {
|
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
|
||||||
// Standard Constructor
|
|
||||||
PreintegratedCombinedMeasurements(
|
|
||||||
const gtsam::imuBias::ConstantBias& bias,
|
|
||||||
Matrix measuredAccCovariance,
|
|
||||||
Matrix measuredOmegaCovariance,
|
|
||||||
Matrix integrationErrorCovariance,
|
|
||||||
Matrix biasAccCovariance,
|
|
||||||
Matrix biasOmegaCovariance,
|
|
||||||
Matrix biasAccOmegaInit);
|
|
||||||
PreintegratedCombinedMeasurements(
|
|
||||||
const gtsam::imuBias::ConstantBias& bias,
|
|
||||||
Matrix measuredAccCovariance,
|
|
||||||
Matrix measuredOmegaCovariance,
|
|
||||||
Matrix integrationErrorCovariance,
|
|
||||||
Matrix biasAccCovariance,
|
|
||||||
Matrix biasOmegaCovariance,
|
|
||||||
Matrix biasAccOmegaInit,
|
|
||||||
bool use2ndOrderIntegration);
|
|
||||||
// PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs);
|
|
||||||
|
|
||||||
// Testable
|
// Testable
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol);
|
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
|
||||||
|
double tol);
|
||||||
double deltaTij() const;
|
|
||||||
gtsam::Rot3 deltaRij() const;
|
|
||||||
Vector deltaPij() const;
|
|
||||||
Vector deltaVij() const;
|
|
||||||
Vector biasHatVector() const;
|
|
||||||
Matrix delPdelBiasAcc() const;
|
|
||||||
Matrix delPdelBiasOmega() const;
|
|
||||||
Matrix delVdelBiasAcc() const;
|
|
||||||
Matrix delVdelBiasOmega() const;
|
|
||||||
Matrix delRdelBiasOmega() const;
|
|
||||||
Matrix preintMeasCov() const;
|
|
||||||
|
|
||||||
// Standard Interface
|
// Standard Interface
|
||||||
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
|
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
|
||||||
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
|
double deltaT);
|
||||||
Vector gravity, Vector omegaCoriolis) const;
|
void resetIntegration();
|
||||||
|
Matrix preintMeasCov() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
|
virtual class CombinedImuFactor: gtsam::NonlinearFactor {
|
||||||
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
|
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
|
||||||
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
|
size_t bias_i, size_t bias_j,
|
||||||
|
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
|
||||||
|
|
||||||
// Standard Interface
|
// Standard Interface
|
||||||
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
|
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
|
||||||
|
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
|
||||||
|
const gtsam::Pose3& pose_j, Vector vel_j,
|
||||||
|
const gtsam::imuBias::ConstantBias& bias_i,
|
||||||
|
const gtsam::imuBias::ConstantBias& bias_j);
|
||||||
};
|
};
|
||||||
|
|
||||||
#include <gtsam/navigation/AHRSFactor.h>
|
#include <gtsam/navigation/AHRSFactor.h>
|
||||||
|
|
|
||||||
|
|
@ -21,15 +21,17 @@
|
||||||
// \callgraph
|
// \callgraph
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
#include <gtsam/base/OptionalJacobian.h>
|
||||||
#include <gtsam/base/Vector.h>
|
#include <gtsam/base/Vector.h>
|
||||||
#include <gtsam/config.h> // Configuration from CMake
|
#include <gtsam/config.h> // Configuration from CMake
|
||||||
|
|
||||||
#include <boost/math/special_functions/fpclassify.hpp>
|
|
||||||
#include <Eigen/Core>
|
#include <Eigen/Core>
|
||||||
#include <Eigen/Cholesky>
|
#include <Eigen/Cholesky>
|
||||||
#include <Eigen/LU>
|
#include <Eigen/LU>
|
||||||
#include <boost/format.hpp>
|
#include <boost/format.hpp>
|
||||||
|
#include <boost/function.hpp>
|
||||||
#include <boost/tuple/tuple.hpp>
|
#include <boost/tuple/tuple.hpp>
|
||||||
|
#include <boost/math/special_functions/fpclassify.hpp>
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
|
@ -532,6 +534,75 @@ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
|
||||||
|
|
||||||
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
|
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Functor that implements multiplication of a vector b with the inverse of a
|
||||||
|
* matrix A. The derivatives are inspired by Mike Giles' "An extended collection
|
||||||
|
* of matrix derivative results for forward and reverse mode algorithmic
|
||||||
|
* differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf
|
||||||
|
*/
|
||||||
|
template <int N>
|
||||||
|
struct MultiplyWithInverse {
|
||||||
|
typedef Eigen::Matrix<double, N, 1> VectorN;
|
||||||
|
typedef Eigen::Matrix<double, N, N> MatrixN;
|
||||||
|
|
||||||
|
/// A.inverse() * b, with optional derivatives
|
||||||
|
VectorN operator()(const MatrixN& A, const VectorN& b,
|
||||||
|
OptionalJacobian<N, N* N> H1 = boost::none,
|
||||||
|
OptionalJacobian<N, N> H2 = boost::none) const {
|
||||||
|
const MatrixN invA = A.inverse();
|
||||||
|
const VectorN c = invA * b;
|
||||||
|
// The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
|
||||||
|
if (H1)
|
||||||
|
for (size_t j = 0; j < N; j++)
|
||||||
|
H1->template middleCols<N>(N * j) = -c[j] * invA;
|
||||||
|
// The derivative in b is easy, as invA*b is just a linear map:
|
||||||
|
if (H2) *H2 = invA;
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Functor that implements multiplication with the inverse of a matrix, itself
|
||||||
|
* the result of a function f. It turn out we only need the derivatives of the
|
||||||
|
* operator phi(a): b -> f(a) * b
|
||||||
|
*/
|
||||||
|
template <typename T, int N>
|
||||||
|
struct MultiplyWithInverseFunction {
|
||||||
|
enum { M = traits<T>::dimension };
|
||||||
|
typedef Eigen::Matrix<double, N, 1> VectorN;
|
||||||
|
typedef Eigen::Matrix<double, N, N> MatrixN;
|
||||||
|
|
||||||
|
// The function phi should calculate f(a)*b, with derivatives in a and b.
|
||||||
|
// Naturally, the derivative in b is f(a).
|
||||||
|
typedef boost::function<VectorN(
|
||||||
|
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
|
||||||
|
Operator;
|
||||||
|
|
||||||
|
/// Construct with function as explained above
|
||||||
|
MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}
|
||||||
|
|
||||||
|
/// f(a).inverse() * b, with optional derivatives
|
||||||
|
VectorN operator()(const T& a, const VectorN& b,
|
||||||
|
OptionalJacobian<N, M> H1 = boost::none,
|
||||||
|
OptionalJacobian<N, N> H2 = boost::none) const {
|
||||||
|
MatrixN A;
|
||||||
|
phi_(a, b, boost::none, A); // get A = f(a) by calling f once
|
||||||
|
const MatrixN invA = A.inverse();
|
||||||
|
const VectorN c = invA * b;
|
||||||
|
|
||||||
|
if (H1) {
|
||||||
|
Eigen::Matrix<double, N, M> H;
|
||||||
|
phi_(a, c, H, boost::none); // get derivative H of forward mapping
|
||||||
|
*H1 = -invA* H;
|
||||||
|
}
|
||||||
|
if (H2) *H2 = invA;
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
const Operator phi_;
|
||||||
|
};
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
||||||
#include <boost/serialization/nvp.hpp>
|
#include <boost/serialization/nvp.hpp>
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -66,64 +66,69 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegratedCombinedMeasurements::integrateMeasurement(
|
void PreintegratedCombinedMeasurements::integrateMeasurement(
|
||||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {
|
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||||
|
|
||||||
const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
|
|
||||||
|
|
||||||
// Update preintegrated measurements.
|
// Update preintegrated measurements.
|
||||||
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
|
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||||
Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
|
Matrix93 B, C;
|
||||||
Matrix93 G1,G2;
|
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||||
PreintegrationBase::update(measuredAcc, measuredOmega, deltaT,
|
|
||||||
&D_incrR_integratedOmega, &F_9x9, &G1, &G2);
|
|
||||||
|
|
||||||
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
|
// Update preintegrated measurements covariance: as in [2] we consider a first
|
||||||
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
|
// order propagation that can be seen as a prediction phase in an EKF
|
||||||
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
|
// framework. In this implementation, in contrast to [2], we consider the
|
||||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
// uncertainty of the bias selection and we keep correlation between biases
|
||||||
|
// and preintegrated measurements
|
||||||
|
|
||||||
// Single Jacobians to propagate covariance
|
// Single Jacobians to propagate covariance
|
||||||
Matrix3 H_vel_biasacc = -dRij * deltaT;
|
// TODO(frank): should we not also accout for bias on position?
|
||||||
Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT;
|
Matrix3 theta_H_biasOmega = - C.topRows<3>();
|
||||||
|
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
|
||||||
|
|
||||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||||
Eigen::Matrix<double,15,15> F;
|
Eigen::Matrix<double, 15, 15> F;
|
||||||
F.setZero();
|
F.setZero();
|
||||||
F.block<9, 9>(0, 0) = F_9x9;
|
F.block<9, 9>(0, 0) = A;
|
||||||
F.block<3, 3>(0, 12) = H_angles_biasomega;
|
F.block<3, 3>(0, 12) = theta_H_biasOmega;
|
||||||
F.block<3, 3>(6, 9) = H_vel_biasacc;
|
F.block<3, 3>(6, 9) = vel_H_biasAcc;
|
||||||
F.block<6, 6>(9, 9) = I_6x6;
|
F.block<6, 6>(9, 9) = I_6x6;
|
||||||
|
|
||||||
|
// propagate uncertainty
|
||||||
|
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
|
||||||
|
const Matrix3& aCov = p().accelerometerCovariance;
|
||||||
|
const Matrix3& wCov = p().gyroscopeCovariance;
|
||||||
|
const Matrix3& iCov = p().integrationCovariance;
|
||||||
|
|
||||||
// first order uncertainty propagation
|
// first order uncertainty propagation
|
||||||
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
|
// Optimized matrix multiplication (1/dt) * G * measurementCovariance *
|
||||||
Eigen::Matrix<double,15,15> G_measCov_Gt;
|
// G.transpose()
|
||||||
|
Eigen::Matrix<double, 15, 15> G_measCov_Gt;
|
||||||
G_measCov_Gt.setZero(15, 15);
|
G_measCov_Gt.setZero(15, 15);
|
||||||
|
|
||||||
// BLOCK DIAGONAL TERMS
|
// BLOCK DIAGONAL TERMS
|
||||||
D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
|
D_t_t(&G_measCov_Gt) = dt * iCov;
|
||||||
D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
|
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc *
|
||||||
* (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
|
(aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
|
||||||
* (H_vel_biasacc.transpose());
|
(vel_H_biasAcc.transpose());
|
||||||
D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
|
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega *
|
||||||
* (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
|
(wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
|
||||||
* (H_angles_biasomega.transpose());
|
(theta_H_biasOmega.transpose());
|
||||||
D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance;
|
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
|
||||||
D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance;
|
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
|
||||||
|
|
||||||
// OFF BLOCK DIAGONAL TERMS
|
// OFF BLOCK DIAGONAL TERMS
|
||||||
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
|
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
|
||||||
* H_angles_biasomega.transpose();
|
theta_H_biasOmega.transpose();
|
||||||
D_v_R(&G_measCov_Gt) = temp;
|
D_v_R(&G_measCov_Gt) = temp;
|
||||||
D_R_v(&G_measCov_Gt) = temp.transpose();
|
D_R_v(&G_measCov_Gt) = temp.transpose();
|
||||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
|
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
|
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
|
||||||
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
|
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
|
||||||
const Matrix3& measuredOmegaCovariance,
|
const Matrix3& measuredOmegaCovariance,
|
||||||
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
|
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
|
||||||
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
|
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
|
||||||
const bool use2ndOrderIntegration) {
|
const bool use2ndOrderIntegration) {
|
||||||
if (!use2ndOrderIntegration)
|
if (!use2ndOrderIntegration)
|
||||||
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
||||||
|
|
@ -134,11 +139,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
|
||||||
p->integrationCovariance = integrationErrorCovariance;
|
p->integrationCovariance = integrationErrorCovariance;
|
||||||
p->biasAccCovariance = biasAccCovariance;
|
p->biasAccCovariance = biasAccCovariance;
|
||||||
p->biasOmegaCovariance = biasOmegaCovariance;
|
p->biasOmegaCovariance = biasOmegaCovariance;
|
||||||
p->biasAccOmegaInit = biasAccOmegaInit;
|
p->biasAccOmegaInt = biasAccOmegaInt;
|
||||||
p_ = p;
|
p_ = p;
|
||||||
resetIntegration();
|
resetIntegration();
|
||||||
preintMeasCov_.setZero();
|
preintMeasCov_.setZero();
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
// CombinedImuFactor methods
|
// CombinedImuFactor methods
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
|
@ -238,6 +244,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
CombinedImuFactor::CombinedImuFactor(
|
CombinedImuFactor::CombinedImuFactor(
|
||||||
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
|
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
|
||||||
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
|
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
|
||||||
|
|
@ -254,7 +261,7 @@ CombinedImuFactor::CombinedImuFactor(
|
||||||
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
|
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
|
||||||
_PIM_.p_ = p;
|
_PIM_.p_ = p;
|
||||||
}
|
}
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
Pose3& pose_j, Vector3& vel_j,
|
Pose3& pose_j, Vector3& vel_j,
|
||||||
const imuBias::ConstantBias& bias_i,
|
const imuBias::ConstantBias& bias_i,
|
||||||
|
|
@ -268,6 +275,7 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
pose_j = pvb.pose;
|
pose_j = pvb.pose;
|
||||||
vel_j = pvb.velocity;
|
vel_j = pvb.velocity;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
} /// namespace gtsam
|
} /// namespace gtsam
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -63,15 +63,15 @@ public:
|
||||||
|
|
||||||
/// Parameters for pre-integration:
|
/// Parameters for pre-integration:
|
||||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||||
struct Params : PreintegrationBase::Params {
|
struct Params : PreintegrationParams {
|
||||||
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
|
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
|
||||||
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
|
||||||
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
|
Matrix6 biasAccOmegaInt; ///< covariance of bias used for pre-integration
|
||||||
|
|
||||||
/// See two named constructors below for good values of n_gravity in body frame
|
/// See two named constructors below for good values of n_gravity in body frame
|
||||||
Params(const Vector3& n_gravity) :
|
Params(const Vector3& n_gravity) :
|
||||||
PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
|
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
|
||||||
I_3x3), biasAccOmegaInit(I_6x6) {
|
I_3x3), biasAccOmegaInt(I_6x6) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
|
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
|
||||||
|
|
@ -95,7 +95,7 @@ public:
|
||||||
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
|
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
|
||||||
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
|
||||||
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
|
||||||
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
|
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
@ -113,28 +113,46 @@ public:
|
||||||
friend class CombinedImuFactor;
|
friend class CombinedImuFactor;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
/// @name Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Default constructor, initializes the class with no measurements
|
* Default constructor, initializes the class with no measurements
|
||||||
* @param bias Current estimate of acceleration and rotation rate biases
|
* @param bias Current estimate of acceleration and rotation rate biases
|
||||||
*/
|
*/
|
||||||
PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p,
|
PreintegratedCombinedMeasurements(
|
||||||
const imuBias::ConstantBias& biasHat)
|
const boost::shared_ptr<Params>& p,
|
||||||
|
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
|
||||||
: PreintegrationBase(p, biasHat) {
|
: PreintegrationBase(p, biasHat) {
|
||||||
preintMeasCov_.setZero();
|
preintMeasCov_.setZero();
|
||||||
}
|
}
|
||||||
|
|
||||||
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
|
/// @}
|
||||||
|
|
||||||
/// print
|
/// @name Basic utilities
|
||||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
/// @{
|
||||||
|
|
||||||
/// equals
|
|
||||||
bool equals(const PreintegratedCombinedMeasurements& expected,
|
|
||||||
double tol = 1e-9) const;
|
|
||||||
|
|
||||||
/// Re-initialize PreintegratedCombinedMeasurements
|
/// Re-initialize PreintegratedCombinedMeasurements
|
||||||
void resetIntegration();
|
void resetIntegration();
|
||||||
|
|
||||||
|
/// const reference to params, shadows definition in base class
|
||||||
|
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Access instance variables
|
||||||
|
/// @{
|
||||||
|
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||||
|
bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Main functionality
|
||||||
|
/// @{
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Add a single IMU measurement to the preintegration.
|
* Add a single IMU measurement to the preintegration.
|
||||||
* @param measuredAcc Measured acceleration (in body frame, as given by the
|
* @param measuredAcc Measured acceleration (in body frame, as given by the
|
||||||
|
|
@ -147,9 +165,9 @@ public:
|
||||||
void integrateMeasurement(const Vector3& measuredAcc,
|
void integrateMeasurement(const Vector3& measuredAcc,
|
||||||
const Vector3& measuredOmega, double deltaT);
|
const Vector3& measuredOmega, double deltaT);
|
||||||
|
|
||||||
/// methods to access class variables
|
/// @}
|
||||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
/// deprecated constructor
|
/// deprecated constructor
|
||||||
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
|
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
|
||||||
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
|
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
|
||||||
|
|
@ -157,7 +175,8 @@ public:
|
||||||
const Matrix3& measuredOmegaCovariance,
|
const Matrix3& measuredOmegaCovariance,
|
||||||
const Matrix3& integrationErrorCovariance,
|
const Matrix3& integrationErrorCovariance,
|
||||||
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
|
||||||
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
|
const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true);
|
||||||
|
#endif
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// Serialization function
|
/// Serialization function
|
||||||
|
|
@ -257,6 +276,7 @@ public:
|
||||||
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
|
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
|
||||||
boost::none, boost::optional<Matrix&> H6 = boost::none) const;
|
boost::none, boost::optional<Matrix&> H6 = boost::none) const;
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
/// @deprecated typename
|
/// @deprecated typename
|
||||||
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
|
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
|
||||||
|
|
||||||
|
|
@ -273,6 +293,7 @@ public:
|
||||||
CombinedPreintegratedMeasurements& pim,
|
CombinedPreintegratedMeasurements& pim,
|
||||||
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
||||||
const bool use2ndOrderCoriolis = false);
|
const bool use2ndOrderCoriolis = false);
|
||||||
|
#endif
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,82 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file ImuBias.cpp
|
||||||
|
* @date Feb 2, 2012
|
||||||
|
* @author Vadim Indelman, Stephen Williams
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "ImuBias.h"
|
||||||
|
|
||||||
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/// All bias models live in the imuBias namespace
|
||||||
|
namespace imuBias {
|
||||||
|
|
||||||
|
/*
|
||||||
|
* NOTES:
|
||||||
|
* - Earth-rate correction:
|
||||||
|
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
|
||||||
|
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||||
|
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
|
||||||
|
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||||
|
*
|
||||||
|
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
|
||||||
|
*/
|
||||||
|
// // H1: Jacobian w.r.t. IMUBias
|
||||||
|
// // H2: Jacobian w.r.t. pose
|
||||||
|
// Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G,
|
||||||
|
// boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
|
||||||
|
//
|
||||||
|
// Matrix R_G_to_I( pose.rotation().matrix().transpose() );
|
||||||
|
// Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G;
|
||||||
|
//
|
||||||
|
// if (H1){
|
||||||
|
// Matrix zeros3_3(zeros(3,3));
|
||||||
|
// Matrix m_eye3(-eye(3));
|
||||||
|
//
|
||||||
|
// *H1 = collect(2, &zeros3_3, &m_eye3);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// if (H2){
|
||||||
|
// Matrix zeros3_3(zeros(3,3));
|
||||||
|
// Matrix H = -skewSymmetric(w_earth_rate_I);
|
||||||
|
//
|
||||||
|
// *H2 = collect(2, &H, &zeros3_3);
|
||||||
|
// }
|
||||||
|
//
|
||||||
|
// //TODO: Make sure H2 is correct.
|
||||||
|
//
|
||||||
|
// return measurement - biasGyro_ - w_earth_rate_I;
|
||||||
|
//
|
||||||
|
//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
|
||||||
|
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
|
||||||
|
// }
|
||||||
|
/// ostream operator
|
||||||
|
std::ostream& operator<<(std::ostream& os, const ConstantBias& bias) {
|
||||||
|
os << "acc = " << Point3(bias.accelerometer());
|
||||||
|
os << " gyro = " << Point3(bias.gyroscope());
|
||||||
|
return os;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// print with optional string
|
||||||
|
void ConstantBias::print(const std::string& s) const {
|
||||||
|
std::cout << s << *this << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace imuBias
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
||||||
|
|
@ -17,22 +17,11 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/OptionalJacobian.h>
|
||||||
#include <gtsam/base/Vector.h>
|
|
||||||
#include <gtsam/base/VectorSpace.h>
|
#include <gtsam/base/VectorSpace.h>
|
||||||
|
#include <iosfwd>
|
||||||
#include <boost/serialization/nvp.hpp>
|
#include <boost/serialization/nvp.hpp>
|
||||||
|
|
||||||
/*
|
|
||||||
* NOTES:
|
|
||||||
* - Earth-rate correction:
|
|
||||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined by the user).
|
|
||||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
|
||||||
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
|
|
||||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
|
||||||
*
|
|
||||||
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
|
|
||||||
*/
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/// All bias models live in the imuBias namespace
|
/// All bias models live in the imuBias namespace
|
||||||
|
|
@ -94,46 +83,16 @@ public:
|
||||||
return measurement - biasGyro_;
|
return measurement - biasGyro_;
|
||||||
}
|
}
|
||||||
|
|
||||||
// // H1: Jacobian w.r.t. IMUBias
|
/// @}
|
||||||
// // H2: Jacobian w.r.t. pose
|
/// @name Testable
|
||||||
// Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G,
|
/// @{
|
||||||
// boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
|
|
||||||
//
|
|
||||||
// Matrix R_G_to_I( pose.rotation().matrix().transpose() );
|
|
||||||
// Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G;
|
|
||||||
//
|
|
||||||
// if (H1){
|
|
||||||
// Matrix zeros3_3(zeros(3,3));
|
|
||||||
// Matrix m_eye3(-eye(3));
|
|
||||||
//
|
|
||||||
// *H1 = collect(2, &zeros3_3, &m_eye3);
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// if (H2){
|
|
||||||
// Matrix zeros3_3(zeros(3,3));
|
|
||||||
// Matrix H = -skewSymmetric(w_earth_rate_I);
|
|
||||||
//
|
|
||||||
// *H2 = collect(2, &H, &zeros3_3);
|
|
||||||
// }
|
|
||||||
//
|
|
||||||
// //TODO: Make sure H2 is correct.
|
|
||||||
//
|
|
||||||
// return measurement - biasGyro_ - w_earth_rate_I;
|
|
||||||
//
|
|
||||||
//// Vector bias_gyro_temp((Vector(3) << -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
|
|
||||||
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
|
|
||||||
// }
|
|
||||||
|
|
||||||
/// @}
|
/// ostream operator
|
||||||
/// @name Testable
|
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
|
||||||
/// @{
|
const ConstantBias& bias);
|
||||||
|
|
||||||
/// print with optional string
|
/// print with optional string
|
||||||
void print(const std::string& s = "") const {
|
void print(const std::string& s = "") const;
|
||||||
// explicit printing for now.
|
|
||||||
std::cout << s + ".Acc [" << biasAcc_.transpose() << "]" << std::endl;
|
|
||||||
std::cout << s + ".Gyro [" << biasGyro_.transpose() << "]" << std::endl;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** equality up to tolerance */
|
/** equality up to tolerance */
|
||||||
inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
|
inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -29,7 +29,7 @@ namespace gtsam {
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
// Inner class PreintegratedMeasurements
|
// Inner class PreintegratedImuMeasurements
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegratedImuMeasurements::print(const string& s) const {
|
void PreintegratedImuMeasurements::print(const string& s) const {
|
||||||
PreintegrationBase::print(s);
|
PreintegrationBase::print(s);
|
||||||
|
|
@ -52,45 +52,39 @@ void PreintegratedImuMeasurements::resetIntegration() {
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegratedImuMeasurements::integrateMeasurement(
|
void PreintegratedImuMeasurements::integrateMeasurement(
|
||||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||||
|
|
||||||
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
|
|
||||||
|
|
||||||
// Update preintegrated measurements (also get Jacobian)
|
// Update preintegrated measurements (also get Jacobian)
|
||||||
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
|
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
|
||||||
Matrix93 G1, G2;
|
Matrix93 B, C;
|
||||||
Matrix3 D_incrR_integratedOmega;
|
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||||
PreintegrationBase::update(measuredAcc, measuredOmega, dt,
|
|
||||||
&D_incrR_integratedOmega, &F, &G1, &G2);
|
|
||||||
|
|
||||||
// first order covariance propagation:
|
// first order covariance propagation:
|
||||||
// as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
|
// as in [2] we consider a first order propagation that can be seen as a
|
||||||
/* --------------------------------------------------------------------------------------------*/
|
// prediction phase in EKF
|
||||||
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
|
|
||||||
// NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
|
// propagate uncertainty
|
||||||
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
|
// TODO(frank): use noiseModel routine so we can have arbitrary noise models.
|
||||||
#ifdef OLD_JACOBIAN_CALCULATION
|
const Matrix3& aCov = p().accelerometerCovariance;
|
||||||
Matrix9 G;
|
const Matrix3& wCov = p().gyroscopeCovariance;
|
||||||
G << G1, Gi, G2;
|
const Matrix3& iCov = p().integrationCovariance;
|
||||||
Matrix9 Cov;
|
|
||||||
Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3,
|
// (1/dt) allows to pass from continuous time noise to discrete time noise
|
||||||
Z_3x3, p().integrationCovariance * dt, Z_3x3,
|
preintMeasCov_ = A * preintMeasCov_ * A.transpose();
|
||||||
Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
|
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
|
||||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
|
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
|
||||||
#else
|
|
||||||
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
|
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
|
||||||
+ Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
|
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
|
||||||
+ G1 * (p().accelerometerCovariance / dt) * G1.transpose()
|
preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose();
|
||||||
+ G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
||||||
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
|
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
|
||||||
const Matrix3& measuredOmegaCovariance,
|
const Matrix3& measuredOmegaCovariance,
|
||||||
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
|
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
|
||||||
if (!use2ndOrderIntegration)
|
if (!use2ndOrderIntegration)
|
||||||
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
|
||||||
biasHat_ = biasHat;
|
biasHat_ = biasHat;
|
||||||
boost::shared_ptr<Params> p = Params::MakeSharedD();
|
boost::shared_ptr<Params> p = Params::MakeSharedD();
|
||||||
p->gyroscopeCovariance = measuredOmegaCovariance;
|
p->gyroscopeCovariance = measuredOmegaCovariance;
|
||||||
|
|
@ -100,7 +94,6 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
|
||||||
resetIntegration();
|
resetIntegration();
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
void PreintegratedImuMeasurements::integrateMeasurement(
|
void PreintegratedImuMeasurements::integrateMeasurement(
|
||||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||||
boost::optional<Pose3> body_P_sensor) {
|
boost::optional<Pose3> body_P_sensor) {
|
||||||
|
|
@ -108,6 +101,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
|
||||||
p_->body_P_sensor = body_P_sensor;
|
p_->body_P_sensor = body_P_sensor;
|
||||||
integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
// ImuFactor methods
|
// ImuFactor methods
|
||||||
|
|
@ -119,9 +113,18 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
|
NonlinearFactor::shared_ptr ImuFactor::clone() const {
|
||||||
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
return boost::static_pointer_cast<NonlinearFactor>(
|
||||||
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
NonlinearFactor::shared_ptr(new This(*this)));
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
std::ostream& operator<<(std::ostream& os, const ImuFactor& f) {
|
||||||
|
os << " preintegrated measurements:\n" << f._PIM_;
|
||||||
|
;
|
||||||
|
// Print standard deviations on covariance only
|
||||||
|
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
|
||||||
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
|
@ -130,18 +133,15 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
|
||||||
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
|
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
|
||||||
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
|
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
|
||||||
<< ")\n";
|
<< ")\n";
|
||||||
Base::print("");
|
cout << *this << endl;
|
||||||
_PIM_.print(" preintegrated measurements:");
|
|
||||||
// Print standard deviations on covariance only
|
|
||||||
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose()
|
|
||||||
<< endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
|
bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
|
||||||
const This *e = dynamic_cast<const This*>(&other);
|
const This *e = dynamic_cast<const This*>(&other);
|
||||||
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
|
const bool base = Base::equals(*e, tol);
|
||||||
&& Base::equals(*e, tol);
|
const bool pim = _PIM_.equals(e->_PIM_, tol);
|
||||||
|
return e != nullptr && base && pim;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
|
@ -155,14 +155,62 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
PreintegratedImuMeasurements ImuFactor::Merge(
|
||||||
|
const PreintegratedImuMeasurements& pim01,
|
||||||
|
const PreintegratedImuMeasurements& pim12) {
|
||||||
|
if (!pim01.matchesParamsWith(pim12))
|
||||||
|
throw std::domain_error(
|
||||||
|
"Cannot merge PreintegratedImuMeasurements with different params");
|
||||||
|
|
||||||
|
if (pim01.params()->body_P_sensor)
|
||||||
|
throw std::domain_error(
|
||||||
|
"Cannot merge PreintegratedImuMeasurements with sensor pose yet");
|
||||||
|
|
||||||
|
// the bias for the merged factor will be the bias from 01
|
||||||
|
PreintegratedImuMeasurements pim02 = pim01;
|
||||||
|
|
||||||
|
Matrix9 H1, H2;
|
||||||
|
pim02.mergeWith(pim12, &H1, &H2);
|
||||||
|
|
||||||
|
pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() +
|
||||||
|
H2 * pim12.preintMeasCov_ * H2.transpose();
|
||||||
|
|
||||||
|
return pim02;
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
|
||||||
|
const shared_ptr& f12) {
|
||||||
|
// IMU bias keys must be the same.
|
||||||
|
if (f01->key5() != f12->key5())
|
||||||
|
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
|
||||||
|
|
||||||
|
// expect intermediate pose, velocity keys to matchup.
|
||||||
|
if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
|
||||||
|
throw std::domain_error(
|
||||||
|
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
|
||||||
|
|
||||||
|
// return new factor
|
||||||
|
auto pim02 =
|
||||||
|
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
|
||||||
|
return boost::make_shared<ImuFactor>(f01->key1(), // P0
|
||||||
|
f01->key2(), // V0
|
||||||
|
f12->key3(), // P2
|
||||||
|
f12->key4(), // V2
|
||||||
|
f01->key5(), // B
|
||||||
|
pim02);
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||||
const PreintegratedMeasurements& pim, const Vector3& n_gravity,
|
const PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
|
||||||
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
|
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
|
||||||
const bool use2ndOrderCoriolis) :
|
const bool use2ndOrderCoriolis) :
|
||||||
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
|
||||||
pose_j, vel_j, bias), _PIM_(pim) {
|
pose_j, vel_j, bias), _PIM_(pim) {
|
||||||
boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
|
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
|
||||||
PreintegratedMeasurements::Params>(pim.p());
|
PreintegratedImuMeasurements::Params>(pim.p());
|
||||||
p->n_gravity = n_gravity;
|
p->n_gravity = n_gravity;
|
||||||
p->omegaCoriolis = omegaCoriolis;
|
p->omegaCoriolis = omegaCoriolis;
|
||||||
p->body_P_sensor = body_P_sensor;
|
p->body_P_sensor = body_P_sensor;
|
||||||
|
|
@ -170,10 +218,9 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||||
_PIM_.p_ = p;
|
_PIM_.p_ = p;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
|
||||||
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
||||||
PreintegratedMeasurements& pim, const Vector3& n_gravity,
|
PreintegratedImuMeasurements& pim, const Vector3& n_gravity,
|
||||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
|
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
|
||||||
// use deprecated predict
|
// use deprecated predict
|
||||||
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
|
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
|
||||||
|
|
@ -181,5 +228,8 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
pose_j = pvb.pose;
|
pose_j = pvb.pose;
|
||||||
vel_j = pvb.velocity;
|
vel_j = pvb.velocity;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
|
||||||
} // namespace gtsam
|
}
|
||||||
|
// namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -71,7 +71,9 @@ protected:
|
||||||
///< (first-order propagation from *measurementCovariance*).
|
///< (first-order propagation from *measurementCovariance*).
|
||||||
|
|
||||||
/// Default constructor for serialization
|
/// Default constructor for serialization
|
||||||
PreintegratedImuMeasurements() {}
|
PreintegratedImuMeasurements() {
|
||||||
|
preintMeasCov_.setZero();
|
||||||
|
}
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
@ -80,12 +82,22 @@ public:
|
||||||
* @param bias Current estimate of acceleration and rotation rate biases
|
* @param bias Current estimate of acceleration and rotation rate biases
|
||||||
* @param p Parameters, typically fixed in a single application
|
* @param p Parameters, typically fixed in a single application
|
||||||
*/
|
*/
|
||||||
PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
|
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
|
||||||
const imuBias::ConstantBias& biasHat) :
|
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
|
||||||
PreintegrationBase(p, biasHat) {
|
PreintegrationBase(p, biasHat) {
|
||||||
preintMeasCov_.setZero();
|
preintMeasCov_.setZero();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Construct preintegrated directly from members: base class and preintMeasCov
|
||||||
|
* @param base PreintegrationBase instance
|
||||||
|
* @param preintMeasCov Covariance matrix used in noise model.
|
||||||
|
*/
|
||||||
|
PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
|
||||||
|
: PreintegrationBase(base),
|
||||||
|
preintMeasCov_(preintMeasCov) {
|
||||||
|
}
|
||||||
|
|
||||||
/// print
|
/// print
|
||||||
void print(const std::string& s = "Preintegrated Measurements:") const;
|
void print(const std::string& s = "Preintegrated Measurements:") const;
|
||||||
|
|
||||||
|
|
@ -108,6 +120,7 @@ public:
|
||||||
/// Return pre-integrated measurement covariance
|
/// Return pre-integrated measurement covariance
|
||||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
/// @deprecated constructor
|
/// @deprecated constructor
|
||||||
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
|
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
|
||||||
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
|
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
|
||||||
|
|
@ -121,6 +134,7 @@ public:
|
||||||
void integrateMeasurement(const Vector3& measuredAcc,
|
void integrateMeasurement(const Vector3& measuredAcc,
|
||||||
const Vector3& measuredOmega, double dt,
|
const Vector3& measuredOmega, double dt,
|
||||||
boost::optional<Pose3> body_P_sensor);
|
boost::optional<Pose3> body_P_sensor);
|
||||||
|
#endif
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
@ -187,14 +201,13 @@ public:
|
||||||
/// @return a deep copy of this factor
|
/// @return a deep copy of this factor
|
||||||
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
|
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
|
||||||
|
|
||||||
/** implement functions needed for Testable */
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
/// print
|
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
|
||||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
|
||||||
DefaultKeyFormatter) const;
|
DefaultKeyFormatter) const;
|
||||||
|
|
||||||
/// equals
|
|
||||||
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
|
||||||
|
/// @}
|
||||||
|
|
||||||
/** Access the preintegrated measurements. */
|
/** Access the preintegrated measurements. */
|
||||||
|
|
||||||
|
|
@ -212,10 +225,19 @@ public:
|
||||||
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
|
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
|
||||||
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
|
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
|
||||||
|
|
||||||
|
/// Merge two pre-integrated measurement classes
|
||||||
|
static PreintegratedImuMeasurements Merge(
|
||||||
|
const PreintegratedImuMeasurements& pim01,
|
||||||
|
const PreintegratedImuMeasurements& pim12);
|
||||||
|
|
||||||
|
/// Merge two factors
|
||||||
|
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
/// @deprecated typename
|
/// @deprecated typename
|
||||||
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
|
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
|
||||||
|
|
||||||
/// @deprecated constructor, in the new one gravity, coriolis settings are in Params
|
/// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams
|
||||||
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
|
||||||
const PreintegratedMeasurements& preintegratedMeasurements,
|
const PreintegratedMeasurements& preintegratedMeasurements,
|
||||||
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
||||||
|
|
@ -223,11 +245,12 @@ public:
|
||||||
const bool use2ndOrderCoriolis = false);
|
const bool use2ndOrderCoriolis = false);
|
||||||
|
|
||||||
/// @deprecated use PreintegrationBase::predict,
|
/// @deprecated use PreintegrationBase::predict,
|
||||||
/// in the new one gravity, coriolis settings are in Params
|
/// in the new one gravity, coriolis settings are in PreintegrationParams
|
||||||
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
|
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
|
||||||
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
|
||||||
PreintegratedMeasurements& pim, const Vector3& n_gravity,
|
PreintegratedMeasurements& pim, const Vector3& n_gravity,
|
||||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
||||||
|
#endif
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
|
@ -242,7 +265,10 @@ private:
|
||||||
};
|
};
|
||||||
// class ImuFactor
|
// class ImuFactor
|
||||||
|
|
||||||
/// traits
|
template <>
|
||||||
template<> struct traits<ImuFactor> : public Testable<ImuFactor> {};
|
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
|
||||||
|
|
||||||
|
template <>
|
||||||
|
struct traits<ImuFactor> : public Testable<ImuFactor> {};
|
||||||
|
|
||||||
} /// namespace gtsam
|
} /// namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -74,11 +74,17 @@ Matrix7 NavState::matrix() const {
|
||||||
return T;
|
return T;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
ostream& operator<<(ostream& os, const NavState& state) {
|
||||||
|
os << "R:" << state.attitude();
|
||||||
|
os << "p:" << state.position() << endl;
|
||||||
|
os << "v:" << Point3(state.velocity()) << endl;
|
||||||
|
return os;
|
||||||
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void NavState::print(const string& s) const {
|
void NavState::print(const string& s) const {
|
||||||
attitude().print(s + ".R");
|
cout << s << *this << endl;
|
||||||
position().print(s + ".p");
|
|
||||||
gtsam::print((Vector) v_, s + ".v");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -75,18 +75,9 @@ public:
|
||||||
/// @name Component Access
|
/// @name Component Access
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
inline const Rot3& attitude() const {
|
const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
|
||||||
return R_;
|
const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
|
||||||
}
|
const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
|
||||||
inline const Point3& position() const {
|
|
||||||
return t_;
|
|
||||||
}
|
|
||||||
inline const Velocity3& velocity() const {
|
|
||||||
return v_;
|
|
||||||
}
|
|
||||||
const Rot3& attitude(OptionalJacobian<3, 9> H) const;
|
|
||||||
const Point3& position(OptionalJacobian<3, 9> H) const;
|
|
||||||
const Velocity3& velocity(OptionalJacobian<3, 9> H) const;
|
|
||||||
|
|
||||||
const Pose3 pose() const {
|
const Pose3 pose() const {
|
||||||
return Pose3(attitude(), position());
|
return Pose3(attitude(), position());
|
||||||
|
|
@ -124,6 +115,9 @@ public:
|
||||||
/// @name Testable
|
/// @name Testable
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
|
/// Output stream operator
|
||||||
|
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state);
|
||||||
|
|
||||||
/// print
|
/// print
|
||||||
void print(const std::string& s = "") const;
|
void print(const std::string& s = "") const;
|
||||||
|
|
||||||
|
|
@ -229,6 +223,8 @@ public:
|
||||||
false, OptionalJacobian<9, 9> H1 = boost::none,
|
false, OptionalJacobian<9, 9> H1 = boost::none,
|
||||||
OptionalJacobian<9, 9> H2 = boost::none) const;
|
OptionalJacobian<9, 9> H2 = boost::none) const;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/// @{
|
/// @{
|
||||||
/// serialization
|
/// serialization
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -29,12 +29,26 @@ void PreintegratedRotation::Params::print(const string& s) const {
|
||||||
cout << s << endl;
|
cout << s << endl;
|
||||||
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
|
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
|
||||||
if (omegaCoriolis)
|
if (omegaCoriolis)
|
||||||
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
|
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
|
||||||
<< endl;
|
|
||||||
if (body_P_sensor)
|
if (body_P_sensor)
|
||||||
body_P_sensor->print("body_P_sensor");
|
body_P_sensor->print("body_P_sensor");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool PreintegratedRotation::Params::equals(
|
||||||
|
const PreintegratedRotation::Params& other, double tol) const {
|
||||||
|
if (body_P_sensor) {
|
||||||
|
if (!other.body_P_sensor
|
||||||
|
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (omegaCoriolis) {
|
||||||
|
if (!other.omegaCoriolis
|
||||||
|
|| !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol))
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol);
|
||||||
|
}
|
||||||
|
|
||||||
void PreintegratedRotation::resetIntegration() {
|
void PreintegratedRotation::resetIntegration() {
|
||||||
deltaTij_ = 0.0;
|
deltaTij_ = 0.0;
|
||||||
deltaRij_ = Rot3();
|
deltaRij_ = Rot3();
|
||||||
|
|
@ -42,14 +56,14 @@ void PreintegratedRotation::resetIntegration() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void PreintegratedRotation::print(const string& s) const {
|
void PreintegratedRotation::print(const string& s) const {
|
||||||
cout << s << endl;
|
cout << s;
|
||||||
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
||||||
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
|
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
||||||
double tol) const {
|
double tol) const {
|
||||||
return deltaRij_.equals(other.deltaRij_, tol)
|
return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol)
|
||||||
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
&& fabs(deltaTij_ - other.deltaTij_) < tol
|
||||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
||||||
}
|
}
|
||||||
|
|
@ -76,12 +90,18 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
|
||||||
}
|
}
|
||||||
|
|
||||||
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
||||||
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
|
const Vector3& biasHat, double deltaT,
|
||||||
Matrix3* F) {
|
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
|
||||||
|
OptionalJacobian<3, 3> F) {
|
||||||
|
Matrix3 D_incrR_integratedOmega;
|
||||||
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
|
||||||
D_incrR_integratedOmega);
|
D_incrR_integratedOmega);
|
||||||
|
|
||||||
|
// If asked, pass first derivative as well
|
||||||
|
if (optional_D_incrR_integratedOmega) {
|
||||||
|
*optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
|
||||||
|
}
|
||||||
|
|
||||||
// Update deltaTij and rotation
|
// Update deltaTij and rotation
|
||||||
deltaTij_ += deltaT;
|
deltaTij_ += deltaT;
|
||||||
deltaRij_ = deltaRij_.compose(incrR, F);
|
deltaRij_ = deltaRij_.compose(incrR, F);
|
||||||
|
|
@ -89,7 +109,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
|
||||||
// Update Jacobian
|
// Update Jacobian
|
||||||
const Matrix3 incrRt = incrR.transpose();
|
const Matrix3 incrRt = incrR.transpose();
|
||||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
||||||
- *D_incrR_integratedOmega * deltaT;
|
- D_incrR_integratedOmega * deltaT;
|
||||||
}
|
}
|
||||||
|
|
||||||
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -32,22 +32,20 @@ namespace gtsam {
|
||||||
* It includes the definitions of the preintegrated rotation.
|
* It includes the definitions of the preintegrated rotation.
|
||||||
*/
|
*/
|
||||||
class PreintegratedRotation {
|
class PreintegratedRotation {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
/// Parameters for pre-integration:
|
/// Parameters for pre-integration:
|
||||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||||
struct Params {
|
struct Params {
|
||||||
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
|
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
|
||||||
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
|
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
|
||||||
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
|
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
|
||||||
|
|
||||||
Params() :
|
Params() : gyroscopeCovariance(I_3x3) {}
|
||||||
gyroscopeCovariance(I_3x3) {
|
|
||||||
}
|
|
||||||
|
|
||||||
virtual void print(const std::string& s) const;
|
virtual void print(const std::string& s) const;
|
||||||
|
virtual bool equals(const Params& other, double tol=1e-9) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template<class ARCHIVE>
|
||||||
|
|
@ -59,30 +57,51 @@ public:
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
double deltaTij_; ///< Time interval from i to j
|
|
||||||
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
|
||||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
|
||||||
|
|
||||||
/// Parameters
|
/// Parameters
|
||||||
boost::shared_ptr<Params> p_;
|
boost::shared_ptr<Params> p_;
|
||||||
|
|
||||||
/// Default constructor for serialization
|
double deltaTij_; ///< Time interval from i to j
|
||||||
PreintegratedRotation() {
|
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
||||||
}
|
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||||
|
|
||||||
|
/// Default constructor for serialization
|
||||||
|
PreintegratedRotation() {}
|
||||||
|
|
||||||
|
public:
|
||||||
|
/// @name Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
public:
|
|
||||||
/// Default constructor, resets integration to zero
|
/// Default constructor, resets integration to zero
|
||||||
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
|
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
|
||||||
p_(p) {
|
|
||||||
resetIntegration();
|
resetIntegration();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Explicit initialization of all class members
|
||||||
|
PreintegratedRotation(const boost::shared_ptr<Params>& p,
|
||||||
|
double deltaTij, const Rot3& deltaRij,
|
||||||
|
const Matrix3& delRdelBiasOmega)
|
||||||
|
: p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Basic utilities
|
||||||
|
/// @{
|
||||||
|
|
||||||
/// Re-initialize PreintegratedMeasurements
|
/// Re-initialize PreintegratedMeasurements
|
||||||
void resetIntegration();
|
void resetIntegration();
|
||||||
|
|
||||||
|
/// check parameters equality: checks whether shared pointer points to same Params object.
|
||||||
|
bool matchesParamsWith(const PreintegratedRotation& other) const {
|
||||||
|
return p_ == other.p_;
|
||||||
|
}
|
||||||
|
/// @}
|
||||||
|
|
||||||
/// @name Access instance variables
|
/// @name Access instance variables
|
||||||
/// @{
|
/// @{
|
||||||
|
const boost::shared_ptr<Params>& params() const {
|
||||||
|
return p_;
|
||||||
|
}
|
||||||
const double& deltaTij() const {
|
const double& deltaTij() const {
|
||||||
return deltaTij_;
|
return deltaTij_;
|
||||||
}
|
}
|
||||||
|
|
@ -96,41 +115,47 @@ public:
|
||||||
|
|
||||||
/// @name Testable
|
/// @name Testable
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
void print(const std::string& s) const;
|
void print(const std::string& s) const;
|
||||||
bool equals(const PreintegratedRotation& other, double tol) const;
|
bool equals(const PreintegratedRotation& other, double tol) const;
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
||||||
|
/// @name Main functionality
|
||||||
|
/// @{
|
||||||
|
|
||||||
/// Take the gyro measurement, correct it using the (constant) bias estimate
|
/// Take the gyro measurement, correct it using the (constant) bias estimate
|
||||||
/// and possibly the sensor pose, and then integrate it forward in time to yield
|
/// and possibly the sensor pose, and then integrate it forward in time to yield
|
||||||
/// an incremental rotation.
|
/// an incremental rotation.
|
||||||
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
|
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||||
double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
|
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
|
||||||
|
|
||||||
/// Calculate an incremental rotation given the gyro measurement and a time interval,
|
/// Calculate an incremental rotation given the gyro measurement and a time interval,
|
||||||
/// and update both deltaTij_ and deltaRij_.
|
/// and update both deltaTij_ and deltaRij_.
|
||||||
void integrateMeasurement(const Vector3& measuredOmega,
|
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
||||||
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
|
OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
|
||||||
Matrix3* F);
|
OptionalJacobian<3, 3> F = boost::none);
|
||||||
|
|
||||||
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
|
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
|
||||||
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
||||||
OptionalJacobian<3, 3> H = boost::none) const;
|
OptionalJacobian<3, 3> H = boost::none) const;
|
||||||
|
|
||||||
/// Integrate coriolis correction in body frame rot_i
|
/// Integrate coriolis correction in body frame rot_i
|
||||||
Vector3 integrateCoriolis(const Rot3& rot_i) const;
|
Vector3 integrateCoriolis(const Rot3& rot_i) const;
|
||||||
|
|
||||||
private:
|
/// @}
|
||||||
|
|
||||||
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
friend class boost::serialization::access;
|
friend class boost::serialization::access;
|
||||||
template<class ARCHIVE>
|
template <class ARCHIVE>
|
||||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT
|
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
|
||||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
ar& BOOST_SERIALIZATION_NVP(p_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
ar& BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
|
ar& BOOST_SERIALIZATION_NVP(deltaRij_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace gtsam
|
template <>
|
||||||
|
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
|
||||||
|
|
||||||
|
} /// namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -20,6 +20,7 @@
|
||||||
**/
|
**/
|
||||||
|
|
||||||
#include "PreintegrationBase.h"
|
#include "PreintegrationBase.h"
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
#include <boost/make_shared.hpp>
|
#include <boost/make_shared.hpp>
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
|
|
@ -27,229 +28,254 @@ using namespace std;
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegrationBase::Params::print(const string& s) const {
|
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
|
||||||
PreintegratedRotation::Params::print(s);
|
const Bias& biasHat)
|
||||||
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
|
: p_(p), biasHat_(biasHat), deltaTij_(0.0) {
|
||||||
<< endl;
|
resetIntegration();
|
||||||
cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
|
|
||||||
<< endl;
|
|
||||||
if (omegaCoriolis && use2ndOrderCoriolis)
|
|
||||||
cout << "Using 2nd-order Coriolis" << endl;
|
|
||||||
if (body_P_sensor)
|
|
||||||
body_P_sensor->print(" ");
|
|
||||||
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegrationBase::resetIntegration() {
|
void PreintegrationBase::resetIntegration() {
|
||||||
deltaTij_ = 0.0;
|
deltaTij_ = 0.0;
|
||||||
deltaXij_ = NavState();
|
preintegrated_.setZero();
|
||||||
delRdelBiasOmega_ = Z_3x3;
|
preintegrated_H_biasAcc_.setZero();
|
||||||
delPdelBiasAcc_ = Z_3x3;
|
preintegrated_H_biasOmega_.setZero();
|
||||||
delPdelBiasOmega_ = Z_3x3;
|
}
|
||||||
delVdelBiasAcc_ = Z_3x3;
|
|
||||||
delVdelBiasOmega_ = Z_3x3;
|
//------------------------------------------------------------------------------
|
||||||
|
ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
|
||||||
|
os << " deltaTij " << pim.deltaTij_ << endl;
|
||||||
|
os << " deltaRij " << Point3(pim.theta()) << endl;
|
||||||
|
os << " deltaPij " << Point3(pim.deltaPij()) << endl;
|
||||||
|
os << " deltaVij " << Point3(pim.deltaVij()) << endl;
|
||||||
|
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
|
||||||
|
os << " acc_bias " << Point3(pim.biasHat_.accelerometer()) << endl;
|
||||||
|
return os;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegrationBase::print(const string& s) const {
|
void PreintegrationBase::print(const string& s) const {
|
||||||
cout << s << endl;
|
cout << s << *this << endl;
|
||||||
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
|
||||||
cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl;
|
|
||||||
cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl;
|
|
||||||
cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl;
|
|
||||||
biasHat_.print(" biasHat");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
bool PreintegrationBase::equals(const PreintegrationBase& other,
|
bool PreintegrationBase::equals(const PreintegrationBase& other,
|
||||||
double tol) const {
|
double tol) const {
|
||||||
return fabs(deltaTij_ - other.deltaTij_) < tol
|
const bool params_match = p_->equals(*other.p_, tol);
|
||||||
&& deltaXij_.equals(other.deltaXij_, tol)
|
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
|
||||||
&& biasHat_.equals(other.biasHat_, tol)
|
&& biasHat_.equals(other.biasHat_, tol)
|
||||||
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
|
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
|
||||||
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
|
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
|
||||||
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
|
&& equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
|
||||||
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
|
|
||||||
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
|
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
|
||||||
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
|
||||||
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
|
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc,
|
||||||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
|
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega,
|
||||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
|
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega) const {
|
||||||
|
assert(p().body_P_sensor);
|
||||||
// Correct for bias in the sensor frame
|
|
||||||
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
|
|
||||||
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
|
|
||||||
|
|
||||||
// Compensate for sensor-body displacement if needed: we express the quantities
|
// Compensate for sensor-body displacement if needed: we express the quantities
|
||||||
// (originally in the IMU frame) into the body frame
|
// (originally in the IMU frame) into the body frame
|
||||||
// Equations below assume the "body" frame is the CG
|
// Equations below assume the "body" frame is the CG
|
||||||
if (p().body_P_sensor) {
|
|
||||||
// Correct omega to rotation rate vector in the body frame
|
|
||||||
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
|
|
||||||
j_correctedOmega = bRs * j_correctedOmega;
|
|
||||||
|
|
||||||
// Correct acceleration
|
// Get sensor to body rotation matrix
|
||||||
j_correctedAcc = bRs * j_correctedAcc;
|
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
|
||||||
|
|
||||||
// Jacobians
|
// Convert angular velocity and acceleration from sensor to body frame
|
||||||
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
|
Vector3 correctedAcc = bRs * unbiasedAcc;
|
||||||
if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
|
const Vector3 correctedOmega = bRs * unbiasedOmega;
|
||||||
if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
|
|
||||||
|
|
||||||
// Centrifugal acceleration
|
// Jacobians
|
||||||
const Vector3 b_arm = p().body_P_sensor->translation().vector();
|
if (D_correctedAcc_unbiasedAcc) *D_correctedAcc_unbiasedAcc = bRs;
|
||||||
if (!b_arm.isZero()) {
|
if (D_correctedAcc_unbiasedOmega) *D_correctedAcc_unbiasedOmega = Z_3x3;
|
||||||
// Subtract out the the centripetal acceleration from the measured one
|
if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs;
|
||||||
// to get linear acceleration vector in the body frame:
|
|
||||||
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
|
// Centrifugal acceleration
|
||||||
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
|
const Vector3 b_arm = p().body_P_sensor->translation().vector();
|
||||||
j_correctedAcc -= body_Omega_body * b_velocity_bs;
|
if (!b_arm.isZero()) {
|
||||||
// Update derivative: centrifugal causes the correlation between acc and omega!!!
|
// Subtract out the the centripetal acceleration from the unbiased one
|
||||||
if (D_correctedAcc_measuredOmega) {
|
// to get linear acceleration vector in the body frame:
|
||||||
double wdp = j_correctedOmega.dot(b_arm);
|
const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
|
||||||
*D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
|
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
|
||||||
+ j_correctedOmega * b_arm.transpose()) * bRs.matrix()
|
correctedAcc -= body_Omega_body * b_velocity_bs;
|
||||||
+ 2 * b_arm * j_measuredOmega.transpose();
|
|
||||||
}
|
// Update derivative: centrifugal causes the correlation between acc and omega!!!
|
||||||
|
if (D_correctedAcc_unbiasedOmega) {
|
||||||
|
double wdp = correctedOmega.dot(b_arm);
|
||||||
|
*D_correctedAcc_unbiasedOmega = -(diag(Vector3::Constant(wdp))
|
||||||
|
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
|
||||||
|
+ 2 * b_arm * unbiasedOmega.transpose();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Do update in one fell swoop
|
return make_pair(correctedAcc, correctedOmega);
|
||||||
return make_pair(j_correctedAcc, j_correctedOmega);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
|
// See extensive discussion in ImuFactor.lyx
|
||||||
const Vector3& j_measuredOmega, const double dt,
|
Vector9 PreintegrationBase::UpdatePreintegrated(
|
||||||
OptionalJacobian<9, 9> D_updated_current,
|
const Vector3& a_body, const Vector3& w_body, double dt,
|
||||||
OptionalJacobian<9, 3> D_updated_measuredAcc,
|
const Vector9& preintegrated, OptionalJacobian<9, 9> A,
|
||||||
OptionalJacobian<9, 3> D_updated_measuredOmega) const {
|
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
|
||||||
|
const auto theta = preintegrated.segment<3>(0);
|
||||||
|
const auto position = preintegrated.segment<3>(3);
|
||||||
|
const auto velocity = preintegrated.segment<3>(6);
|
||||||
|
|
||||||
Vector3 j_correctedAcc, j_correctedOmega;
|
// This functor allows for saving computation when exponential map and its
|
||||||
Matrix3 D_correctedAcc_measuredAcc, //
|
// derivatives are needed at the same location in so<3>
|
||||||
D_correctedAcc_measuredOmega, //
|
so3::DexpFunctor local(theta);
|
||||||
D_correctedOmega_measuredOmega;
|
|
||||||
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
|
// Calculate exact mean propagation
|
||||||
boost::tie(j_correctedAcc, j_correctedOmega) =
|
Matrix3 w_tangent_H_theta, invH;
|
||||||
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
|
const Vector3 w_tangent = // angular velocity mapped back to tangent space
|
||||||
(needDerivs ? &D_correctedAcc_measuredAcc : 0),
|
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
|
||||||
(needDerivs ? &D_correctedAcc_measuredOmega : 0),
|
const SO3 R = local.expmap();
|
||||||
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
|
const Vector3 a_nav = R * a_body;
|
||||||
// Do update in one fell swoop
|
const double dt22 = 0.5 * dt * dt;
|
||||||
Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
|
|
||||||
NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
|
Vector9 preintegratedPlus;
|
||||||
(needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
|
preintegratedPlus << // new preintegrated vector:
|
||||||
(needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
|
theta + w_tangent* dt, // theta
|
||||||
if (needDerivs) {
|
position + velocity* dt + a_nav* dt22, // position
|
||||||
*D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
|
velocity + a_nav* dt; // velocity
|
||||||
*D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
|
|
||||||
if (!p().body_P_sensor->translation().vector().isZero()) {
|
if (A) {
|
||||||
*D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
|
// Exact derivative of R*a with respect to theta:
|
||||||
}
|
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
|
||||||
|
|
||||||
|
A->setIdentity();
|
||||||
|
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
|
||||||
|
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
|
||||||
|
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
|
||||||
|
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
|
||||||
}
|
}
|
||||||
return updated;
|
if (B) {
|
||||||
|
B->block<3, 3>(0, 0) = Z_3x3;
|
||||||
|
B->block<3, 3>(3, 0) = R * dt22;
|
||||||
|
B->block<3, 3>(6, 0) = R * dt;
|
||||||
|
}
|
||||||
|
if (C) {
|
||||||
|
C->block<3, 3>(0, 0) = invH * dt;
|
||||||
|
C->block<3, 3>(3, 0) = Z_3x3;
|
||||||
|
C->block<3, 3>(6, 0) = Z_3x3;
|
||||||
|
}
|
||||||
|
|
||||||
|
return preintegratedPlus;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
void PreintegrationBase::update(const Vector3& j_measuredAcc,
|
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
|
||||||
const Vector3& j_measuredOmega, const double dt,
|
const Vector3& measuredOmega,
|
||||||
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
|
double dt, Matrix9* A,
|
||||||
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
|
Matrix93* B, Matrix93* C) {
|
||||||
|
// Correct for bias in the sensor frame
|
||||||
|
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
|
||||||
|
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
|
||||||
|
|
||||||
// Save current rotation for updating Jacobians
|
// Possibly correct for sensor pose
|
||||||
const Rot3 oldRij = deltaXij_.attitude();
|
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
|
||||||
|
if (p().body_P_sensor)
|
||||||
|
boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega,
|
||||||
|
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
|
||||||
|
|
||||||
// Do update
|
// Do update
|
||||||
deltaTij_ += dt;
|
deltaTij_ += dt;
|
||||||
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
|
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
|
||||||
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
|
|
||||||
|
|
||||||
// Update Jacobians
|
if (p().body_P_sensor) {
|
||||||
// TODO(frank): we are repeating some computation here: accessible in F ?
|
// More complicated derivatives in case of non-trivial sensor pose
|
||||||
Vector3 j_correctedAcc, j_correctedOmega;
|
*C *= D_correctedOmega_omega;
|
||||||
boost::tie(j_correctedAcc, j_correctedOmega) =
|
if (!p().body_P_sensor->translation().vector().isZero())
|
||||||
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
|
*C += *B* D_correctedAcc_omega;
|
||||||
|
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
|
||||||
|
}
|
||||||
|
|
||||||
Matrix3 D_acc_R;
|
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
|
||||||
oldRij.rotate(j_correctedAcc, D_acc_R);
|
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
|
||||||
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
|
|
||||||
|
|
||||||
const Vector3 integratedOmega = j_correctedOmega * dt;
|
// D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias
|
||||||
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
|
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
|
||||||
const Matrix3 incrRt = incrR.transpose();
|
|
||||||
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
|
|
||||||
- *D_incrR_integratedOmega * dt;
|
|
||||||
|
|
||||||
double dt22 = 0.5 * dt * dt;
|
|
||||||
const Matrix3 dRij = oldRij.matrix(); // expensive
|
|
||||||
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
|
|
||||||
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
|
|
||||||
delVdelBiasAcc_ += -dRij * dt;
|
|
||||||
delVdelBiasOmega_ += D_acc_biasOmega * dt;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
|
||||||
|
const Vector3& measuredOmega,
|
||||||
|
double dt) {
|
||||||
|
// NOTE(frank): integrateMeasuremtn always needs to compute the derivatives,
|
||||||
|
// even when not of interest to the caller. Provide scratch space here.
|
||||||
|
Matrix9 A;
|
||||||
|
Matrix93 B, C;
|
||||||
|
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
|
||||||
|
}
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
Vector9 PreintegrationBase::biasCorrectedDelta(
|
Vector9 PreintegrationBase::biasCorrectedDelta(
|
||||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
|
||||||
// Correct deltaRij, derivative is delRdelBiasOmega_
|
// We correct for a change between bias_i and the biasHat_ used to integrate
|
||||||
|
// This is a simple linear correction with obvious derivatives
|
||||||
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
|
||||||
Matrix3 D_correctedRij_bias;
|
const Vector9 biasCorrected =
|
||||||
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
|
preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() +
|
||||||
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
|
preintegrated_H_biasOmega_ * biasIncr.gyroscope();
|
||||||
H ? &D_correctedRij_bias : 0);
|
|
||||||
if (H)
|
|
||||||
D_correctedRij_bias *= delRdelBiasOmega_;
|
|
||||||
|
|
||||||
Vector9 xi;
|
|
||||||
Matrix3 D_dR_correctedRij;
|
|
||||||
// TODO(frank): could line below be simplified? It is equivalent to
|
|
||||||
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
|
|
||||||
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
|
|
||||||
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
|
|
||||||
+ delPdelBiasOmega_ * biasIncr.gyroscope();
|
|
||||||
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
|
|
||||||
+ delVdelBiasOmega_ * biasIncr.gyroscope();
|
|
||||||
|
|
||||||
if (H) {
|
if (H) {
|
||||||
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
|
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
|
||||||
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
|
|
||||||
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
|
|
||||||
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
|
|
||||||
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
|
|
||||||
}
|
}
|
||||||
return xi;
|
return biasCorrected;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
NavState PreintegrationBase::predict(const NavState& state_i,
|
NavState PreintegrationBase::predict(const NavState& state_i,
|
||||||
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
|
const imuBias::ConstantBias& bias_i,
|
||||||
OptionalJacobian<9, 6> H2) const {
|
OptionalJacobian<9, 9> H1,
|
||||||
// correct for bias
|
OptionalJacobian<9, 6> H2) const {
|
||||||
|
// TODO(frank): make sure this stuff is still correct
|
||||||
Matrix96 D_biasCorrected_bias;
|
Matrix96 D_biasCorrected_bias;
|
||||||
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
|
Vector9 biasCorrected =
|
||||||
H2 ? &D_biasCorrected_bias : 0);
|
biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0);
|
||||||
|
|
||||||
// integrate on tangent space
|
// Correct for initial velocity and gravity
|
||||||
Matrix9 D_delta_state, D_delta_biasCorrected;
|
Matrix9 D_delta_state, D_delta_biasCorrected;
|
||||||
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
|
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
|
||||||
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
|
p().omegaCoriolis, p().use2ndOrderCoriolis,
|
||||||
H2 ? &D_delta_biasCorrected : 0);
|
H1 ? &D_delta_state : 0,
|
||||||
|
H2 ? &D_delta_biasCorrected : 0);
|
||||||
|
|
||||||
// Use retract to get back to NavState manifold
|
// Use retract to get back to NavState manifold
|
||||||
Matrix9 D_predict_state, D_predict_delta;
|
Matrix9 D_predict_state, D_predict_delta;
|
||||||
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
|
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
|
||||||
if (H1)
|
if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state;
|
||||||
*H1 = D_predict_state + D_predict_delta * D_delta_state;
|
if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias;
|
||||||
if (H2)
|
|
||||||
*H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
|
|
||||||
return state_j;
|
return state_j;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
Vector9 PreintegrationBase::computeError(const NavState& state_i,
|
||||||
|
const NavState& state_j,
|
||||||
|
const imuBias::ConstantBias& bias_i,
|
||||||
|
OptionalJacobian<9, 9> H1,
|
||||||
|
OptionalJacobian<9, 9> H2,
|
||||||
|
OptionalJacobian<9, 6> H3) const {
|
||||||
|
// Predict state at time j
|
||||||
|
Matrix9 D_predict_state_i;
|
||||||
|
Matrix96 D_predict_bias_i;
|
||||||
|
NavState predictedState_j = predict(
|
||||||
|
state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0);
|
||||||
|
|
||||||
|
// Calculate error
|
||||||
|
Matrix9 D_error_state_j, D_error_predict;
|
||||||
|
Vector9 error =
|
||||||
|
state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
|
||||||
|
H1 || H3 ? &D_error_predict : 0);
|
||||||
|
|
||||||
|
if (H1) *H1 << D_error_predict* D_predict_state_i;
|
||||||
|
if (H2) *H2 << D_error_state_j;
|
||||||
|
if (H3) *H3 << D_error_predict* D_predict_bias_i;
|
||||||
|
|
||||||
|
return error;
|
||||||
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
||||||
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||||
|
|
@ -262,43 +288,120 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
||||||
NavState state_i(pose_i, vel_i);
|
NavState state_i(pose_i, vel_i);
|
||||||
NavState state_j(pose_j, vel_j);
|
NavState state_j(pose_j, vel_j);
|
||||||
|
|
||||||
/// Predict state at time j
|
// Predict state at time j
|
||||||
Matrix99 D_predict_state_i;
|
Matrix9 D_error_state_i, D_error_state_j;
|
||||||
Matrix96 D_predict_bias_i;
|
Vector9 error = computeError(state_i, state_j, bias_i,
|
||||||
NavState predictedState_j = predict(state_i, bias_i,
|
H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5);
|
||||||
H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0);
|
|
||||||
|
|
||||||
Matrix9 D_error_state_j, D_error_predict;
|
|
||||||
Vector9 error = state_j.localCoordinates(predictedState_j,
|
|
||||||
H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0);
|
|
||||||
|
|
||||||
// Separate out derivatives in terms of 5 arguments
|
// Separate out derivatives in terms of 5 arguments
|
||||||
// Note that doing so requires special treatment of velocities, as when treated as
|
// Note that doing so requires special treatment of velocities, as when treated as
|
||||||
// separate variables the retract applied will not be the semi-direct product in NavState
|
// separate variables the retract applied will not be the semi-direct product in NavState
|
||||||
// Instead, the velocities in nav are updated using a straight addition
|
// Instead, the velocities in nav are updated using a straight addition
|
||||||
// This is difference is accounted for by the R().transpose calls below
|
// This is difference is accounted for by the R().transpose calls below
|
||||||
if (H1)
|
if (H1) *H1 << D_error_state_i.leftCols<6>();
|
||||||
*H1 << D_error_predict * D_predict_state_i.leftCols<6>();
|
if (H2) *H2 << D_error_state_i.rightCols<3>() * state_i.R().transpose();
|
||||||
if (H2)
|
if (H3) *H3 << D_error_state_j.leftCols<6>();
|
||||||
*H2
|
if (H4) *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose();
|
||||||
<< D_error_predict * D_predict_state_i.rightCols<3>()
|
|
||||||
* state_i.R().transpose();
|
|
||||||
if (H3)
|
|
||||||
*H3 << D_error_state_j.leftCols<6>();
|
|
||||||
if (H4)
|
|
||||||
*H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose();
|
|
||||||
if (H5)
|
|
||||||
*H5 << D_error_predict * D_predict_bias_i;
|
|
||||||
|
|
||||||
return error;
|
return error;
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
// sugar for derivative blocks
|
||||||
|
#define D_R_R(H) (H)->block<3,3>(0,0)
|
||||||
|
#define D_R_t(H) (H)->block<3,3>(0,3)
|
||||||
|
#define D_R_v(H) (H)->block<3,3>(0,6)
|
||||||
|
#define D_t_R(H) (H)->block<3,3>(3,0)
|
||||||
|
#define D_t_t(H) (H)->block<3,3>(3,3)
|
||||||
|
#define D_t_v(H) (H)->block<3,3>(3,6)
|
||||||
|
#define D_v_R(H) (H)->block<3,3>(6,0)
|
||||||
|
#define D_v_t(H) (H)->block<3,3>(6,3)
|
||||||
|
#define D_v_v(H) (H)->block<3,3>(6,6)
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
|
||||||
|
const Vector9& zeta12, double deltaT12,
|
||||||
|
OptionalJacobian<9, 9> H1,
|
||||||
|
OptionalJacobian<9, 9> H2) {
|
||||||
|
const auto t01 = zeta01.segment<3>(0);
|
||||||
|
const auto p01 = zeta01.segment<3>(3);
|
||||||
|
const auto v01 = zeta01.segment<3>(6);
|
||||||
|
|
||||||
|
const auto t12 = zeta12.segment<3>(0);
|
||||||
|
const auto p12 = zeta12.segment<3>(3);
|
||||||
|
const auto v12 = zeta12.segment<3>(6);
|
||||||
|
|
||||||
|
Matrix3 R01_H_t01, R12_H_t12;
|
||||||
|
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
|
||||||
|
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
|
||||||
|
|
||||||
|
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
|
||||||
|
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
|
||||||
|
|
||||||
|
Matrix3 t02_H_R02;
|
||||||
|
Vector9 zeta02;
|
||||||
|
const Matrix3 R = R01.matrix();
|
||||||
|
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
|
||||||
|
p01 + v01 * deltaT12 + R * p12, // position
|
||||||
|
v01 + R * v12; // velocity
|
||||||
|
|
||||||
|
if (H1) {
|
||||||
|
H1->setIdentity();
|
||||||
|
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
|
||||||
|
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
|
||||||
|
D_t_v(H1) = I_3x3 * deltaT12;
|
||||||
|
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (H2) {
|
||||||
|
H2->setZero();
|
||||||
|
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
|
||||||
|
D_t_t(H2) = R;
|
||||||
|
D_v_v(H2) = R;
|
||||||
|
}
|
||||||
|
|
||||||
|
return zeta02;
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
|
||||||
|
Matrix9* H2) {
|
||||||
|
if (!matchesParamsWith(pim12))
|
||||||
|
throw std::domain_error(
|
||||||
|
"Cannot merge pre-integrated measurements with different params");
|
||||||
|
|
||||||
|
if (params()->body_P_sensor)
|
||||||
|
throw std::domain_error(
|
||||||
|
"Cannot merge pre-integrated measurements with sensor pose yet");
|
||||||
|
|
||||||
|
const double& t01 = deltaTij();
|
||||||
|
const double& t12 = pim12.deltaTij();
|
||||||
|
deltaTij_ = t01 + t12;
|
||||||
|
|
||||||
|
Vector9 zeta01 = preintegrated();
|
||||||
|
Vector9 zeta12 = pim12.preintegrated();
|
||||||
|
|
||||||
|
// TODO(frank): adjust zeta12 due to bias difference
|
||||||
|
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
|
||||||
|
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
|
||||||
|
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
|
||||||
|
|
||||||
|
preintegrated_ << PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2);
|
||||||
|
|
||||||
|
preintegrated_H_biasAcc_ =
|
||||||
|
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
|
||||||
|
|
||||||
|
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
|
||||||
|
(*H2) * pim12.preintegrated_H_biasOmega_;
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
||||||
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
|
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
|
||||||
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
const Vector3& n_gravity, const Vector3& omegaCoriolis,
|
||||||
const bool use2ndOrderCoriolis) {
|
const bool use2ndOrderCoriolis) const {
|
||||||
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
|
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
|
||||||
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
|
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
|
||||||
q->n_gravity = n_gravity;
|
q->n_gravity = n_gravity;
|
||||||
q->omegaCoriolis = omegaCoriolis;
|
q->omegaCoriolis = omegaCoriolis;
|
||||||
|
|
@ -306,7 +409,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
|
||||||
p_ = q;
|
p_ = q;
|
||||||
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
|
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
//------------------------------------------------------------------------------
|
//------------------------------------------------------------------------------
|
||||||
|
|
||||||
}/// namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -21,13 +21,16 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/navigation/PreintegratedRotation.h>
|
#include <gtsam/navigation/PreintegrationParams.h>
|
||||||
#include <gtsam/navigation/NavState.h>
|
#include <gtsam/navigation/NavState.h>
|
||||||
#include <gtsam/navigation/ImuBias.h>
|
#include <gtsam/navigation/ImuBias.h>
|
||||||
#include <boost/make_shared.hpp>
|
#include <gtsam/linear/NoiseModel.h>
|
||||||
|
|
||||||
|
#include <iosfwd>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
/// @deprecated
|
/// @deprecated
|
||||||
struct PoseVelocityBias {
|
struct PoseVelocityBias {
|
||||||
Pose3 pose;
|
Pose3 pose;
|
||||||
|
|
@ -44,6 +47,7 @@ struct PoseVelocityBias {
|
||||||
return NavState(pose, velocity);
|
return NavState(pose, velocity);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* PreintegrationBase is the base class for PreintegratedMeasurements
|
* PreintegrationBase is the base class for PreintegratedMeasurements
|
||||||
|
|
@ -52,173 +56,140 @@ struct PoseVelocityBias {
|
||||||
* to access, print, and compare them.
|
* to access, print, and compare them.
|
||||||
*/
|
*/
|
||||||
class PreintegrationBase {
|
class PreintegrationBase {
|
||||||
|
public:
|
||||||
|
typedef imuBias::ConstantBias Bias;
|
||||||
|
typedef PreintegrationParams Params;
|
||||||
|
|
||||||
public:
|
protected:
|
||||||
|
|
||||||
/// Parameters for pre-integration:
|
/// Parameters. Declared mutable only for deprecated predict method.
|
||||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
/// TODO(frank): make const once deprecated method is removed
|
||||||
struct Params: PreintegratedRotation::Params {
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
|
mutable
|
||||||
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
|
#endif
|
||||||
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
|
boost::shared_ptr<Params> p_;
|
||||||
Vector3 n_gravity; ///< Gravity vector in nav frame
|
|
||||||
|
|
||||||
/// The Params constructor insists on getting the navigation frame gravity vector
|
/// Acceleration and gyro bias used for preintegration
|
||||||
/// For convenience, two commonly used conventions are provided by named constructors below
|
Bias biasHat_;
|
||||||
Params(const Vector3& n_gravity) :
|
|
||||||
accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
|
|
||||||
false), n_gravity(n_gravity) {
|
|
||||||
}
|
|
||||||
|
|
||||||
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
|
/// Time interval from i to j
|
||||||
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
|
double deltaTij_;
|
||||||
return boost::make_shared<Params>(Vector3(0, 0, g));
|
|
||||||
}
|
|
||||||
|
|
||||||
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
|
|
||||||
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
|
|
||||||
return boost::make_shared<Params>(Vector3(0, 0, -g));
|
|
||||||
}
|
|
||||||
|
|
||||||
void print(const std::string& s) const;
|
|
||||||
|
|
||||||
protected:
|
|
||||||
/// Default constructor for serialization only: uninitialized!
|
|
||||||
Params() {}
|
|
||||||
|
|
||||||
/** Serialization function */
|
|
||||||
friend class boost::serialization::access;
|
|
||||||
template<class ARCHIVE>
|
|
||||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
|
||||||
namespace bs = ::boost::serialization;
|
|
||||||
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
|
|
||||||
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
|
|
||||||
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
|
|
||||||
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(n_gravity);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
protected:
|
|
||||||
|
|
||||||
double deltaTij_; ///< Time interval from i to j
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Preintegrated navigation state, from frame i to frame j
|
* Preintegrated navigation state, from frame i to frame j
|
||||||
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
|
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
|
||||||
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
|
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
|
||||||
*/
|
*/
|
||||||
NavState deltaXij_;
|
Vector9 preintegrated_;
|
||||||
|
|
||||||
/// Parameters
|
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
|
||||||
boost::shared_ptr<Params> p_;
|
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
|
||||||
|
|
||||||
/// Acceleration and gyro bias used for preintegration
|
|
||||||
imuBias::ConstantBias biasHat_;
|
|
||||||
|
|
||||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
|
||||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
|
||||||
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
|
|
||||||
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
|
|
||||||
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
|
|
||||||
|
|
||||||
/// Default constructor for serialization
|
/// Default constructor for serialization
|
||||||
PreintegrationBase() {
|
PreintegrationBase() {
|
||||||
}
|
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Constructor, initializes the variables in the base class
|
|
||||||
* @param bias Current estimate of acceleration and rotation rate biases
|
|
||||||
* @param p Parameters, typically fixed in a single application
|
|
||||||
*/
|
|
||||||
PreintegrationBase(const boost::shared_ptr<Params>& p,
|
|
||||||
const imuBias::ConstantBias& biasHat) :
|
|
||||||
p_(p), biasHat_(biasHat) {
|
|
||||||
resetIntegration();
|
resetIntegration();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
/// @name Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor, initializes the variables in the base class
|
||||||
|
* @param p Parameters, typically fixed in a single application
|
||||||
|
* @param bias Current estimate of acceleration and rotation rate biases
|
||||||
|
*/
|
||||||
|
PreintegrationBase(const boost::shared_ptr<Params>& p,
|
||||||
|
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Basic utilities
|
||||||
|
/// @{
|
||||||
/// Re-initialize PreintegratedMeasurements
|
/// Re-initialize PreintegratedMeasurements
|
||||||
void resetIntegration();
|
void resetIntegration();
|
||||||
|
|
||||||
|
/// check parameters equality: checks whether shared pointer points to same Params object.
|
||||||
|
bool matchesParamsWith(const PreintegrationBase& other) const {
|
||||||
|
return p_ == other.p_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// shared pointer to params
|
||||||
|
const boost::shared_ptr<Params>& params() const {
|
||||||
|
return p_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// const reference to params
|
||||||
const Params& p() const {
|
const Params& p() const {
|
||||||
return *boost::static_pointer_cast<Params>(p_);
|
return *boost::static_pointer_cast<Params>(p_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
void set_body_P_sensor(const Pose3& body_P_sensor) {
|
void set_body_P_sensor(const Pose3& body_P_sensor) {
|
||||||
p_->body_P_sensor = body_P_sensor;
|
p_->body_P_sensor = body_P_sensor;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
/// @}
|
||||||
|
|
||||||
/// getters
|
/// @name Instance variables access
|
||||||
const NavState& deltaXij() const {
|
/// @{
|
||||||
return deltaXij_;
|
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
|
||||||
}
|
const double& deltaTij() const { return deltaTij_; }
|
||||||
const double& deltaTij() const {
|
|
||||||
return deltaTij_;
|
const Vector9& preintegrated() const { return preintegrated_; }
|
||||||
}
|
|
||||||
const Rot3& deltaRij() const {
|
Vector3 theta() const { return preintegrated_.head<3>(); }
|
||||||
return deltaXij_.attitude();
|
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
|
||||||
}
|
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
|
||||||
Vector3 deltaPij() const {
|
|
||||||
return deltaXij_.position().vector();
|
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
|
||||||
}
|
NavState deltaXij() const { return NavState::Retract(preintegrated_); }
|
||||||
Vector3 deltaVij() const {
|
|
||||||
return deltaXij_.velocity();
|
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
|
||||||
}
|
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
|
||||||
const imuBias::ConstantBias& biasHat() const {
|
|
||||||
return biasHat_;
|
|
||||||
}
|
|
||||||
const Matrix3& delRdelBiasOmega() const {
|
|
||||||
return delRdelBiasOmega_;
|
|
||||||
}
|
|
||||||
const Matrix3& delPdelBiasAcc() const {
|
|
||||||
return delPdelBiasAcc_;
|
|
||||||
}
|
|
||||||
const Matrix3& delPdelBiasOmega() const {
|
|
||||||
return delPdelBiasOmega_;
|
|
||||||
}
|
|
||||||
const Matrix3& delVdelBiasAcc() const {
|
|
||||||
return delVdelBiasAcc_;
|
|
||||||
}
|
|
||||||
const Matrix3& delVdelBiasOmega() const {
|
|
||||||
return delVdelBiasOmega_;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Exposed for MATLAB
|
// Exposed for MATLAB
|
||||||
Vector6 biasHatVector() const {
|
Vector6 biasHatVector() const { return biasHat_.vector(); }
|
||||||
return biasHat_.vector();
|
/// @}
|
||||||
}
|
|
||||||
|
|
||||||
/// print
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
|
||||||
void print(const std::string& s) const;
|
void print(const std::string& s) const;
|
||||||
|
|
||||||
/// check equality
|
|
||||||
bool equals(const PreintegrationBase& other, double tol) const;
|
bool equals(const PreintegrationBase& other, double tol) const;
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/// @name Main functionality
|
||||||
|
/// @{
|
||||||
|
|
||||||
/// Subtract estimate and correct for sensor pose
|
/// Subtract estimate and correct for sensor pose
|
||||||
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
|
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
|
||||||
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
|
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
|
||||||
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
|
std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
|
||||||
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
|
||||||
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
|
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc = boost::none,
|
||||||
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
|
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega = boost::none,
|
||||||
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
|
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const;
|
||||||
|
|
||||||
/// Calculate the updated preintegrated measurement, does not modify
|
// Update integrated vector on tangent manifold preintegrated with acceleration
|
||||||
/// It takes measured quantities in the j frame
|
// Static, functional version.
|
||||||
NavState updatedDeltaXij(const Vector3& j_measuredAcc,
|
static Vector9 UpdatePreintegrated(const Vector3& a_body,
|
||||||
const Vector3& j_measuredOmega, const double dt,
|
const Vector3& w_body, double dt,
|
||||||
OptionalJacobian<9, 9> D_updated_current = boost::none,
|
const Vector9& preintegrated,
|
||||||
OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
|
OptionalJacobian<9, 9> A = boost::none,
|
||||||
OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
|
OptionalJacobian<9, 3> B = boost::none,
|
||||||
|
OptionalJacobian<9, 3> C = boost::none);
|
||||||
|
|
||||||
/// Update preintegrated measurements and get derivatives
|
/// Update preintegrated measurements and get derivatives
|
||||||
/// It takes measured quantities in the j frame
|
/// It takes measured quantities in the j frame
|
||||||
void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
|
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
|
||||||
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
|
void integrateMeasurement(const Vector3& measuredAcc,
|
||||||
Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
|
const Vector3& measuredOmega, const double deltaT,
|
||||||
|
Matrix9* A, Matrix93* B, Matrix93* C);
|
||||||
|
|
||||||
|
// Version without derivatives
|
||||||
|
void integrateMeasurement(const Vector3& measuredAcc,
|
||||||
|
const Vector3& measuredOmega, const double deltaT);
|
||||||
|
|
||||||
/// Given the estimate of the bias, return a NavState tangent vector
|
/// Given the estimate of the bias, return a NavState tangent vector
|
||||||
/// summarizing the preintegrated IMU measurements so far
|
/// summarizing the preintegrated IMU measurements so far
|
||||||
|
|
@ -227,8 +198,14 @@ public:
|
||||||
|
|
||||||
/// Predict state at time j
|
/// Predict state at time j
|
||||||
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
|
||||||
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
|
OptionalJacobian<9, 9> H1 = boost::none,
|
||||||
boost::none) const;
|
OptionalJacobian<9, 6> H2 = boost::none) const;
|
||||||
|
|
||||||
|
/// Calculate error given navStates
|
||||||
|
Vector9 computeError(const NavState& state_i, const NavState& state_j,
|
||||||
|
const imuBias::ConstantBias& bias_i,
|
||||||
|
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
|
||||||
|
OptionalJacobian<9, 6> H3) const;
|
||||||
|
|
||||||
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
|
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
|
||||||
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
|
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
|
|
@ -238,10 +215,34 @@ public:
|
||||||
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
|
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
|
||||||
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
|
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
|
||||||
|
|
||||||
|
// Compose the two preintegrated vectors
|
||||||
|
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
|
||||||
|
double deltaT12,
|
||||||
|
OptionalJacobian<9, 9> H1 = boost::none,
|
||||||
|
OptionalJacobian<9, 9> H2 = boost::none);
|
||||||
|
|
||||||
|
/// Merge in a different set of measurements and update bias derivatives accordingly
|
||||||
|
/// The derivatives apply to the preintegrated Vector9
|
||||||
|
void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
|
||||||
|
/// @}
|
||||||
|
|
||||||
|
/** Dummy clone for MATLAB */
|
||||||
|
virtual boost::shared_ptr<PreintegrationBase> clone() const {
|
||||||
|
return boost::shared_ptr<PreintegrationBase>();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
|
/// @name Deprecated
|
||||||
|
/// @{
|
||||||
|
|
||||||
/// @deprecated predict
|
/// @deprecated predict
|
||||||
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||||
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
|
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
|
||||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
|
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
#endif
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/** Serialization function */
|
/** Serialization function */
|
||||||
|
|
@ -250,14 +251,11 @@ private:
|
||||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||||
namespace bs = ::boost::serialization;
|
namespace bs = ::boost::serialization;
|
||||||
ar & BOOST_SERIALIZATION_NVP(p_);
|
ar & BOOST_SERIALIZATION_NVP(p_);
|
||||||
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
ar & BOOST_SERIALIZATION_NVP(biasHat_);
|
||||||
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
|
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
|
||||||
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
|
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
|
||||||
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
|
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
|
||||||
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
|
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
|
||||||
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
|
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,54 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file PreintegrationParams.h
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Stephen Williams
|
||||||
|
* @author Richard Roberts
|
||||||
|
* @author Vadim Indelman
|
||||||
|
* @author David Jensen
|
||||||
|
* @author Frank Dellaert
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include "PreintegrationParams.h"
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
void PreintegrationParams::print(const string& s) const {
|
||||||
|
PreintegratedRotation::Params::print(s);
|
||||||
|
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
|
||||||
|
<< endl;
|
||||||
|
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
|
||||||
|
<< endl;
|
||||||
|
if (omegaCoriolis && use2ndOrderCoriolis)
|
||||||
|
cout << "Using 2nd-order Coriolis" << endl;
|
||||||
|
if (body_P_sensor) body_P_sensor->print(" ");
|
||||||
|
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
//------------------------------------------------------------------------------
|
||||||
|
bool PreintegrationParams::equals(const PreintegratedRotation::Params& other,
|
||||||
|
double tol) const {
|
||||||
|
auto e = dynamic_cast<const PreintegrationParams*>(&other);
|
||||||
|
return e != nullptr && PreintegratedRotation::Params::equals(other, tol) &&
|
||||||
|
use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
|
||||||
|
equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
|
||||||
|
tol) &&
|
||||||
|
equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
|
||||||
|
tol) &&
|
||||||
|
equal_with_abs_tol(n_gravity, e->n_gravity, tol);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,71 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file PreintegrationParams.h
|
||||||
|
* @author Frank Dellaert
|
||||||
|
**/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/navigation/PreintegratedRotation.h>
|
||||||
|
#include <boost/make_shared.hpp>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/// Parameters for pre-integration:
|
||||||
|
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||||
|
struct PreintegrationParams: PreintegratedRotation::Params {
|
||||||
|
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
|
||||||
|
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
|
||||||
|
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
|
||||||
|
Vector3 n_gravity; ///< Gravity vector in nav frame
|
||||||
|
|
||||||
|
/// The Params constructor insists on getting the navigation frame gravity vector
|
||||||
|
/// For convenience, two commonly used conventions are provided by named constructors below
|
||||||
|
PreintegrationParams(const Vector3& n_gravity)
|
||||||
|
: accelerometerCovariance(I_3x3),
|
||||||
|
integrationCovariance(I_3x3),
|
||||||
|
use2ndOrderCoriolis(false),
|
||||||
|
n_gravity(n_gravity) {}
|
||||||
|
|
||||||
|
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
|
||||||
|
static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
|
||||||
|
return boost::make_shared<PreintegrationParams>(Vector3(0, 0, g));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
|
||||||
|
static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
|
||||||
|
return boost::make_shared<PreintegrationParams>(Vector3(0, 0, -g));
|
||||||
|
}
|
||||||
|
|
||||||
|
void print(const std::string& s) const;
|
||||||
|
bool equals(const PreintegratedRotation::Params& other, double tol) const;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
/// Default constructor for serialization only: uninitialized!
|
||||||
|
PreintegrationParams() {}
|
||||||
|
|
||||||
|
/** Serialization function */
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||||
|
namespace bs = ::boost::serialization;
|
||||||
|
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
|
||||||
|
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
|
||||||
|
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
|
||||||
|
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(n_gravity);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,97 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Scenario.h
|
||||||
|
* @brief Simple class to test navigation scenarios
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
#include <gtsam/linear/NoiseModel.h>
|
||||||
|
#include <gtsam/navigation/NavState.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/// Simple trajectory simulator.
|
||||||
|
class Scenario {
|
||||||
|
public:
|
||||||
|
// Quantities a Scenario needs to specify:
|
||||||
|
|
||||||
|
virtual Pose3 pose(double t) const = 0;
|
||||||
|
virtual Vector3 omega_b(double t) const = 0;
|
||||||
|
virtual Vector3 velocity_n(double t) const = 0;
|
||||||
|
virtual Vector3 acceleration_n(double t) const = 0;
|
||||||
|
|
||||||
|
// Derived quantities:
|
||||||
|
|
||||||
|
Rot3 rotation(double t) const { return pose(t).rotation(); }
|
||||||
|
NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
|
||||||
|
|
||||||
|
Vector3 velocity_b(double t) const {
|
||||||
|
const Rot3 nRb = rotation(t);
|
||||||
|
return nRb.transpose() * velocity_n(t);
|
||||||
|
}
|
||||||
|
|
||||||
|
Vector3 acceleration_b(double t) const {
|
||||||
|
const Rot3 nRb = rotation(t);
|
||||||
|
return nRb.transpose() * acceleration_n(t);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Scenario with constant twist 3D trajectory.
|
||||||
|
* Note that a plane flying level does not feel any acceleration: gravity is
|
||||||
|
* being perfectly compensated by the lift generated by the wings, and drag is
|
||||||
|
* compensated by thrust. The accelerometer will add the gravity component back
|
||||||
|
* in, as modeled by the specificForce method in ScenarioRunner.
|
||||||
|
*/
|
||||||
|
class ConstantTwistScenario : public Scenario {
|
||||||
|
public:
|
||||||
|
/// Construct scenario with constant twist [w,v]
|
||||||
|
ConstantTwistScenario(const Vector3& w, const Vector3& v)
|
||||||
|
: twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)) {}
|
||||||
|
|
||||||
|
Pose3 pose(double t) const override { return Pose3::Expmap(twist_ * t); }
|
||||||
|
Vector3 omega_b(double t) const override { return twist_.head<3>(); }
|
||||||
|
Vector3 velocity_n(double t) const override {
|
||||||
|
return rotation(t).matrix() * twist_.tail<3>();
|
||||||
|
}
|
||||||
|
Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
const Vector6 twist_;
|
||||||
|
const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Accelerating from an arbitrary initial state, with optional rotation
|
||||||
|
class AcceleratingScenario : public Scenario {
|
||||||
|
public:
|
||||||
|
/// Construct scenario with constant acceleration in navigation frame and
|
||||||
|
/// optional angular velocity in body: rotating while translating...
|
||||||
|
AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
|
||||||
|
const Vector3& a_n,
|
||||||
|
const Vector3& omega_b = Vector3::Zero())
|
||||||
|
: nRb_(nRb), p0_(p0.vector()), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
|
||||||
|
|
||||||
|
Pose3 pose(double t) const override {
|
||||||
|
return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
|
||||||
|
}
|
||||||
|
Vector3 omega_b(double t) const override { return omega_b_; }
|
||||||
|
Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
|
||||||
|
Vector3 acceleration_n(double t) const override { return a_n_; }
|
||||||
|
|
||||||
|
private:
|
||||||
|
const Rot3 nRb_;
|
||||||
|
const Vector3 p0_, v0_, a_n_, omega_b_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,107 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file ScenarioRunner.h
|
||||||
|
* @brief Simple class to test navigation scenarios
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/navigation/ScenarioRunner.h>
|
||||||
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
|
#include <boost/assign.hpp>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
static double intNoiseVar = 0.0000001;
|
||||||
|
static const Matrix3 kIntegrationErrorCovariance = intNoiseVar * I_3x3;
|
||||||
|
|
||||||
|
PreintegratedImuMeasurements ScenarioRunner::integrate(
|
||||||
|
double T, const Bias& estimatedBias, bool corrupted) const {
|
||||||
|
PreintegratedImuMeasurements pim(p_, estimatedBias);
|
||||||
|
|
||||||
|
const double dt = imuSampleTime();
|
||||||
|
const size_t nrSteps = T / dt;
|
||||||
|
double t = 0;
|
||||||
|
for (size_t k = 0; k < nrSteps; k++, t += dt) {
|
||||||
|
Vector3 measuredOmega =
|
||||||
|
corrupted ? measuredAngularVelocity(t) : actualAngularVelocity(t);
|
||||||
|
Vector3 measuredAcc =
|
||||||
|
corrupted ? measuredSpecificForce(t) : actualSpecificForce(t);
|
||||||
|
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
return pim;
|
||||||
|
}
|
||||||
|
|
||||||
|
NavState ScenarioRunner::predict(const PreintegratedImuMeasurements& pim,
|
||||||
|
const Bias& estimatedBias) const {
|
||||||
|
const NavState state_i(scenario_->pose(0), scenario_->velocity_n(0));
|
||||||
|
return pim.predict(state_i, estimatedBias);
|
||||||
|
}
|
||||||
|
|
||||||
|
Matrix9 ScenarioRunner::estimateCovariance(double T, size_t N,
|
||||||
|
const Bias& estimatedBias) const {
|
||||||
|
gttic_(estimateCovariance);
|
||||||
|
|
||||||
|
// Get predict prediction from ground truth measurements
|
||||||
|
NavState prediction = predict(integrate(T));
|
||||||
|
|
||||||
|
// Sample !
|
||||||
|
Matrix samples(9, N);
|
||||||
|
Vector9 sum = Vector9::Zero();
|
||||||
|
for (size_t i = 0; i < N; i++) {
|
||||||
|
auto pim = integrate(T, estimatedBias, true);
|
||||||
|
NavState sampled = predict(pim);
|
||||||
|
Vector9 xi = sampled.localCoordinates(prediction);
|
||||||
|
samples.col(i) = xi;
|
||||||
|
sum += xi;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute MC covariance
|
||||||
|
Vector9 sampleMean = sum / N;
|
||||||
|
Matrix9 Q;
|
||||||
|
Q.setZero();
|
||||||
|
for (size_t i = 0; i < N; i++) {
|
||||||
|
Vector9 xi = samples.col(i) - sampleMean;
|
||||||
|
Q += xi * xi.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
|
return Q / (N - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
Matrix6 ScenarioRunner::estimateNoiseCovariance(size_t N) const {
|
||||||
|
Matrix samples(6, N);
|
||||||
|
Vector6 sum = Vector6::Zero();
|
||||||
|
for (size_t i = 0; i < N; i++) {
|
||||||
|
samples.col(i) << accSampler_.sample() / sqrt_dt_,
|
||||||
|
gyroSampler_.sample() / sqrt_dt_;
|
||||||
|
sum += samples.col(i);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute MC covariance
|
||||||
|
Vector6 sampleMean = sum / N;
|
||||||
|
Matrix6 Q;
|
||||||
|
Q.setZero();
|
||||||
|
for (size_t i = 0; i < N; i++) {
|
||||||
|
Vector6 xi = samples.col(i) - sampleMean;
|
||||||
|
Q += xi * xi.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
|
return Q / (N - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,109 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file ScenarioRunner.h
|
||||||
|
* @brief Simple class to test navigation scenarios
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
#include <gtsam/navigation/ImuFactor.h>
|
||||||
|
#include <gtsam/navigation/Scenario.h>
|
||||||
|
#include <gtsam/linear/Sampler.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
// Convert covariance to diagonal noise model, if possible, otherwise throw
|
||||||
|
static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
|
||||||
|
bool smart = true;
|
||||||
|
auto model = noiseModel::Gaussian::Covariance(covariance, smart);
|
||||||
|
auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||||
|
if (!diagonal)
|
||||||
|
throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
|
||||||
|
return diagonal;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Simple class to test navigation scenarios.
|
||||||
|
* Takes a trajectory scenario as input, and can generate IMU measurements
|
||||||
|
*/
|
||||||
|
class ScenarioRunner {
|
||||||
|
public:
|
||||||
|
typedef imuBias::ConstantBias Bias;
|
||||||
|
typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
|
||||||
|
|
||||||
|
private:
|
||||||
|
const Scenario* scenario_;
|
||||||
|
const SharedParams p_;
|
||||||
|
const double imuSampleTime_, sqrt_dt_;
|
||||||
|
const Bias estimatedBias_;
|
||||||
|
|
||||||
|
// Create two samplers for acceleration and omega noise
|
||||||
|
mutable Sampler gyroSampler_, accSampler_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
|
||||||
|
double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
|
||||||
|
: scenario_(scenario),
|
||||||
|
p_(p),
|
||||||
|
imuSampleTime_(imuSampleTime),
|
||||||
|
sqrt_dt_(std::sqrt(imuSampleTime)),
|
||||||
|
estimatedBias_(bias),
|
||||||
|
// NOTE(duy): random seeds that work well:
|
||||||
|
gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
|
||||||
|
accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
|
||||||
|
|
||||||
|
// NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
|
||||||
|
// also, uses g=10 for easy debugging
|
||||||
|
const Vector3& gravity_n() const { return p_->n_gravity; }
|
||||||
|
|
||||||
|
// A gyro simply measures angular velocity in body frame
|
||||||
|
Vector3 actualAngularVelocity(double t) const {
|
||||||
|
return scenario_->omega_b(t);
|
||||||
|
}
|
||||||
|
|
||||||
|
// An accelerometer measures acceleration in body, but not gravity
|
||||||
|
Vector3 actualSpecificForce(double t) const {
|
||||||
|
Rot3 bRn = scenario_->rotation(t).transpose();
|
||||||
|
return scenario_->acceleration_b(t) - bRn * gravity_n();
|
||||||
|
}
|
||||||
|
|
||||||
|
// versions corrupted by bias and noise
|
||||||
|
Vector3 measuredAngularVelocity(double t) const {
|
||||||
|
return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
|
||||||
|
gyroSampler_.sample() / sqrt_dt_;
|
||||||
|
}
|
||||||
|
Vector3 measuredSpecificForce(double t) const {
|
||||||
|
return actualSpecificForce(t) + estimatedBias_.accelerometer() +
|
||||||
|
accSampler_.sample() / sqrt_dt_;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double& imuSampleTime() const { return imuSampleTime_; }
|
||||||
|
|
||||||
|
/// Integrate measurements for T seconds into a PIM
|
||||||
|
PreintegratedImuMeasurements integrate(double T,
|
||||||
|
const Bias& estimatedBias = Bias(),
|
||||||
|
bool corrupted = false) const;
|
||||||
|
|
||||||
|
/// Predict predict given a PIM
|
||||||
|
NavState predict(const PreintegratedImuMeasurements& pim,
|
||||||
|
const Bias& estimatedBias = Bias()) const;
|
||||||
|
|
||||||
|
/// Compute a Monte Carlo estimate of the predict covariance using N samples
|
||||||
|
Matrix9 estimateCovariance(double T, size_t N = 1000,
|
||||||
|
const Bias& estimatedBias = Bias()) const;
|
||||||
|
|
||||||
|
/// Estimate covariance of sampled noise for sanity-check
|
||||||
|
Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,76 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file imuFactorTesting.h
|
||||||
|
* @brief Common testing infrastructure
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/navigation/ImuBias.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
// Convenience for named keys
|
||||||
|
using symbol_shorthand::X;
|
||||||
|
using symbol_shorthand::V;
|
||||||
|
using symbol_shorthand::B;
|
||||||
|
|
||||||
|
typedef imuBias::ConstantBias Bias;
|
||||||
|
static const Vector3 Z_3x1 = Vector3::Zero();
|
||||||
|
static const Bias kZeroBiasHat, kZeroBias;
|
||||||
|
|
||||||
|
static const Vector3 kZeroOmegaCoriolis(0, 0, 0);
|
||||||
|
static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
|
||||||
|
|
||||||
|
static const double kGravity = 10;
|
||||||
|
static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
|
||||||
|
|
||||||
|
// Realistic MEMS white noise characteristics. Angular and velocity random walk
|
||||||
|
// expressed in degrees respectively m/s per sqrt(hr).
|
||||||
|
auto radians = [](double t) { return t * M_PI / 180; };
|
||||||
|
static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW
|
||||||
|
static const double kAccelSigma = 0.1 / 60; // 10 cm VRW
|
||||||
|
|
||||||
|
namespace testing {
|
||||||
|
|
||||||
|
struct ImuMeasurement {
|
||||||
|
ImuMeasurement(const Vector3& acc, const Vector3& gyro, double dt)
|
||||||
|
: acc(acc), gyro(gyro), dt(dt) {}
|
||||||
|
const Vector3 acc, gyro;
|
||||||
|
const double dt;
|
||||||
|
};
|
||||||
|
|
||||||
|
template <typename PIM>
|
||||||
|
void integrateMeasurements(const vector<ImuMeasurement>& measurements,
|
||||||
|
PIM* pim) {
|
||||||
|
for (const auto& m : measurements)
|
||||||
|
pim->integrateMeasurement(m.acc, m.gyro, m.dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
struct SomeMeasurements : vector<ImuMeasurement> {
|
||||||
|
SomeMeasurements() {
|
||||||
|
reserve(102);
|
||||||
|
const double dt = 0.01, pi100 = M_PI / 100;
|
||||||
|
emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt);
|
||||||
|
emplace_back(Vector3(0.1, 0, 0), Vector3(pi100, 0, 0), dt);
|
||||||
|
for (int i = 1; i < 100; i++) {
|
||||||
|
emplace_back(Vector3(0.05, 0.09, 0.01),
|
||||||
|
Vector3(pi100, pi100 * 3, 2 * pi100), dt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace testing
|
||||||
|
|
@ -13,8 +13,9 @@
|
||||||
* @file testCombinedImuFactor.cpp
|
* @file testCombinedImuFactor.cpp
|
||||||
* @brief Unit test for Lupton-style combined IMU factor
|
* @brief Unit test for Lupton-style combined IMU factor
|
||||||
* @author Luca Carlone
|
* @author Luca Carlone
|
||||||
* @author Stephen Williams
|
* @author Frank Dellaert
|
||||||
* @author Richard Roberts
|
* @author Richard Roberts
|
||||||
|
* @author Stephen Williams
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <gtsam/navigation/ImuFactor.h>
|
#include <gtsam/navigation/ImuFactor.h>
|
||||||
|
|
@ -31,62 +32,23 @@
|
||||||
#include <boost/bind.hpp>
|
#include <boost/bind.hpp>
|
||||||
#include <list>
|
#include <list>
|
||||||
|
|
||||||
using namespace std;
|
#include "imuFactorTesting.h"
|
||||||
using namespace gtsam;
|
|
||||||
|
|
||||||
// Convenience for named keys
|
namespace testing {
|
||||||
using symbol_shorthand::X;
|
// Create default parameters with Z-down and above noise parameters
|
||||||
using symbol_shorthand::V;
|
static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
|
||||||
using symbol_shorthand::B;
|
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
|
||||||
|
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||||
namespace {
|
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||||
|
p->integrationCovariance = 0.0001 * I_3x3;
|
||||||
// Auxiliary functions to test preintegrated Jacobians
|
return p;
|
||||||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
|
||||||
/* ************************************************************************* */
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
|
|
||||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
|
||||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3,
|
|
||||||
I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
|
|
||||||
|
|
||||||
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
|
|
||||||
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
|
|
||||||
list<double>::const_iterator itDeltaT = deltaTs.begin();
|
|
||||||
for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
|
|
||||||
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
|
|
||||||
}
|
|
||||||
return result;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector3 evaluatePreintegratedMeasurementsPosition(
|
|
||||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
|
||||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
|
||||||
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
|
||||||
deltaTs).deltaPij();
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector3 evaluatePreintegratedMeasurementsVelocity(
|
|
||||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
|
||||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
|
||||||
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
|
||||||
deltaTs).deltaVij();
|
|
||||||
}
|
|
||||||
|
|
||||||
Rot3 evaluatePreintegratedMeasurementsRotation(
|
|
||||||
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
|
|
||||||
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
|
|
||||||
return Rot3(
|
|
||||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
|
||||||
deltaTs).deltaRij());
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
||||||
// Linearization point
|
// Linearization point
|
||||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
|
Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
|
||||||
|
|
||||||
// Measurements
|
// Measurements
|
||||||
Vector3 measuredAcc(0.1, 0.0, 0.0);
|
Vector3 measuredAcc(0.1, 0.0, 0.0);
|
||||||
|
|
@ -94,12 +56,13 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
||||||
double deltaT = 0.5;
|
double deltaT = 0.5;
|
||||||
double tol = 1e-6;
|
double tol = 1e-6;
|
||||||
|
|
||||||
|
auto p = testing::Params();
|
||||||
|
|
||||||
// Actual preintegrated values
|
// Actual preintegrated values
|
||||||
ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3);
|
PreintegratedImuMeasurements expected1(p, bias);
|
||||||
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3,
|
PreintegratedCombinedMeasurements actual1(p, bias);
|
||||||
Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6);
|
|
||||||
|
|
||||||
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
|
|
@ -111,46 +74,41 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( CombinedImuFactor, ErrorWithBiases ) {
|
TEST( CombinedImuFactor, ErrorWithBiases ) {
|
||||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
|
||||||
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
|
Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
|
||||||
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
|
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
|
||||||
Vector3 v1(0.5, 0.0, 0.0);
|
Vector3 v1(0.5, 0.0, 0.0);
|
||||||
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
|
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
|
||||||
Point3(5.5, 1.0, -50.0));
|
Point3(5.5, 1.0, -50.0));
|
||||||
Vector3 v2(0.5, 0.0, 0.0);
|
Vector3 v2(0.5, 0.0, 0.0);
|
||||||
|
|
||||||
|
auto p = testing::Params();
|
||||||
|
p->omegaCoriolis = Vector3(0,0.1,0.1);
|
||||||
|
PreintegratedImuMeasurements pim(
|
||||||
|
p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||||
|
|
||||||
// Measurements
|
// Measurements
|
||||||
Vector3 gravity;
|
|
||||||
gravity << 0, 0, 9.81;
|
|
||||||
Vector3 omegaCoriolis;
|
|
||||||
omegaCoriolis << 0, 0.1, 0.1;
|
|
||||||
Vector3 measuredOmega;
|
Vector3 measuredOmega;
|
||||||
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
|
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
|
||||||
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector()
|
Vector3 measuredAcc =
|
||||||
+ Vector3(0.2, 0.0, 0.0);
|
x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
|
||||||
double deltaT = 1.0;
|
double deltaT = 1.0;
|
||||||
double tol = 1e-6;
|
double tol = 1e-6;
|
||||||
|
|
||||||
ImuFactor::PreintegratedMeasurements pim(
|
|
||||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
|
||||||
I_3x3, I_3x3, I_3x3);
|
|
||||||
|
|
||||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim(
|
PreintegratedCombinedMeasurements combined_pim(p,
|
||||||
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
|
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
|
||||||
I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6);
|
|
||||||
|
|
||||||
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
// Create factor
|
// Create factor
|
||||||
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity,
|
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim);
|
||||||
omegaCoriolis);
|
|
||||||
|
|
||||||
noiseModel::Gaussian::shared_ptr Combinedmodel =
|
noiseModel::Gaussian::shared_ptr Combinedmodel =
|
||||||
noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
|
noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
|
||||||
CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
|
CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
|
||||||
combined_pim, gravity, omegaCoriolis);
|
combined_pim);
|
||||||
|
|
||||||
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
|
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
|
||||||
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
|
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
|
||||||
|
|
@ -174,130 +132,75 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
|
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
|
||||||
// Linearization point
|
auto p = testing::Params();
|
||||||
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
|
testing::SomeMeasurements measurements;
|
||||||
|
|
||||||
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
|
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
|
||||||
|
[=](const Vector3& a, const Vector3& w) {
|
||||||
|
PreintegratedImuMeasurements pim(p, Bias(a, w));
|
||||||
|
testing::integrateMeasurements(measurements, &pim);
|
||||||
|
return pim.preintegrated();
|
||||||
|
};
|
||||||
|
|
||||||
// Measurements
|
// Actual pre-integrated values
|
||||||
list<Vector3> measuredAccs, measuredOmegas;
|
PreintegratedCombinedMeasurements pim(p);
|
||||||
list<double> deltaTs;
|
testing::integrateMeasurements(measurements, &pim);
|
||||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
|
||||||
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
|
|
||||||
deltaTs.push_back(0.01);
|
|
||||||
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
|
|
||||||
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
|
|
||||||
deltaTs.push_back(0.01);
|
|
||||||
for (int i = 1; i < 100; i++) {
|
|
||||||
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
|
|
||||||
measuredOmegas.push_back(
|
|
||||||
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
|
|
||||||
deltaTs.push_back(0.01);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Actual preintegrated values
|
EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1),
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements pim =
|
pim.preintegrated_H_biasAcc()));
|
||||||
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
|
EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1),
|
||||||
deltaTs);
|
pim.preintegrated_H_biasOmega(), 1e-3));
|
||||||
|
|
||||||
// Compute numerical derivatives
|
|
||||||
Matrix expectedDelPdelBias = numericalDerivative11<Vector,
|
|
||||||
imuBias::ConstantBias>(
|
|
||||||
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
|
|
||||||
measuredOmegas, deltaTs), bias);
|
|
||||||
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
|
|
||||||
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
|
|
||||||
|
|
||||||
Matrix expectedDelVdelBias = numericalDerivative11<Vector,
|
|
||||||
imuBias::ConstantBias>(
|
|
||||||
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
|
|
||||||
measuredOmegas, deltaTs), bias);
|
|
||||||
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
|
|
||||||
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
|
|
||||||
|
|
||||||
Matrix expectedDelRdelBias =
|
|
||||||
numericalDerivative11<Rot3, imuBias::ConstantBias>(
|
|
||||||
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
|
|
||||||
measuredAccs, measuredOmegas, deltaTs), bias);
|
|
||||||
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
|
|
||||||
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
|
|
||||||
|
|
||||||
// Compare Jacobians
|
|
||||||
EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc()));
|
|
||||||
EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega()));
|
|
||||||
EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc()));
|
|
||||||
EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega()));
|
|
||||||
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
|
|
||||||
EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
|
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
|
||||||
imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
|
||||||
|
|
||||||
|
auto p = testing::Params();
|
||||||
|
|
||||||
// Measurements
|
// Measurements
|
||||||
Vector3 gravity;
|
const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3;
|
||||||
gravity << 0, 0, 9.81;
|
const Vector3 measuredAcc(0, 1.1, -kGravity);
|
||||||
Vector3 omegaCoriolis;
|
const double deltaT = 0.001;
|
||||||
omegaCoriolis << 0, 0, 0;
|
|
||||||
Vector3 measuredOmega;
|
|
||||||
measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3;
|
|
||||||
Vector3 measuredAcc;
|
|
||||||
measuredAcc << 0, 1.1, -9.81;
|
|
||||||
double deltaT = 0.001;
|
|
||||||
|
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
|
PreintegratedCombinedMeasurements pim(p, bias);
|
||||||
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
|
|
||||||
|
|
||||||
for (int i = 0; i < 1000; ++i)
|
for (int i = 0; i < 1000; ++i)
|
||||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
// Create factor
|
// Create factor
|
||||||
noiseModel::Gaussian::shared_ptr combinedmodel =
|
const noiseModel::Gaussian::shared_ptr combinedmodel =
|
||||||
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
|
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
|
||||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
|
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
|
||||||
gravity, omegaCoriolis);
|
|
||||||
|
|
||||||
// Predict
|
// Predict
|
||||||
Pose3 x1;
|
const NavState actual = pim.predict(NavState(), bias);
|
||||||
Vector3 v1(0, 0.0, 0.0);
|
const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
||||||
PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity,
|
const Vector3 expectedVelocity(0, 1, 0);
|
||||||
omegaCoriolis);
|
EXPECT(assert_equal(expectedPose, actual.pose()));
|
||||||
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
|
EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
|
||||||
Vector3 expectedVelocity;
|
|
||||||
expectedVelocity << 0, 1, 0;
|
|
||||||
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
|
|
||||||
EXPECT(
|
|
||||||
assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST(CombinedImuFactor, PredictRotation) {
|
TEST(CombinedImuFactor, PredictRotation) {
|
||||||
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
|
||||||
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
|
auto p = testing::Params();
|
||||||
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
|
PreintegratedCombinedMeasurements pim(p, bias);
|
||||||
Vector3 measuredAcc;
|
const Vector3 measuredAcc = - kGravityAlongNavZDown;
|
||||||
measuredAcc << 0, 0, -9.81;
|
const Vector3 measuredOmega(0, 0, M_PI / 10.0);
|
||||||
Vector3 gravity;
|
const double deltaT = 0.001;
|
||||||
gravity << 0, 0, 9.81;
|
const double tol = 1e-4;
|
||||||
Vector3 omegaCoriolis;
|
|
||||||
omegaCoriolis << 0, 0, 0;
|
|
||||||
Vector3 measuredOmega;
|
|
||||||
measuredOmega << 0, 0, M_PI / 10.0;
|
|
||||||
double deltaT = 0.001;
|
|
||||||
double tol = 1e-4;
|
|
||||||
for (int i = 0; i < 1000; ++i)
|
for (int i = 0; i < 1000; ++i)
|
||||||
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
|
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
|
||||||
gravity, omegaCoriolis);
|
|
||||||
|
|
||||||
// Predict
|
// Predict
|
||||||
Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
|
const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
|
||||||
Vector3 v(0, 0, 0), v2;
|
const Vector3 v(0, 0, 0), v2;
|
||||||
CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
|
const NavState actual = pim.predict(NavState(x, v), bias);
|
||||||
Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
|
const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
|
||||||
EXPECT(assert_equal(expectedPose, x2, tol));
|
EXPECT(assert_equal(expectedPose, actual.pose(), tol));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,77 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file testImuFactor.cpp
|
||||||
|
* @brief Unit test for ImuFactor
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @author Richard Roberts
|
||||||
|
* @author Stephen Williams
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/navigation/ImuFactor.h>
|
||||||
|
#include <gtsam/base/serializationTestHelpers.h>
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
|
||||||
|
"gtsam_noiseModel_Constrained");
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
|
||||||
|
"gtsam_noiseModel_Diagonal");
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
|
||||||
|
"gtsam_noiseModel_Gaussian");
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
|
||||||
|
"gtsam_noiseModel_Isotropic");
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
|
||||||
|
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
|
||||||
|
|
||||||
|
TEST(ImuFactor, serialization) {
|
||||||
|
using namespace gtsam::serializationTestHelpers;
|
||||||
|
|
||||||
|
// Create default parameters with Z-down and above noise paramaters
|
||||||
|
auto p = PreintegrationParams::MakeSharedD(9.81);
|
||||||
|
p->body_P_sensor = Pose3(Rot3::Ypr(0, 0, M_PI), Point3());
|
||||||
|
p->accelerometerCovariance = 1e-7 * I_3x3;
|
||||||
|
p->gyroscopeCovariance = 1e-8 * I_3x3;
|
||||||
|
p->integrationCovariance = 1e-9 * I_3x3;
|
||||||
|
|
||||||
|
const double deltaT = 0.005;
|
||||||
|
const imuBias::ConstantBias priorBias(
|
||||||
|
Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
|
||||||
|
|
||||||
|
PreintegratedImuMeasurements pim(p, priorBias);
|
||||||
|
|
||||||
|
// measurements are needed for non-inf noise model, otherwise will throw err
|
||||||
|
// when deserialize
|
||||||
|
const Vector3 measuredOmega(0, 0.01, 0);
|
||||||
|
const Vector3 measuredAcc(0, 0, -9.81);
|
||||||
|
|
||||||
|
for (int j = 0; j < 200; ++j)
|
||||||
|
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||||
|
|
||||||
|
ImuFactor factor(1, 2, 3, 4, 5, pim);
|
||||||
|
|
||||||
|
EXPECT(equalsObj(factor));
|
||||||
|
EXPECT(equalsXML(factor));
|
||||||
|
EXPECT(equalsBinary(factor));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -25,6 +25,8 @@
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||||
|
|
||||||
// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
|
// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
|
||||||
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
|
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
|
||||||
Matrix3 R1 = pvb1.pose.rotation().matrix();
|
Matrix3 R1 = pvb1.pose.rotation().matrix();
|
||||||
|
|
@ -55,6 +57,7 @@ TEST(PoseVelocityBias, error) {
|
||||||
expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3;
|
expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3;
|
||||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
/* ************************************************************************************************/
|
/* ************************************************************************************************/
|
||||||
int main() {
|
int main() {
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,150 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file testPreintegrationBase.cpp
|
||||||
|
* @brief Unit test for the InertialNavFactor
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/navigation/PreintegrationBase.h>
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <gtsam/nonlinear/expressions.h>
|
||||||
|
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||||
|
#include <gtsam/nonlinear/expressionTesting.h>
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <boost/bind.hpp>
|
||||||
|
|
||||||
|
#include "imuFactorTesting.h"
|
||||||
|
|
||||||
|
static const double kDt = 0.1;
|
||||||
|
|
||||||
|
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
|
||||||
|
return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
|
||||||
|
}
|
||||||
|
|
||||||
|
namespace testing {
|
||||||
|
// Create default parameters with Z-down and above noise parameters
|
||||||
|
static boost::shared_ptr<PreintegrationParams> Params() {
|
||||||
|
auto p = PreintegrationParams::MakeSharedD(kGravity);
|
||||||
|
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||||
|
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||||
|
p->integrationCovariance = 0.0001 * I_3x3;
|
||||||
|
return p;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(PreintegrationBase, UpdateEstimate1) {
|
||||||
|
PreintegrationBase pim(testing::Params());
|
||||||
|
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
|
||||||
|
Vector9 zeta;
|
||||||
|
zeta.setZero();
|
||||||
|
Matrix9 aH1;
|
||||||
|
Matrix93 aH2, aH3;
|
||||||
|
pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
|
||||||
|
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-9));
|
||||||
|
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-9));
|
||||||
|
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(PreintegrationBase, UpdateEstimate2) {
|
||||||
|
PreintegrationBase pim(testing::Params());
|
||||||
|
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
|
||||||
|
Vector9 zeta;
|
||||||
|
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
|
||||||
|
Matrix9 aH1;
|
||||||
|
Matrix93 aH2, aH3;
|
||||||
|
pim.UpdatePreintegrated(acc, omega, kDt, zeta, aH1, aH2, aH3);
|
||||||
|
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
|
||||||
|
EXPECT(assert_equal(numericalDerivative31(f, zeta, acc, omega), aH1, 1e-3));
|
||||||
|
EXPECT(assert_equal(numericalDerivative32(f, zeta, acc, omega), aH2, 1e-8));
|
||||||
|
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(PreintegrationBase, computeError) {
|
||||||
|
PreintegrationBase pim(testing::Params());
|
||||||
|
NavState x1, x2;
|
||||||
|
imuBias::ConstantBias bias;
|
||||||
|
Matrix9 aH1, aH2;
|
||||||
|
Matrix96 aH3;
|
||||||
|
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
|
||||||
|
boost::function<Vector9(const NavState&, const NavState&,
|
||||||
|
const imuBias::ConstantBias&)> f =
|
||||||
|
boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3,
|
||||||
|
boost::none, boost::none, boost::none);
|
||||||
|
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
|
||||||
|
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
|
||||||
|
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
|
||||||
|
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(PreintegrationBase, Compose) {
|
||||||
|
testing::SomeMeasurements measurements;
|
||||||
|
PreintegrationBase pim(testing::Params());
|
||||||
|
testing::integrateMeasurements(measurements, &pim);
|
||||||
|
|
||||||
|
boost::function<Vector9(const Vector9&, const Vector9&)> f =
|
||||||
|
[pim](const Vector9& zeta01, const Vector9& zeta12) {
|
||||||
|
return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij());
|
||||||
|
};
|
||||||
|
|
||||||
|
// Expected merge result
|
||||||
|
PreintegrationBase expected_pim02(testing::Params());
|
||||||
|
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||||
|
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||||
|
|
||||||
|
// Actual result
|
||||||
|
Matrix9 H1, H2;
|
||||||
|
PreintegrationBase actual_pim02 = pim;
|
||||||
|
actual_pim02.mergeWith(pim, &H1, &H2);
|
||||||
|
|
||||||
|
const Vector9 zeta = pim.preintegrated();
|
||||||
|
const Vector9 actual_zeta =
|
||||||
|
PreintegrationBase::Compose(zeta, zeta, pim.deltaTij());
|
||||||
|
EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7));
|
||||||
|
EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7));
|
||||||
|
EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(PreintegrationBase, MergedBiasDerivatives) {
|
||||||
|
testing::SomeMeasurements measurements;
|
||||||
|
|
||||||
|
boost::function<Vector9(const Vector3&, const Vector3&)> f =
|
||||||
|
[=](const Vector3& a, const Vector3& w) {
|
||||||
|
PreintegrationBase pim02(testing::Params(), Bias(a, w));
|
||||||
|
testing::integrateMeasurements(measurements, &pim02);
|
||||||
|
testing::integrateMeasurements(measurements, &pim02);
|
||||||
|
return pim02.preintegrated();
|
||||||
|
};
|
||||||
|
|
||||||
|
// Expected merge result
|
||||||
|
PreintegrationBase expected_pim02(testing::Params());
|
||||||
|
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||||
|
testing::integrateMeasurements(measurements, &expected_pim02);
|
||||||
|
|
||||||
|
EXPECT(assert_equal(numericalDerivative21(f, Z_3x1, Z_3x1),
|
||||||
|
expected_pim02.preintegrated_H_biasAcc()));
|
||||||
|
EXPECT(assert_equal(numericalDerivative22(f, Z_3x1, Z_3x1),
|
||||||
|
expected_pim02.preintegrated_H_biasOmega(), 1e-7));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -0,0 +1,137 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file testScenario.cpp
|
||||||
|
* @brief Unit test Scenario class
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/navigation/Scenario.h>
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <boost/bind.hpp>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
static const double kDegree = M_PI / 180.0;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Spin) {
|
||||||
|
// angular velocity 6 kDegree/sec
|
||||||
|
const double w = 6 * kDegree;
|
||||||
|
const Vector3 W(0, 0, w), V(0, 0, 0);
|
||||||
|
const ConstantTwistScenario scenario(W, V);
|
||||||
|
|
||||||
|
const double T = 10;
|
||||||
|
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||||
|
|
||||||
|
const Pose3 T10 = scenario.pose(T);
|
||||||
|
EXPECT(assert_equal(Vector3(0, 0, 60 * kDegree), T10.rotation().xyz(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(0, 0, 0), T10.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Forward) {
|
||||||
|
const double v = 2; // m/s
|
||||||
|
const Vector3 W(0, 0, 0), V(v, 0, 0);
|
||||||
|
const ConstantTwistScenario scenario(W, V);
|
||||||
|
|
||||||
|
const double T = 15;
|
||||||
|
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||||
|
|
||||||
|
const Pose3 T15 = scenario.pose(T);
|
||||||
|
EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Circle) {
|
||||||
|
// Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z
|
||||||
|
const double v = 2, w = 6 * kDegree;
|
||||||
|
const Vector3 W(0, 0, w), V(v, 0, 0);
|
||||||
|
const ConstantTwistScenario scenario(W, V);
|
||||||
|
|
||||||
|
const double T = 15;
|
||||||
|
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||||
|
|
||||||
|
// R = v/w, so test if circle is of right size
|
||||||
|
const double R = v / w;
|
||||||
|
const Pose3 T15 = scenario.pose(T);
|
||||||
|
EXPECT(assert_equal(Vector3(0, 0, 90 * kDegree), T15.rotation().xyz(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Loop) {
|
||||||
|
// Forward velocity 2m/s
|
||||||
|
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
||||||
|
const double v = 2, w = 6 * kDegree;
|
||||||
|
const Vector3 W(0, -w, 0), V(v, 0, 0);
|
||||||
|
const ConstantTwistScenario scenario(W, V);
|
||||||
|
|
||||||
|
const double T = 30;
|
||||||
|
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));
|
||||||
|
|
||||||
|
// R = v/w, so test if loop crests at 2*R
|
||||||
|
const double R = v / w;
|
||||||
|
const Pose3 T30 = scenario.pose(30);
|
||||||
|
EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Scenario, Accelerating) {
|
||||||
|
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||||
|
// going in X. The body itself has Z axis pointing down
|
||||||
|
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||||
|
const Point3 P0(10, 20, 0);
|
||||||
|
const Vector3 V0(50, 0, 0);
|
||||||
|
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||||
|
|
||||||
|
const double T = 3;
|
||||||
|
EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
|
||||||
|
EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));
|
||||||
|
|
||||||
|
{
|
||||||
|
// Check acceleration in nav
|
||||||
|
Matrix expected = numericalDerivative11<Vector3, double>(
|
||||||
|
boost::bind(&Scenario::velocity_n, scenario, _1), T);
|
||||||
|
EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
const Pose3 T3 = scenario.pose(3);
|
||||||
|
EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
|
||||||
|
EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
|
||||||
|
T3.translation(), 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -0,0 +1,385 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file testScenarioRunner.cpp
|
||||||
|
* @brief test ImuFacor with ScenarioRunner class
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/navigation/ScenarioRunner.h>
|
||||||
|
#include <gtsam/base/timing.h>
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
static const double kDegree = M_PI / 180.0;
|
||||||
|
static const double kDt = 1e-2;
|
||||||
|
|
||||||
|
// realistic white noise strengths are 0.5 deg/sqrt(hr) and 0.1 (m/s)/sqrt(h)
|
||||||
|
static const double kGyroSigma = 0.5 * kDegree / 60;
|
||||||
|
static const double kAccelSigma = 0.1 / 60.0;
|
||||||
|
|
||||||
|
static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
|
||||||
|
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
|
||||||
|
|
||||||
|
// Create default parameters with Z-up and above noise parameters
|
||||||
|
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
|
||||||
|
auto p = PreintegrationParams::MakeSharedU(10);
|
||||||
|
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||||
|
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||||
|
p->integrationCovariance = 0.0000001 * I_3x3;
|
||||||
|
return p;
|
||||||
|
}
|
||||||
|
|
||||||
|
#define EXPECT_NEAR(a, b, c) EXPECT(assert_equal(Vector(a), Vector(b), c));
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Spin) {
|
||||||
|
// angular velocity 6 degree/sec
|
||||||
|
const double w = 6 * kDegree;
|
||||||
|
const Vector3 W(0, 0, w), V(0, 0, 0);
|
||||||
|
const ConstantTwistScenario scenario(W, V);
|
||||||
|
|
||||||
|
auto p = defaultParams();
|
||||||
|
ScenarioRunner runner(&scenario, p, kDt);
|
||||||
|
const double T = 2 * kDt; // seconds
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
// Check sampled noise is kosher
|
||||||
|
Matrix6 expected;
|
||||||
|
expected << p->accelerometerCovariance / kDt, Z_3x3, //
|
||||||
|
Z_3x3, p->gyroscopeCovariance / kDt;
|
||||||
|
Matrix6 actual = runner.estimateNoiseCovariance(10000);
|
||||||
|
EXPECT(assert_equal(expected, actual, 1e-2));
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Check calculated covariance against Monte Carlo estimate
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace forward {
|
||||||
|
const double v = 2; // m/s
|
||||||
|
ConstantTwistScenario scenario(Vector3::Zero(), Vector3(v, 0, 0));
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Forward) {
|
||||||
|
using namespace forward;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||||
|
const double T = 0.1; // seconds
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, ForwardWithBias) {
|
||||||
|
using namespace forward;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||||
|
const double T = 0.1; // seconds
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T, kNonZeroBias);
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T, 1000, kNonZeroBias);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Circle) {
|
||||||
|
// Forward velocity 2m/s, angular velocity 6 degree/sec
|
||||||
|
const double v = 2, w = 6 * kDegree;
|
||||||
|
ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||||
|
const double T = 0.1; // seconds
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Loop) {
|
||||||
|
// Forward velocity 2m/s
|
||||||
|
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||||
|
const double v = 2, w = 6 * kDegree;
|
||||||
|
ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||||
|
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), kDt);
|
||||||
|
const double T = 0.1; // seconds
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 0.1));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace initial {
|
||||||
|
const Rot3 nRb;
|
||||||
|
const Point3 P0;
|
||||||
|
const Vector3 V0(0, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace accelerating {
|
||||||
|
using namespace initial;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A);
|
||||||
|
|
||||||
|
const double T = 3; // seconds
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Accelerating) {
|
||||||
|
using namespace accelerating;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingWithBias) {
|
||||||
|
using namespace accelerating;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T, kNonZeroBias);
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingAndRotating) {
|
||||||
|
using namespace initial;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0), W(0, 0.1, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||||
|
|
||||||
|
const double T = 10; // seconds
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace initial2 {
|
||||||
|
// No rotation, but non-zero position and velocities
|
||||||
|
const Rot3 nRb;
|
||||||
|
const Point3 P0(10, 20, 0);
|
||||||
|
const Vector3 V0(50, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace accelerating2 {
|
||||||
|
using namespace initial2;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A);
|
||||||
|
|
||||||
|
const double T = 3; // seconds
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Accelerating2) {
|
||||||
|
using namespace accelerating2;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingWithBias2) {
|
||||||
|
using namespace accelerating2;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T, kNonZeroBias);
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingAndRotating2) {
|
||||||
|
using namespace initial2;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0), W(0, 0.1, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||||
|
|
||||||
|
const double T = 10; // seconds
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace initial3 {
|
||||||
|
// Rotation only
|
||||||
|
// Set up body pointing towards y axis. The body itself has Z axis pointing down
|
||||||
|
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||||
|
const Point3 P0;
|
||||||
|
const Vector3 V0(0, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace accelerating3 {
|
||||||
|
using namespace initial3;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A);
|
||||||
|
|
||||||
|
const double T = 3; // seconds
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Accelerating3) {
|
||||||
|
using namespace accelerating3;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingWithBias3) {
|
||||||
|
using namespace accelerating3;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T, kNonZeroBias);
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingAndRotating3) {
|
||||||
|
using namespace initial3;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0), W(0, 0.1, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||||
|
|
||||||
|
const double T = 10; // seconds
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace initial4 {
|
||||||
|
// Both rotation and translation
|
||||||
|
// Set up body pointing towards y axis, and start at 10,20,0 with velocity
|
||||||
|
// going in X. The body itself has Z axis pointing down
|
||||||
|
const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||||
|
const Point3 P0(10, 20, 0);
|
||||||
|
const Vector3 V0(50, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
namespace accelerating4 {
|
||||||
|
using namespace initial4;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A);
|
||||||
|
|
||||||
|
const double T = 3; // seconds
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, Accelerating4) {
|
||||||
|
using namespace accelerating4;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingWithBias4) {
|
||||||
|
using namespace accelerating4;
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10, kNonZeroBias);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T, kNonZeroBias);
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T, 10000, kNonZeroBias);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(ScenarioRunner, AcceleratingAndRotating4) {
|
||||||
|
using namespace initial4;
|
||||||
|
const double a = 0.2; // m/s^2
|
||||||
|
const Vector3 A(0, a, 0), W(0, 0.1, 0);
|
||||||
|
const AcceleratingScenario scenario(nRb, P0, V0, A, W);
|
||||||
|
|
||||||
|
const double T = 10; // seconds
|
||||||
|
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
|
||||||
|
|
||||||
|
auto pim = runner.integrate(T);
|
||||||
|
EXPECT(assert_equal(scenario.pose(T), runner.predict(pim).pose(), 1e-9));
|
||||||
|
|
||||||
|
Matrix9 estimatedCov = runner.estimateCovariance(T);
|
||||||
|
EXPECT_NEAR(estimatedCov.diagonal(), pim.preintMeasCov().diagonal(), 0.1);
|
||||||
|
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
auto result = TestRegistry::runAllTests(tr);
|
||||||
|
tictoc_print_();
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -24,90 +24,6 @@ Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
|
||||||
return Expression<T>(traits<T>::Compose, t1, t2);
|
return Expression<T>(traits<T>::Compose, t1, t2);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* Functor that implements multiplication of a vector b with the inverse of a
|
|
||||||
* matrix A. The derivatives are inspired by Mike Giles' "An extended collection
|
|
||||||
* of matrix derivative results for forward and reverse mode algorithmic
|
|
||||||
* differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf
|
|
||||||
*
|
|
||||||
* Usage example:
|
|
||||||
* Expression<Vector3> expression = MultiplyWithInverse<3>()(Key(0), Key(1));
|
|
||||||
*/
|
|
||||||
template <int N>
|
|
||||||
struct MultiplyWithInverse {
|
|
||||||
typedef Eigen::Matrix<double, N, 1> VectorN;
|
|
||||||
typedef Eigen::Matrix<double, N, N> MatrixN;
|
|
||||||
|
|
||||||
/// A.inverse() * b, with optional derivatives
|
|
||||||
VectorN operator()(const MatrixN& A, const VectorN& b,
|
|
||||||
OptionalJacobian<N, N* N> H1 = boost::none,
|
|
||||||
OptionalJacobian<N, N> H2 = boost::none) const {
|
|
||||||
const MatrixN invA = A.inverse();
|
|
||||||
const VectorN c = invA * b;
|
|
||||||
// The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
|
|
||||||
if (H1)
|
|
||||||
for (size_t j = 0; j < N; j++)
|
|
||||||
H1->template middleCols<N>(N * j) = -c[j] * invA;
|
|
||||||
// The derivative in b is easy, as invA*b is just a linear map:
|
|
||||||
if (H2) *H2 = invA;
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Create expression
|
|
||||||
Expression<VectorN> operator()(const Expression<MatrixN>& A_,
|
|
||||||
const Expression<VectorN>& b_) const {
|
|
||||||
return Expression<VectorN>(*this, A_, b_);
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Functor that implements multiplication with the inverse of a matrix, itself
|
|
||||||
* the result of a function f. It turn out we only need the derivatives of the
|
|
||||||
* operator phi(a): b -> f(a) * b
|
|
||||||
*/
|
|
||||||
template <typename T, int N>
|
|
||||||
struct MultiplyWithInverseFunction {
|
|
||||||
enum { M = traits<T>::dimension };
|
|
||||||
typedef Eigen::Matrix<double, N, 1> VectorN;
|
|
||||||
typedef Eigen::Matrix<double, N, N> MatrixN;
|
|
||||||
|
|
||||||
// The function phi should calculate f(a)*b, with derivatives in a and b.
|
|
||||||
// Naturally, the derivative in b is f(a).
|
|
||||||
typedef boost::function<VectorN(
|
|
||||||
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
|
|
||||||
Operator;
|
|
||||||
|
|
||||||
/// Construct with function as explained above
|
|
||||||
MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}
|
|
||||||
|
|
||||||
/// f(a).inverse() * b, with optional derivatives
|
|
||||||
VectorN operator()(const T& a, const VectorN& b,
|
|
||||||
OptionalJacobian<N, M> H1 = boost::none,
|
|
||||||
OptionalJacobian<N, N> H2 = boost::none) const {
|
|
||||||
MatrixN A;
|
|
||||||
phi_(a, b, boost::none, A); // get A = f(a) by calling f once
|
|
||||||
const MatrixN invA = A.inverse();
|
|
||||||
const VectorN c = invA * b;
|
|
||||||
|
|
||||||
if (H1) {
|
|
||||||
Eigen::Matrix<double, N, M> H;
|
|
||||||
phi_(a, c, H, boost::none); // get derivative H of forward mapping
|
|
||||||
*H1 = -invA* H;
|
|
||||||
}
|
|
||||||
if (H2) *H2 = invA;
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Create expression
|
|
||||||
Expression<VectorN> operator()(const Expression<T>& a_,
|
|
||||||
const Expression<VectorN>& b_) const {
|
|
||||||
return Expression<VectorN>(*this, a_, b_);
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
const Operator phi_;
|
|
||||||
};
|
|
||||||
|
|
||||||
// Some typedefs
|
// Some typedefs
|
||||||
typedef Expression<double> double_;
|
typedef Expression<double> double_;
|
||||||
typedef Expression<Vector1> Vector1_;
|
typedef Expression<Vector1> Vector1_;
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,105 @@
|
||||||
|
"""
|
||||||
|
A script validating the ImuFactor inference.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import print_function
|
||||||
|
import math
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from mpl_toolkits.mplot3d import Axes3D
|
||||||
|
|
||||||
|
import gtsam
|
||||||
|
from gtsam_utils import plotPose3
|
||||||
|
from PreintegrationExample import PreintegrationExample, POSES_FIG
|
||||||
|
|
||||||
|
# shorthand symbols:
|
||||||
|
BIAS_KEY = int(gtsam.Symbol('b', 0))
|
||||||
|
V = lambda j: int(gtsam.Symbol('v', j))
|
||||||
|
X = lambda i: int(gtsam.Symbol('x', i))
|
||||||
|
|
||||||
|
class ImuFactorExample(PreintegrationExample):
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.velocity = np.array([2, 0, 0])
|
||||||
|
self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
|
||||||
|
self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
|
||||||
|
|
||||||
|
forward_twist = (np.zeros(3), self.velocity)
|
||||||
|
loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
|
||||||
|
|
||||||
|
accBias = np.array([-0.3, 0.1, 0.2])
|
||||||
|
gyroBias = np.array([0.1, 0.3, -0.1])
|
||||||
|
bias = gtsam.ConstantBias(accBias, gyroBias)
|
||||||
|
|
||||||
|
super(ImuFactorExample, self).__init__(loop_twist, bias)
|
||||||
|
|
||||||
|
def addPrior(self, i, graph):
|
||||||
|
state = self.scenario.navState(i)
|
||||||
|
graph.push_back(gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
|
||||||
|
graph.push_back(gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise))
|
||||||
|
|
||||||
|
def run(self):
|
||||||
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
|
|
||||||
|
i = 0 # state index
|
||||||
|
|
||||||
|
# initialize data structure for pre-integrated IMU measurements
|
||||||
|
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
|
||||||
|
|
||||||
|
# simulate the loop
|
||||||
|
T = 12
|
||||||
|
actual_state_i = self.scenario.navState(0)
|
||||||
|
for k, t in enumerate(np.arange(0, T, self.dt)):
|
||||||
|
# get measurements and add them to PIM
|
||||||
|
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||||
|
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||||
|
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
|
||||||
|
|
||||||
|
# Plot IMU many times
|
||||||
|
if k % 10 == 0:
|
||||||
|
self.plotImu(t, measuredOmega, measuredAcc)
|
||||||
|
|
||||||
|
# Plot every second
|
||||||
|
if k % 100 == 0:
|
||||||
|
self.plotGroundTruthPose(t)
|
||||||
|
|
||||||
|
# create IMU factor every second
|
||||||
|
if (k + 1) % 100 == 0:
|
||||||
|
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim)
|
||||||
|
graph.push_back(factor)
|
||||||
|
pim.resetIntegration()
|
||||||
|
actual_state_i = self.scenario.navState(t + self.dt)
|
||||||
|
i += 1
|
||||||
|
|
||||||
|
# add priors on beginning and end
|
||||||
|
num_poses = i + 1
|
||||||
|
self.addPrior(0, graph)
|
||||||
|
self.addPrior(num_poses - 1, graph)
|
||||||
|
|
||||||
|
initial = gtsam.Values()
|
||||||
|
initial.insert(BIAS_KEY, self.actualBias)
|
||||||
|
for i in range(num_poses):
|
||||||
|
state_i = self.scenario.navState(float(i))
|
||||||
|
initial.insert(X(i), state_i.pose())
|
||||||
|
initial.insert(V(i), state_i.velocity())
|
||||||
|
|
||||||
|
# optimize using Levenberg-Marquardt optimization
|
||||||
|
params = gtsam.LevenbergMarquardtParams()
|
||||||
|
params.setVerbosityLM("SUMMARY")
|
||||||
|
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
|
||||||
|
result = optimizer.optimize()
|
||||||
|
|
||||||
|
# Plot resulting poses
|
||||||
|
i = 0
|
||||||
|
while result.exists(X(i)):
|
||||||
|
pose_i = result.atPose3(X(i))
|
||||||
|
plotPose3(POSES_FIG, pose_i, 0.1)
|
||||||
|
i += 1
|
||||||
|
print(result.atConstantBias(BIAS_KEY))
|
||||||
|
|
||||||
|
plt.ioff()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
ImuFactorExample().run()
|
||||||
|
|
@ -0,0 +1,129 @@
|
||||||
|
"""
|
||||||
|
A script validating the Preintegration of IMU measurements
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
import matplotlib.pyplot as plt
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from mpl_toolkits.mplot3d import Axes3D
|
||||||
|
|
||||||
|
import gtsam
|
||||||
|
from gtsam_utils import plotPose3
|
||||||
|
|
||||||
|
IMU_FIG = 1
|
||||||
|
POSES_FIG = 2
|
||||||
|
|
||||||
|
class PreintegrationExample(object):
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def defaultParams(g):
|
||||||
|
"""Create default parameters with Z *up* and realistic noise parameters"""
|
||||||
|
params = gtsam.PreintegrationParams.MakeSharedU(g)
|
||||||
|
kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW
|
||||||
|
kAccelSigma = 0.1 / 60 # 10 cm VRW
|
||||||
|
params.gyroscopeCovariance = kGyroSigma ** 2 * np.identity(3, np.float)
|
||||||
|
params.accelerometerCovariance = kAccelSigma ** 2 * np.identity(3, np.float)
|
||||||
|
params.integrationCovariance = 0.0000001 ** 2 * np.identity(3, np.float)
|
||||||
|
return params
|
||||||
|
|
||||||
|
def __init__(self, twist=None, bias=None):
|
||||||
|
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
|
||||||
|
|
||||||
|
# setup interactive plotting
|
||||||
|
plt.ion()
|
||||||
|
|
||||||
|
# Setup loop as default scenario
|
||||||
|
if twist is not None:
|
||||||
|
(W, V) = twist
|
||||||
|
else:
|
||||||
|
# default = loop with forward velocity 2m/s, while pitching up
|
||||||
|
# with angular velocity 30 degree/sec (negative in FLU)
|
||||||
|
W = np.array([0, -math.radians(30), 0])
|
||||||
|
V = np.array([2, 0, 0])
|
||||||
|
|
||||||
|
self.scenario = gtsam.ConstantTwistScenario(W, V)
|
||||||
|
self.dt = 1e-2
|
||||||
|
|
||||||
|
self.maxDim = 5
|
||||||
|
self.labels = list('xyz')
|
||||||
|
self.colors = list('rgb')
|
||||||
|
|
||||||
|
# Create runner
|
||||||
|
self.g = 10 # simple gravity constant
|
||||||
|
self.params = self.defaultParams(self.g)
|
||||||
|
ptr = gtsam.ScenarioPointer(self.scenario)
|
||||||
|
|
||||||
|
if bias is not None:
|
||||||
|
self.actualBias = bias
|
||||||
|
else:
|
||||||
|
accBias = np.array([0, 0.1, 0])
|
||||||
|
gyroBias = np.array([0, 0, 0])
|
||||||
|
self.actualBias = gtsam.ConstantBias(accBias, gyroBias)
|
||||||
|
|
||||||
|
self.runner = gtsam.ScenarioRunner(ptr, self.params, self.dt, self.actualBias)
|
||||||
|
|
||||||
|
def plotImu(self, t, measuredOmega, measuredAcc):
|
||||||
|
plt.figure(IMU_FIG)
|
||||||
|
|
||||||
|
# plot angular velocity
|
||||||
|
omega_b = self.scenario.omega_b(t)
|
||||||
|
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||||
|
plt.subplot(4, 3, i + 1)
|
||||||
|
plt.scatter(t, omega_b[i], color='k', marker='.')
|
||||||
|
plt.scatter(t, measuredOmega[i], color=color, marker='.')
|
||||||
|
plt.xlabel('angular velocity ' + label)
|
||||||
|
|
||||||
|
# plot acceleration in nav
|
||||||
|
acceleration_n = self.scenario.acceleration_n(t)
|
||||||
|
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||||
|
plt.subplot(4, 3, i + 4)
|
||||||
|
plt.scatter(t, acceleration_n[i], color=color, marker='.')
|
||||||
|
plt.xlabel('acceleration in nav ' + label)
|
||||||
|
|
||||||
|
# plot acceleration in body
|
||||||
|
acceleration_b = self.scenario.acceleration_b(t)
|
||||||
|
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||||
|
plt.subplot(4, 3, i + 7)
|
||||||
|
plt.scatter(t, acceleration_b[i], color=color, marker='.')
|
||||||
|
plt.xlabel('acceleration in body ' + label)
|
||||||
|
|
||||||
|
# plot actual specific force, as well as corrupted
|
||||||
|
actual = self.runner.actualSpecificForce(t)
|
||||||
|
for i, (label, color) in enumerate(zip(self.labels, self.colors)):
|
||||||
|
plt.subplot(4, 3, i + 10)
|
||||||
|
plt.scatter(t, actual[i], color='k', marker='.')
|
||||||
|
plt.scatter(t, measuredAcc[i], color=color, marker='.')
|
||||||
|
plt.xlabel('specific force ' + label)
|
||||||
|
|
||||||
|
def plotGroundTruthPose(self, t):
|
||||||
|
# plot ground truth pose, as well as prediction from integrated IMU measurements
|
||||||
|
actualPose = self.scenario.pose(t)
|
||||||
|
plotPose3(POSES_FIG, actualPose, 0.3)
|
||||||
|
t = actualPose.translation()
|
||||||
|
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
|
||||||
|
ax = plt.gca()
|
||||||
|
ax.set_xlim3d(-self.maxDim, self.maxDim)
|
||||||
|
ax.set_ylim3d(-self.maxDim, self.maxDim)
|
||||||
|
ax.set_zlim3d(-self.maxDim, self.maxDim)
|
||||||
|
|
||||||
|
plt.pause(0.01)
|
||||||
|
|
||||||
|
def run(self):
|
||||||
|
# simulate the loop
|
||||||
|
T = 12
|
||||||
|
for i, t in enumerate(np.arange(0, T, self.dt)):
|
||||||
|
measuredOmega = self.runner.measuredAngularVelocity(t)
|
||||||
|
measuredAcc = self.runner.measuredSpecificForce(t)
|
||||||
|
if i % 25 == 0:
|
||||||
|
self.plotImu(t, measuredOmega, measuredAcc)
|
||||||
|
self.plotGroundTruthPose(t)
|
||||||
|
pim = self.runner.integrate(t, self.actualBias, True)
|
||||||
|
predictedNavState = self.runner.predict(pim, self.actualBias)
|
||||||
|
plotPose3(POSES_FIG, predictedNavState.pose(), 0.1)
|
||||||
|
|
||||||
|
plt.ioff()
|
||||||
|
plt.show()
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
PreintegrationExample().run()
|
||||||
|
|
@ -0,0 +1,32 @@
|
||||||
|
import math
|
||||||
|
import unittest
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
import gtsam
|
||||||
|
|
||||||
|
class TestScenario(unittest.TestCase):
|
||||||
|
def setUp(self):
|
||||||
|
pass
|
||||||
|
|
||||||
|
def test_loop(self):
|
||||||
|
# Forward velocity 2m/s
|
||||||
|
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||||
|
v = 2
|
||||||
|
w = math.radians(6)
|
||||||
|
W = np.array([0, -w, 0])
|
||||||
|
V = np.array([v, 0, 0])
|
||||||
|
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||||
|
|
||||||
|
T = 30
|
||||||
|
np.testing.assert_almost_equal(W, scenario.omega_b(T))
|
||||||
|
np.testing.assert_almost_equal(V, scenario.velocity_b(T))
|
||||||
|
np.testing.assert_almost_equal(np.cross(W, V), scenario.acceleration_b(T))
|
||||||
|
|
||||||
|
# R = v/w, so test if loop crests at 2*R
|
||||||
|
R = v / w
|
||||||
|
T30 = scenario.pose(T)
|
||||||
|
np.testing.assert_almost_equal(np.array([-math.pi, 0, -math.pi]), T30.rotation().xyz())
|
||||||
|
self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation()))
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
unittest.main()
|
||||||
|
|
@ -0,0 +1,30 @@
|
||||||
|
import math
|
||||||
|
import unittest
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
import gtsam
|
||||||
|
|
||||||
|
class TestScenarioRunner(unittest.TestCase):
|
||||||
|
def setUp(self):
|
||||||
|
self.g = 10 # simple gravity constant
|
||||||
|
|
||||||
|
def test_loop_runner(self):
|
||||||
|
# Forward velocity 2m/s
|
||||||
|
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||||
|
v = 2
|
||||||
|
w = math.radians(6)
|
||||||
|
W = np.array([0, -w, 0])
|
||||||
|
V = np.array([v, 0, 0])
|
||||||
|
scenario = gtsam.ConstantTwistScenario(W, V)
|
||||||
|
|
||||||
|
dt = 0.1
|
||||||
|
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
|
||||||
|
runner = gtsam.ScenarioRunner(gtsam.ScenarioPointer(scenario), params, dt)
|
||||||
|
|
||||||
|
# Test specific force at time 0: a is pointing up
|
||||||
|
t = 0.0
|
||||||
|
a = w * v
|
||||||
|
np.testing.assert_almost_equal(np.array([0, 0, a + self.g]), runner.actualSpecificForce(t))
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
unittest.main()
|
||||||
|
|
@ -1,59 +0,0 @@
|
||||||
import numpy as _np
|
|
||||||
import matplotlib.pyplot as _plt
|
|
||||||
from mpl_toolkits.mplot3d import Axes3D as _Axes3D
|
|
||||||
|
|
||||||
def plotPoint3(fignum, point, linespec):
|
|
||||||
fig = _plt.figure(fignum)
|
|
||||||
ax = fig.gca(projection='3d')
|
|
||||||
ax.plot([point.x()],[point.y()],[point.z()], linespec)
|
|
||||||
|
|
||||||
|
|
||||||
def plot3DPoints(fignum, values, linespec, marginals=None):
|
|
||||||
# PLOT3DPOINTS Plots the Point3's in a values, with optional covariances
|
|
||||||
# Finds all the Point3 objects in the given Values object and plots them.
|
|
||||||
# If a Marginals object is given, this function will also plot marginal
|
|
||||||
# covariance ellipses for each point.
|
|
||||||
|
|
||||||
keys = values.keys()
|
|
||||||
|
|
||||||
# Plot points and covariance matrices
|
|
||||||
for key in keys:
|
|
||||||
try:
|
|
||||||
p = values.point3_at(key);
|
|
||||||
# if haveMarginals
|
|
||||||
# P = marginals.marginalCovariance(key);
|
|
||||||
# gtsam.plotPoint3(p, linespec, P);
|
|
||||||
# else
|
|
||||||
plotPoint3(fignum, p, linespec);
|
|
||||||
except RuntimeError:
|
|
||||||
continue
|
|
||||||
#I guess it's not a Point3
|
|
||||||
|
|
||||||
def plotPose3(fignum, pose, axisLength=0.1):
|
|
||||||
# get figure object
|
|
||||||
fig = _plt.figure(fignum)
|
|
||||||
ax = fig.gca(projection='3d')
|
|
||||||
|
|
||||||
# get rotation and translation (center)
|
|
||||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
|
||||||
C = pose.translation().vector()
|
|
||||||
|
|
||||||
# draw the camera axes
|
|
||||||
xAxis = C+gRp[:,0]*axisLength
|
|
||||||
L = _np.append(C[_np.newaxis], xAxis[_np.newaxis], axis=0)
|
|
||||||
ax.plot(L[:,0],L[:,1],L[:,2],'r-')
|
|
||||||
|
|
||||||
yAxis = C+gRp[:,1]*axisLength
|
|
||||||
L = _np.append(C[_np.newaxis], yAxis[_np.newaxis], axis=0)
|
|
||||||
ax.plot(L[:,0],L[:,1],L[:,2],'g-')
|
|
||||||
|
|
||||||
zAxis = C+gRp[:,2]*axisLength
|
|
||||||
L = _np.append(C[_np.newaxis], zAxis[_np.newaxis], axis=0)
|
|
||||||
ax.plot(L[:,0],L[:,1],L[:,2],'b-')
|
|
||||||
|
|
||||||
# # plot the covariance
|
|
||||||
# if (nargin>2) && (~isempty(P))
|
|
||||||
# pPp = P(4:6,4:6); % covariance matrix in pose coordinate frame
|
|
||||||
# gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
|
|
||||||
# gtsam.covarianceEllipse3D(C,gPp);
|
|
||||||
# end
|
|
||||||
|
|
@ -1,6 +1,6 @@
|
||||||
/* ----------------------------------------------------------------------------
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
* Atlanta, Georgia 30332-0415
|
* Atlanta, Georgia 30332-0415
|
||||||
* All Rights Reserved
|
* All Rights Reserved
|
||||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
@ -10,8 +10,8 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief exports the python module
|
* @brief exports the python module
|
||||||
* @author Andrew Melim
|
* @author Andrew Melim
|
||||||
* @author Ellon Paiva Mendes (LAAS-CNRS)
|
* @author Ellon Paiva Mendes (LAAS-CNRS)
|
||||||
**/
|
**/
|
||||||
|
|
||||||
|
|
@ -20,10 +20,10 @@
|
||||||
|
|
||||||
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||||
|
|
||||||
// Base
|
// base
|
||||||
void exportFastVectors();
|
void exportFastVectors();
|
||||||
|
|
||||||
// Geometry
|
// geometry
|
||||||
void exportPoint2();
|
void exportPoint2();
|
||||||
void exportPoint3();
|
void exportPoint3();
|
||||||
void exportRot2();
|
void exportRot2();
|
||||||
|
|
@ -34,24 +34,29 @@ void exportPinholeBaseK();
|
||||||
void exportPinholeCamera();
|
void exportPinholeCamera();
|
||||||
void exportCal3_S2();
|
void exportCal3_S2();
|
||||||
|
|
||||||
// Inference
|
// inference
|
||||||
void exportSymbol();
|
void exportSymbol();
|
||||||
|
|
||||||
// Linear
|
// linear
|
||||||
void exportNoiseModels();
|
void exportNoiseModels();
|
||||||
|
|
||||||
// Nonlinear
|
// nonlinear
|
||||||
void exportValues();
|
void exportValues();
|
||||||
void exportNonlinearFactor();
|
void exportNonlinearFactor();
|
||||||
void exportNonlinearFactorGraph();
|
void exportNonlinearFactorGraph();
|
||||||
void exportLevenbergMarquardtOptimizer();
|
void exportLevenbergMarquardtOptimizer();
|
||||||
void exportISAM2();
|
void exportISAM2();
|
||||||
|
|
||||||
// Slam
|
// slam
|
||||||
void exportPriorFactors();
|
void exportPriorFactors();
|
||||||
void exportBetweenFactors();
|
void exportBetweenFactors();
|
||||||
void exportGenericProjectionFactor();
|
void exportGenericProjectionFactor();
|
||||||
|
|
||||||
|
// navigation
|
||||||
|
void exportImuFactor();
|
||||||
|
void exportScenario();
|
||||||
|
|
||||||
|
|
||||||
// Utils (or Python wrapper specific functions)
|
// Utils (or Python wrapper specific functions)
|
||||||
void registerNumpyEigenConversions();
|
void registerNumpyEigenConversions();
|
||||||
|
|
||||||
|
|
@ -59,14 +64,14 @@ void registerNumpyEigenConversions();
|
||||||
|
|
||||||
BOOST_PYTHON_MODULE(_libgtsam_python){
|
BOOST_PYTHON_MODULE(_libgtsam_python){
|
||||||
|
|
||||||
// NOTE: We need to call import_array1() instead of import_array() to support both python 2
|
// NOTE: We need to call import_array1() instead of import_array() to support both python 2
|
||||||
// and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function
|
// and 3. The reason is that BOOST_PYTHON_MODULE puts all its contents in a function
|
||||||
// returning void, and import_array() is a macro that when expanded for python 3, adds
|
// returning void, and import_array() is a macro that when expanded for python 3, adds
|
||||||
// a 'return __null' statement to that function. For more info check files:
|
// a 'return __null' statement to that function. For more info check files:
|
||||||
// boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file).
|
// boost/python/module_init.hpp and numpy/__multiarray_api.h (bottom of the file).
|
||||||
// Should be the first thing to be done
|
// Should be the first thing to be done
|
||||||
import_array1();
|
import_array1();
|
||||||
|
|
||||||
registerNumpyEigenConversions();
|
registerNumpyEigenConversions();
|
||||||
|
|
||||||
exportFastVectors();
|
exportFastVectors();
|
||||||
|
|
@ -94,4 +99,7 @@ BOOST_PYTHON_MODULE(_libgtsam_python){
|
||||||
exportPriorFactors();
|
exportPriorFactors();
|
||||||
exportBetweenFactors();
|
exportBetweenFactors();
|
||||||
exportGenericProjectionFactor();
|
exportGenericProjectionFactor();
|
||||||
}
|
|
||||||
|
exportImuFactor();
|
||||||
|
exportScenario();
|
||||||
|
}
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,94 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief wraps ConstantTwistScenario class to python
|
||||||
|
* @author Frank Dellaert
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include <boost/python.hpp>
|
||||||
|
|
||||||
|
#define NO_IMPORT_ARRAY
|
||||||
|
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||||
|
|
||||||
|
#include "gtsam/navigation/ImuFactor.h"
|
||||||
|
|
||||||
|
using namespace boost::python;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
typedef gtsam::OptionalJacobian<3, 9> OptionalJacobian39;
|
||||||
|
typedef gtsam::OptionalJacobian<9, 6> OptionalJacobian96;
|
||||||
|
typedef gtsam::OptionalJacobian<9, 9> OptionalJacobian9;
|
||||||
|
|
||||||
|
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, print, 0, 1)
|
||||||
|
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(attitude_overloads, attitude, 0, 1)
|
||||||
|
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(position_overloads, position, 0, 1)
|
||||||
|
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(velocity_overloads, velocity, 0, 1)
|
||||||
|
|
||||||
|
void exportImuFactor() {
|
||||||
|
class_<OptionalJacobian39>("OptionalJacobian39", init<>());
|
||||||
|
class_<OptionalJacobian96>("OptionalJacobian96", init<>());
|
||||||
|
class_<OptionalJacobian9>("OptionalJacobian9", init<>());
|
||||||
|
|
||||||
|
class_<NavState>("NavState", init<>())
|
||||||
|
// TODO(frank): overload with jacobians
|
||||||
|
.def("print", &NavState::print, print_overloads())
|
||||||
|
.def("attitude", &NavState::attitude,
|
||||||
|
attitude_overloads()[return_value_policy<copy_const_reference>()])
|
||||||
|
.def("position", &NavState::position,
|
||||||
|
position_overloads()[return_value_policy<copy_const_reference>()])
|
||||||
|
.def("velocity", &NavState::velocity,
|
||||||
|
velocity_overloads()[return_value_policy<copy_const_reference>()])
|
||||||
|
.def(repr(self))
|
||||||
|
.def("pose", &NavState::pose);
|
||||||
|
|
||||||
|
class_<imuBias::ConstantBias>("ConstantBias", init<>())
|
||||||
|
.def(init<const Vector3&, const Vector3&>())
|
||||||
|
.def(repr(self));
|
||||||
|
|
||||||
|
class_<PreintegrationParams, boost::shared_ptr<PreintegrationParams>>(
|
||||||
|
"PreintegrationParams", init<const Vector3&>())
|
||||||
|
.def_readwrite("gyroscopeCovariance",
|
||||||
|
&PreintegrationParams::gyroscopeCovariance)
|
||||||
|
.def_readwrite("omegaCoriolis", &PreintegrationParams::omegaCoriolis)
|
||||||
|
.def_readwrite("body_P_sensor", &PreintegrationParams::body_P_sensor)
|
||||||
|
.def_readwrite("accelerometerCovariance",
|
||||||
|
&PreintegrationParams::accelerometerCovariance)
|
||||||
|
.def_readwrite("integrationCovariance",
|
||||||
|
&PreintegrationParams::integrationCovariance)
|
||||||
|
.def_readwrite("use2ndOrderCoriolis",
|
||||||
|
&PreintegrationParams::use2ndOrderCoriolis)
|
||||||
|
.def_readwrite("n_gravity", &PreintegrationParams::n_gravity)
|
||||||
|
|
||||||
|
.def("MakeSharedD", &PreintegrationParams::MakeSharedD)
|
||||||
|
.staticmethod("MakeSharedD")
|
||||||
|
.def("MakeSharedU", &PreintegrationParams::MakeSharedU)
|
||||||
|
.staticmethod("MakeSharedU");
|
||||||
|
|
||||||
|
class_<PreintegratedImuMeasurements>(
|
||||||
|
"PreintegratedImuMeasurements",
|
||||||
|
init<const boost::shared_ptr<PreintegrationParams>&,
|
||||||
|
const imuBias::ConstantBias&>())
|
||||||
|
.def(repr(self))
|
||||||
|
.def("predict", &PreintegratedImuMeasurements::predict)
|
||||||
|
.def("computeError", &PreintegratedImuMeasurements::computeError)
|
||||||
|
.def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration)
|
||||||
|
.def("integrateMeasurement",
|
||||||
|
&PreintegratedImuMeasurements::integrateMeasurement)
|
||||||
|
.def("preintMeasCov", &PreintegratedImuMeasurements::preintMeasCov);
|
||||||
|
|
||||||
|
// NOTE(frank): Abstract classes need boost::noncopyable
|
||||||
|
class_<ImuFactor, bases<NonlinearFactor>, boost::shared_ptr<ImuFactor>>(
|
||||||
|
"ImuFactor")
|
||||||
|
.def("error", &ImuFactor::error)
|
||||||
|
.def(init<Key, Key, Key, Key, Key, const PreintegratedImuMeasurements&>())
|
||||||
|
.def(repr(self));
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,64 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief wraps ConstantTwistScenario class to python
|
||||||
|
* @author Frank Dellaert
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include <boost/python.hpp>
|
||||||
|
|
||||||
|
#define NO_IMPORT_ARRAY
|
||||||
|
#include <numpy_eigen/NumpyEigenConverter.hpp>
|
||||||
|
|
||||||
|
#include "gtsam/navigation/ScenarioRunner.h"
|
||||||
|
|
||||||
|
using namespace boost::python;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
// Create const Scenario pointer from ConstantTwistScenario
|
||||||
|
static const Scenario* ScenarioPointer(const ConstantTwistScenario& scenario) {
|
||||||
|
return static_cast<const Scenario*>(&scenario);
|
||||||
|
}
|
||||||
|
|
||||||
|
void exportScenario() {
|
||||||
|
// NOTE(frank): Abstract classes need boost::noncopyable
|
||||||
|
class_<Scenario, boost::noncopyable>("Scenario", no_init);
|
||||||
|
|
||||||
|
// TODO(frank): figure out how to do inheritance
|
||||||
|
class_<ConstantTwistScenario>("ConstantTwistScenario",
|
||||||
|
init<const Vector3&, const Vector3&>())
|
||||||
|
.def("pose", &Scenario::pose)
|
||||||
|
.def("omega_b", &Scenario::omega_b)
|
||||||
|
.def("velocity_n", &Scenario::velocity_n)
|
||||||
|
.def("acceleration_n", &Scenario::acceleration_n)
|
||||||
|
.def("navState", &Scenario::navState)
|
||||||
|
.def("rotation", &Scenario::rotation)
|
||||||
|
.def("velocity_b", &Scenario::velocity_b)
|
||||||
|
.def("acceleration_b", &Scenario::acceleration_b);
|
||||||
|
|
||||||
|
// NOTE(frank): https://wiki.python.org/moin/boost.python/CallPolicy
|
||||||
|
def("ScenarioPointer", &ScenarioPointer,
|
||||||
|
return_value_policy<reference_existing_object>());
|
||||||
|
|
||||||
|
class_<ScenarioRunner>(
|
||||||
|
"ScenarioRunner",
|
||||||
|
init<const Scenario*, const boost::shared_ptr<PreintegrationParams>&,
|
||||||
|
double, const imuBias::ConstantBias&>())
|
||||||
|
.def("actualSpecificForce", &ScenarioRunner::actualSpecificForce)
|
||||||
|
.def("measuredAngularVelocity", &ScenarioRunner::measuredAngularVelocity)
|
||||||
|
.def("measuredSpecificForce", &ScenarioRunner::measuredSpecificForce)
|
||||||
|
.def("imuSampleTime", &ScenarioRunner::imuSampleTime,
|
||||||
|
return_value_policy<copy_const_reference>())
|
||||||
|
.def("integrate", &ScenarioRunner::integrate)
|
||||||
|
.def("predict", &ScenarioRunner::predict)
|
||||||
|
.def("estimateCovariance", &ScenarioRunner::estimateCovariance);
|
||||||
|
}
|
||||||
|
|
@ -605,7 +605,7 @@ TEST(ExpressionFactor, MultiplyWithInverse) {
|
||||||
auto model = noiseModel::Isotropic::Sigma(3, 1);
|
auto model = noiseModel::Isotropic::Sigma(3, 1);
|
||||||
|
|
||||||
// Create expression
|
// Create expression
|
||||||
auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1));
|
Vector3_ f_expr(MultiplyWithInverse<3>(), Expression<Matrix3>(0), Vector3_(1));
|
||||||
|
|
||||||
// Check derivatives
|
// Check derivatives
|
||||||
Values values;
|
Values values;
|
||||||
|
|
@ -638,7 +638,8 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
|
||||||
auto model = noiseModel::Isotropic::Sigma(3, 1);
|
auto model = noiseModel::Isotropic::Sigma(3, 1);
|
||||||
|
|
||||||
using test_operator::f;
|
using test_operator::f;
|
||||||
auto f_expr = MultiplyWithInverseFunction<Point2, 3>(f)(Key(0), Key(1));
|
Vector3_ f_expr(MultiplyWithInverseFunction<Point2, 3>(f),
|
||||||
|
Expression<Point2>(0), Vector3_(1));
|
||||||
|
|
||||||
// Check derivatives
|
// Check derivatives
|
||||||
Point2 a(1, 2);
|
Point2 a(1, 2);
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue