12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/ImuFactorPush2 (pull request #200) 

ImuFactor with NavState manifold pre-integration
release/4.3a0
Frank Dellaert 2016-02-01 17:44:57 -08:00
parent 8fe3da6d9b ff1c27ba62
commit 6da59d73bd
43 changed files with 5076 additions and 1902 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1399
doc/ImuFactor.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/ImuFactor.pdf Normal file
View File

Binary file not shown.

331
doc/LieGroups.lyx
View File

@ -76,335 +76,10 @@ Frank Dellaert
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_inset CommandInset include
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

137
doc/macros.lyx
View File

@ -1,42 +1,60 @@
#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
\lyxformat 345
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
\begin_document
\begin_header
\textclass article
\use_default_options true
\maintain_unincluded_children false
\language english
\language_package default
\inputencoding auto
\fontencoding global
\font_roman default
\font_sans default
\font_typewriter default
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
\bibtex_command default
\index_command default
\paperfontsize default
\use_hyperref false
\papersize default
\use_geometry false
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 0
\cite_engine basic
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\use_refstyle 0
\index Index
\shortcut idx
\color #008000
\end_index
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\defskip medskip
\paragraph_indentation default
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\author ""
\author ""
\html_math_output 0
\html_css_as_file 0
\html_be_strict false
\end_header
\begin_body
@ -62,14 +80,14 @@ Derivatives
\begin_inset FormulaMacro
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
{#1\biggr\rvert_{#2}}
\newcommand{\at}[1]{#1\biggr\vert_{\#2}}
{#1\biggr\vert_{\#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
{\at{\deriv{#1}{#2}}{#3}}
{\at{\deriv{#1}{#2}}#3}
\end_inset
@ -107,6 +125,15 @@ Lie Groups
\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
\begin_layout Standard
@ -144,6 +171,12 @@ SO(2)
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rone}{\mathfrak{\mathbb{R}}}
{\mathfrak{\mathbb{R}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\mathbb{R}^{2}}}
@ -202,6 +235,12 @@ SE(2)
\end_inset
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
\end_layout
\begin_layout Standard
@ -243,7 +282,7 @@ SO(3)
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
\renewcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
@ -288,6 +327,86 @@ SE(3)
\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_body

396
doc/math.lyx
View File

@ -1237,21 +1237,28 @@ reference "eq:ApproximateObjective"
\end_inset
.
In particular, the notion of an exponential map allows us to define an
incremental transformation as tracing out a geodesic curve on the group
manifold along a certain
In particular, the notion of an exponential map allows us to define a mapping
from
\series bold
tangent vector
local coordinates
\series default
\begin_inset Formula $\xi$
\end_inset
back to a neighborhood in
\begin_inset Formula $G$
\end_inset
around
\begin_inset Formula $a$
\end_inset
,
\begin_inset Formula
\[
a\oplus\xi\define a\exp\left(\hat{\xi}\right)
\]
\begin{equation}
a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap}
\end{equation}
\end_inset
@ -1263,11 +1270,12 @@ with
\begin_inset Formula $n$
\end_inset
-dimensional Lie group,
-dimensional Lie group.
Above,
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
\end_inset
the Lie algebra element corresponding to the vector
is the Lie algebra element corresponding to the vector
\begin_inset Formula $\xi$
\end_inset
@ -1305,7 +1313,7 @@ For the Lie group
\end_inset
is denoted as
\begin_inset Formula $\omega$
\begin_inset Formula $\omega t$
\end_inset
and represents an angular displacement.
@ -1314,17 +1322,17 @@ For the Lie group
\end_inset
is a skew symmetric matrix denoted as
\begin_inset Formula $\Skew{\omega}\in\sothree$
\begin_inset Formula $\Skew{\omega t}\in\sothree$
\end_inset
, and is given by
\begin_inset Formula
\[
\Skew{\omega}=\left[\begin{array}{ccc}
\Skew{\omega t}=\left[\begin{array}{ccc}
0 & -\omega_{z} & \omega_{y}\\
\omega_{z} & 0 & -\omega_{x}\\
-\omega_{y} & \omega_{x} & 0
\end{array}\right]
\end{array}\right]t
\]
\end_inset
@ -1334,12 +1342,136 @@ Finally, the increment
\end_inset
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_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
Derivatives
\end_layout
@ -1352,7 +1484,7 @@ reference "def:differentiable"
\end_inset
to map exponential coordinates
to map local coordinates
\begin_inset Formula $\xi$
\end_inset
@ -1368,7 +1500,7 @@ reference "def:differentiable"
\begin_inset Formula $Df_{a}$
\end_inset
locally approximates a function
approximates the function
\begin_inset Formula $f$
\end_inset
@ -1378,6 +1510,10 @@ reference "def:differentiable"
to
\begin_inset Formula $\Reals m$
\end_inset
in a neighborhood around
\begin_inset Formula $a$
\end_inset
:
@ -1455,41 +1591,6 @@ derivative
.
\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
Derivative of an Action
\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
\begin_layout Subsection
Derivative of the Exponential and Logarithm Map
Derivative of the Exponential Map
\end_layout
\begin_layout Theorem
@ -3064,17 +3165,196 @@ For
\begin_inset Formula $\xi\neq0$
\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
\begin_layout Plain Layout
See
\begin_inset Flex URL
status open
http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti
al-of-the-exponential/
\begin_layout Plain Layout
http://deltaepsilons.wordpress.com/2009/11/06/
\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
.
@ -3097,7 +3377,7 @@ Retractions
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\retract}{\mathcal{R}}
\renewcommand{\retract}{\mathcal{R}}
{\mathcal{R}}
\end_inset
@ -6797,7 +7077,7 @@ Then
\begin_layout Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
bibfiles "/Users/dellaert/papers/refs"
bibfiles "refs"
options "plain"
\end_inset

26
doc/refs.bib Normal file
View File

@ -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}
}

146
gtsam.h
View File

@ -2447,7 +2447,7 @@ namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
class ConstantBias {
// Standard Constructor
// Constructors
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2479,97 +2479,107 @@ class ConstantBias {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
#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;
};
class PreintegratedImuMeasurements {
// Standard Constructor
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);
#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>
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
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
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
};
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,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
size_t bias,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
// Standard Interface
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>
class PreintegratedCombinedMeasurements {
// 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);
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
// Testable
void print(string s) const;
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;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
};
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,
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
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,
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
// Standard Interface
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>

73
gtsam/base/Matrix.h
View File

@ -21,15 +21,17 @@
// \callgraph
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/config.h> // Configuration from CMake
#include <boost/math/special_functions/fpclassify.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <boost/format.hpp>
#include <boost/function.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);
/**
* 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
#include <boost/serialization/nvp.hpp>

74
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -66,64 +66,69 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {
const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements.
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 G1,G2;
PreintegrationBase::update(measuredAcc, measuredOmega, deltaT,
&D_incrR_integratedOmega, &F_9x9, &G1, &G2);
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
// Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF
// 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
Matrix3 H_vel_biasacc = -dRij * deltaT;
Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT;
// TODO(frank): should we not also accout for bias on position?
Matrix3 theta_H_biasOmega = - C.topRows<3>();
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
// overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double, 15, 15> F;
F.setZero();
F.block<9, 9>(0, 0) = F_9x9;
F.block<3, 3>(0, 12) = H_angles_biasomega;
F.block<3, 3>(6, 9) = H_vel_biasacc;
F.block<9, 9>(0, 0) = A;
F.block<3, 3>(0, 12) = theta_H_biasOmega;
F.block<3, 3>(6, 9) = vel_H_biasAcc;
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
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
// Optimized matrix multiplication (1/dt) * G * measurementCovariance *
// G.transpose()
Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15);
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
* (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
* (H_vel_biasacc.transpose());
D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
* (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose());
D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance;
D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc *
(aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
(vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega *
(wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
(theta_H_biasOmega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
* H_angles_biasomega.transpose();
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
@ -134,11 +139,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
p->integrationCovariance = integrationErrorCovariance;
p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInit = biasAccOmegaInit;
p->biasAccOmegaInt = biasAccOmegaInt;
p_ = p;
resetIntegration();
preintMeasCov_.setZero();
}
#endif
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
@ -238,6 +244,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
CombinedImuFactor::CombinedImuFactor(
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,
@ -254,7 +261,7 @@ CombinedImuFactor::CombinedImuFactor(
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
@ -268,6 +275,7 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
pose_j = pvb.pose;
vel_j = pvb.velocity;
}
#endif
} /// namespace gtsam

55
gtsam/navigation/CombinedImuFactor.h
View File

@ -63,15 +63,15 @@ public:
/// Parameters for pre-integration:
/// 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 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
Params(const Vector3& n_gravity) :
PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInit(I_6x6) {
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInt(I_6x6) {
}
// 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_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
}
};
@ -113,28 +113,46 @@ public:
friend class CombinedImuFactor;
public:
/// @name Constructors
/// @{
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
*/
PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat)
PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero();
}
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
/// @}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedCombinedMeasurements& expected,
double tol = 1e-9) const;
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedCombinedMeasurements
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.
* @param measuredAcc Measured acceleration (in body frame, as given by the
@ -147,9 +165,9 @@ public:
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT);
/// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_; }
/// @}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
@ -157,7 +175,8 @@ public:
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
const Matrix6& biasAccOmegaInt, const bool use2ndOrderIntegration = true);
#endif
private:
/// Serialization function
@ -257,6 +276,7 @@ public:
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
@ -273,6 +293,7 @@ public:
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis = false);
#endif
private:

82
gtsam/navigation/ImuBias.cpp Normal file
View File

@ -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

55
gtsam/navigation/ImuBias.h
View File

@ -17,22 +17,11 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <iosfwd>
#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 {
/// All bias models live in the imuBias namespace
@ -94,46 +83,16 @@ public:
return measurement - biasGyro_;
}
// // 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;
// }
/// @}
/// @name Testable
/// @{
/// ostream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const ConstantBias& bias);
/// print with optional string
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;
}
void print(const std::string& s = "") const;
/** equality up to tolerance */
inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {

140
gtsam/navigation/ImuFactor.cpp
View File

@ -29,7 +29,7 @@ namespace gtsam {
using namespace std;
//------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements
// Inner class PreintegratedImuMeasurements
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s);
@ -52,39 +52,33 @@ void PreintegratedImuMeasurements::resetIntegration() {
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurement(
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)
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 G1, G2;
Matrix3 D_incrR_integratedOmega;
PreintegrationBase::update(measuredAcc, measuredOmega, dt,
&D_incrR_integratedOmega, &F, &G1, &G2);
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation:
// 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
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
#ifdef OLD_JACOBIAN_CALCULATION
Matrix9 G;
G << G1, Gi, G2;
Matrix9 Cov;
Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3,
Z_3x3, p().integrationCovariance * dt, Z_3x3,
Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
#else
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+ Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
+ G1 * (p().accelerometerCovariance / dt) * G1.transpose()
+ G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
#endif
// as in [2] we consider a first order propagation that can be seen as a
// prediction phase in EKF
// 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;
// (1/dt) allows to pass from continuous time noise to discrete time noise
preintMeasCov_ = A * preintMeasCov_ * A.transpose();
preintMeasCov_.noalias() += B * (aCov / dt) * B.transpose();
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
preintMeasCov_.noalias() += Gi * (iCov * dt) * Gi.transpose();
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
@ -100,7 +94,6 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
resetIntegration();
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Pose3> body_P_sensor) {
@ -108,6 +101,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
p_->body_P_sensor = body_P_sensor;
integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
#endif
//------------------------------------------------------------------------------
// 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 {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
NonlinearFactor::shared_ptr ImuFactor::clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
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->key4()) << "," << keyFormatter(this->key5())
<< ")\n";
Base::print("");
_PIM_.print(" preintegrated measurements:");
// Print standard deviations on covariance only
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose()
<< endl;
cout << *this << endl;
}
//------------------------------------------------------------------------------
bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& Base::equals(*e, tol);
const bool base = 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,
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 bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), _PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
PreintegratedMeasurements::Params>(pim.p());
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
PreintegratedImuMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
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;
}
//------------------------------------------------------------------------------
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_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) {
// use deprecated predict
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;
vel_j = pvb.velocity;
}
#endif
//------------------------------------------------------------------------------
} // namespace gtsam
}
// namespace gtsam

50
gtsam/navigation/ImuFactor.h
View File

@ -71,7 +71,9 @@ protected:
///< (first-order propagation from *measurementCovariance*).
/// Default constructor for serialization
PreintegratedImuMeasurements() {}
PreintegratedImuMeasurements() {
preintMeasCov_.setZero();
}
public:
@ -80,12 +82,22 @@ public:
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
*/
PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat) :
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationBase(p, biasHat) {
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
void print(const std::string& s = "Preintegrated Measurements:") const;
@ -108,6 +120,7 @@ public:
/// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; }
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
@ -121,6 +134,7 @@ public:
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt,
boost::optional<Pose3> body_P_sensor);
#endif
private:
@ -187,14 +201,13 @@ public:
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
/** implement functions needed for Testable */
/// print
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/// @}
/** Access the preintegrated measurements. */
@ -212,10 +225,19 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
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
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,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
@ -223,11 +245,12 @@ public:
const bool use2ndOrderCoriolis = false);
/// @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,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
#endif
private:
@ -242,7 +265,10 @@ private:
};
// class ImuFactor
/// traits
template<> struct traits<ImuFactor> : public Testable<ImuFactor> {};
template <>
struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
template <>
struct traits<ImuFactor> : public Testable<ImuFactor> {};
} /// namespace gtsam

12
gtsam/navigation/NavState.cpp
View File

@ -74,11 +74,17 @@ Matrix7 NavState::matrix() const {
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 {
attitude().print(s + ".R");
position().print(s + ".p");
gtsam::print((Vector) v_, s + ".v");
cout << s << *this << endl;
}
//------------------------------------------------------------------------------

20
gtsam/navigation/NavState.h
View File

@ -75,18 +75,9 @@ public:
/// @name Component Access
/// @{
inline const Rot3& attitude() const {
return R_;
}
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 Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
const Pose3 pose() const {
return Pose3(attitude(), position());
@ -124,6 +115,9 @@ public:
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state);
/// print
void print(const std::string& s = "") const;
@ -229,6 +223,8 @@ public:
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
/// @}
private:
/// @{
/// serialization

36
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -29,12 +29,26 @@ void PreintegratedRotation::Params::print(const string& s) const {
cout << s << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis)
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
<< endl;
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
if (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() {
deltaTij_ = 0.0;
deltaRij_ = Rot3();
@ -42,14 +56,14 @@ void PreintegratedRotation::resetIntegration() {
}
void PreintegratedRotation::print(const string& s) const {
cout << s << endl;
cout << s;
cout << " deltaTij [" << deltaTij_ << "]" << endl;
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
}
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
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
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
}
@ -76,12 +90,18 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
}
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
Matrix3* F) {
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
OptionalJacobian<3, 3> F) {
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
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
deltaTij_ += deltaT;
deltaRij_ = deltaRij_.compose(incrR, F);
@ -89,7 +109,7 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
// Update Jacobian
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * deltaT;
- D_incrR_integratedOmega * deltaT;
}
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

63
gtsam/navigation/PreintegratedRotation.h
View File

@ -33,7 +33,6 @@ namespace gtsam {
*/
class PreintegratedRotation {
public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params {
@ -41,11 +40,10 @@ public:
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
Params() :
gyroscopeCovariance(I_3x3) {
}
Params() : gyroscopeCovariance(I_3x3) {}
virtual void print(const std::string& s) const;
virtual bool equals(const Params& other, double tol=1e-9) const;
private:
/** Serialization function */
@ -60,29 +58,50 @@ public:
};
protected:
/// Parameters
boost::shared_ptr<Params> p_;
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
boost::shared_ptr<Params> p_;
/// Default constructor for serialization
PreintegratedRotation() {
}
PreintegratedRotation() {}
public:
/// @name Constructors
/// @{
/// Default constructor, resets integration to zero
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
p_(p) {
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
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
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
/// @{
const boost::shared_ptr<Params>& params() const {
return p_;
}
const double& deltaTij() const {
return deltaTij_;
}
@ -96,23 +115,24 @@ public:
/// @name Testable
/// @{
void print(const std::string& s) const;
bool equals(const PreintegratedRotation& other, double tol) const;
/// @}
/// @name Main functionality
/// @{
/// 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
/// an incremental rotation.
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
/// Calculate an incremental rotation given the gyro measurement and a time interval,
/// and update both deltaTij_ and deltaRij_.
void integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
Matrix3* F);
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
OptionalJacobian<3, 3> F = boost::none);
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
@ -121,6 +141,8 @@ public:
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i) const;
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
@ -133,4 +155,7 @@ private:
}
};
} // namespace gtsam
template <>
struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
} /// namespace gtsam

447
gtsam/navigation/PreintegrationBase.cpp
View File

@ -20,6 +20,7 @@
**/
#include "PreintegrationBase.h"
#include <gtsam/base/numericalDerivative.h>
#include <boost/make_shared.hpp>
using namespace std;
@ -27,229 +28,254 @@ using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
void PreintegrationBase::Params::print(const string& s) const {
PreintegratedRotation::Params::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
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;
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
const Bias& biasHat)
: p_(p), biasHat_(biasHat), deltaTij_(0.0) {
resetIntegration();
}
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_ = Z_3x3;
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
preintegrated_.setZero();
preintegrated_H_biasAcc_.setZero();
preintegrated_H_biasOmega_.setZero();
}
//------------------------------------------------------------------------------
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 {
cout << s << 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");
cout << s << *this << endl;
}
//------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return fabs(deltaTij_ - other.deltaTij_) < tol
&& deltaXij_.equals(other.deltaXij_, tol)
const bool params_match = p_->equals(*other.p_, tol);
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
}
//------------------------------------------------------------------------------
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
// Correct for bias in the sensor frame
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega,
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega) const {
assert(p().body_P_sensor);
// Compensate for sensor-body displacement if needed: we express the quantities
// (originally in the IMU frame) into the body frame
// 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
j_correctedAcc = bRs * j_correctedAcc;
// Get sensor to body rotation matrix
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
// Convert angular velocity and acceleration from sensor to body frame
Vector3 correctedAcc = bRs * unbiasedAcc;
const Vector3 correctedOmega = bRs * unbiasedOmega;
// Jacobians
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
if (D_correctedAcc_unbiasedAcc) *D_correctedAcc_unbiasedAcc = bRs;
if (D_correctedAcc_unbiasedOmega) *D_correctedAcc_unbiasedOmega = Z_3x3;
if (D_correctedOmega_unbiasedOmega) *D_correctedOmega_unbiasedOmega = bRs;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector();
if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the measured one
// Subtract out the the centripetal acceleration from the unbiased one
// to get linear acceleration vector in the body frame:
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
const Matrix3 body_Omega_body = skewSymmetric(correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
j_correctedAcc -= body_Omega_body * b_velocity_bs;
correctedAcc -= body_Omega_body * b_velocity_bs;
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (D_correctedAcc_measuredOmega) {
double wdp = j_correctedOmega.dot(b_arm);
*D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
+ j_correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * j_measuredOmega.transpose();
}
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(j_correctedAcc, j_correctedOmega);
return make_pair(correctedAcc, correctedOmega);
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
OptionalJacobian<9, 9> D_updated_current,
OptionalJacobian<9, 3> D_updated_measuredAcc,
OptionalJacobian<9, 3> D_updated_measuredOmega) const {
// See extensive discussion in ImuFactor.lyx
Vector9 PreintegrationBase::UpdatePreintegrated(
const Vector3& a_body, const Vector3& w_body, double dt,
const Vector9& preintegrated, OptionalJacobian<9, 9> A,
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;
Matrix3 D_correctedAcc_measuredAcc, //
D_correctedAcc_measuredOmega, //
D_correctedOmega_measuredOmega;
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
(needDerivs ? &D_correctedAcc_measuredAcc : 0),
(needDerivs ? &D_correctedAcc_measuredOmega : 0),
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
// Do update in one fell swoop
Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
(needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
(needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
if (needDerivs) {
*D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
*D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
if (!p().body_P_sensor->translation().vector().isZero()) {
*D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent* dt, // theta
position + velocity* dt + a_nav* dt22, // position
velocity + a_nav* dt; // velocity
if (A) {
// 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
}
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;
}
return updated;
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,
const Vector3& j_measuredOmega, const double dt,
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt, Matrix9* A,
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
const Rot3 oldRij = deltaXij_.attitude();
// Possibly correct for sensor pose
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
deltaTij_ += dt;
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
// Update Jacobians
// TODO(frank): we are repeating some computation here: accessible in F ?
Vector3 j_correctedAcc, j_correctedOmega;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
Matrix3 D_acc_R;
oldRij.rotate(j_correctedAcc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = j_correctedOmega * dt;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
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;
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().vector().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
// D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
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(
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_;
Matrix3 D_correctedRij_bias;
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
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();
const Vector9 biasCorrected =
preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() +
preintegrated_H_biasOmega_ * biasIncr.gyroscope();
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
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;
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
}
return xi;
return biasCorrected;
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
// correct for bias
// TODO(frank): make sure this stuff is still correct
Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0);
Vector9 biasCorrected =
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;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
p().omegaCoriolis, p().use2ndOrderCoriolis,
H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
// Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1)
*H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)
*H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state;
if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias;
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,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
@ -262,42 +288,119 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
NavState state_i(pose_i, vel_i);
NavState state_j(pose_j, vel_j);
/// Predict state at time j
Matrix99 D_predict_state_i;
Matrix96 D_predict_bias_i;
NavState predictedState_j = predict(state_i, bias_i,
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);
// Predict state at time j
Matrix9 D_error_state_i, D_error_state_j;
Vector9 error = computeError(state_i, state_j, bias_i,
H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5);
// Separate out derivatives in terms of 5 arguments
// 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
// Instead, the velocities in nav are updated using a straight addition
// This is difference is accounted for by the R().transpose calls below
if (H1)
*H1 << D_error_predict * D_predict_state_i.leftCols<6>();
if (H2)
*H2
<< 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;
if (H1) *H1 << D_error_state_i.leftCols<6>();
if (H2) *H2 << D_error_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();
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,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
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
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
q->n_gravity = n_gravity;
@ -306,7 +409,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
#endif
//------------------------------------------------------------------------------
}/// namespace gtsam
} // namespace gtsam

274
gtsam/navigation/PreintegrationBase.h
View File

@ -21,13 +21,16 @@
#pragma once
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/navigation/PreintegrationParams.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/ImuBias.h>
#include <boost/make_shared.hpp>
#include <gtsam/linear/NoiseModel.h>
#include <iosfwd>
namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated
struct PoseVelocityBias {
Pose3 pose;
@ -44,6 +47,7 @@ struct PoseVelocityBias {
return NavState(pose, velocity);
}
};
#endif
/**
* PreintegrationBase is the base class for PreintegratedMeasurements
@ -52,173 +56,140 @@ struct PoseVelocityBias {
* to access, print, and compare them.
*/
class PreintegrationBase {
public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params: 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
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
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
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);
}
};
typedef imuBias::ConstantBias Bias;
typedef PreintegrationParams Params;
protected:
double deltaTij_; ///< Time interval from i to j
/// Parameters. Declared mutable only for deprecated predict method.
/// TODO(frank): make const once deprecated method is removed
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
mutable
#endif
boost::shared_ptr<Params> p_;
/// Acceleration and gyro bias used for preintegration
Bias biasHat_;
/// Time interval from i to j
double deltaTij_;
/**
* 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: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
NavState deltaXij_;
Vector9 preintegrated_;
/// Parameters
boost::shared_ptr<Params> p_;
/// 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
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
/// Default constructor for serialization
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();
}
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
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 {
return *boost::static_pointer_cast<Params>(p_);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
void set_body_P_sensor(const Pose3& body_P_sensor) {
p_->body_P_sensor = body_P_sensor;
}
#endif
/// @}
/// getters
const NavState& deltaXij() const {
return deltaXij_;
}
const double& deltaTij() const {
return deltaTij_;
}
const Rot3& deltaRij() const {
return deltaXij_.attitude();
}
Vector3 deltaPij() const {
return deltaXij_.position().vector();
}
Vector3 deltaVij() const {
return deltaXij_.velocity();
}
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_;
}
/// @name Instance variables access
/// @{
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
const double& deltaTij() const { return deltaTij_; }
const Vector9& preintegrated() const { return preintegrated_; }
Vector3 theta() const { return preintegrated_.head<3>(); }
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
NavState deltaXij() const { return NavState::Retract(preintegrated_); }
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
// Exposed for MATLAB
Vector6 biasHatVector() const {
return biasHat_.vector();
}
Vector6 biasHatVector() const { 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;
/// check equality
bool equals(const PreintegrationBase& other, double tol) const;
/// @}
/// @name Main functionality
/// @{
/// Subtract estimate and correct for sensor pose
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedAcc = boost::none,
OptionalJacobian<3, 3> D_correctedAcc_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> D_correctedOmega_unbiasedOmega = boost::none) const;
/// Calculate the updated preintegrated measurement, does not modify
/// It takes measured quantities in the j frame
NavState updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
OptionalJacobian<9, 9> D_updated_current = boost::none,
OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
void integrateMeasurement(const Vector3& measuredAcc,
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
/// summarizing the preintegrated IMU measurements so far
@ -227,8 +198,14 @@ public:
/// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
boost::none) const;
OptionalJacobian<9, 9> H1 = boost::none,
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
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 =
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
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
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:
/** Serialization function */
@ -250,14 +251,11 @@ private:
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
}
};

54
gtsam/navigation/PreintegrationParams.cpp Normal file
View File

@ -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

71
gtsam/navigation/PreintegrationParams.h Normal file
View File

@ -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

97
gtsam/navigation/Scenario.h Normal file
View File

@ -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

107
gtsam/navigation/ScenarioRunner.cpp Normal file
View File

@ -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

109
gtsam/navigation/ScenarioRunner.h Normal file
View File

@ -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

76
gtsam/navigation/tests/imuFactorTesting.h Normal file
View File

@ -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

239
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -13,8 +13,9 @@
* @file testCombinedImuFactor.cpp
* @brief Unit test for Lupton-style combined IMU factor
* @author Luca Carlone
* @author Stephen Williams
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
*/
#include <gtsam/navigation/ImuFactor.h>
@ -31,62 +32,23 @@
#include <boost/bind.hpp>
#include <list>
using namespace std;
using namespace gtsam;
#include "imuFactorTesting.h"
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
namespace {
// Auxiliary functions to test preintegrated Jacobians
// 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);
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0001 * I_3x3;
return p;
}
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 ) {
// 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
Vector3 measuredAcc(0.1, 0.0, 0.0);
@ -94,12 +56,13 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
double deltaT = 0.5;
double tol = 1e-6;
auto p = testing::Params();
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3);
PreintegratedImuMeasurements expected1(p, bias);
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6);
PreintegratedCombinedMeasurements actual1(p, bias);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -111,46 +74,41 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases ) {
imuBias::ConstantBias 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 bias(Vector3(0.2, 0, 0), Vector3(0, 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));
Vector3 v1(0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.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
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
Vector3 measuredAcc =
x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
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);
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim(
imuBias::ConstantBias(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);
PreintegratedCombinedMeasurements combined_pim(p,
Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity,
omegaCoriolis);
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim);
noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
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 errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
@ -175,129 +133,74 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
/* ************************************************************************* */
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
auto p = testing::Params();
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
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
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 pre-integrated values
PreintegratedCombinedMeasurements pim(p);
testing::integrateMeasurements(measurements, &pim);
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements pim =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
// 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
EXPECT(assert_equal(numericalDerivative21(preintegrated, Z_3x1, Z_3x1),
pim.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(preintegrated, Z_3x1, Z_3x1),
pim.preintegrated_H_biasOmega(), 1e-3));
}
/* ************************************************************************* */
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
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
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;
const Vector3 measuredOmega(0, 0.1, 0); // M_PI/10.0+0.3;
const Vector3 measuredAcc(0, 1.1, -kGravity);
const double deltaT = 0.001;
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
PreintegratedCombinedMeasurements pim(p, bias);
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Gaussian::shared_ptr combinedmodel =
const noiseModel::Gaussian::shared_ptr combinedmodel =
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
gravity, omegaCoriolis);
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity,
omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
EXPECT(
assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
const NavState actual = pim.predict(NavState(), bias);
const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
const Vector3 expectedVelocity(0, 1, 0);
EXPECT(assert_equal(expectedPose, actual.pose()));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81;
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
double tol = 1e-4;
const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
auto p = testing::Params();
PreintegratedCombinedMeasurements pim(p, bias);
const Vector3 measuredAcc = - kGravityAlongNavZDown;
const Vector3 measuredOmega(0, 0, M_PI / 10.0);
const double deltaT = 0.001;
const double tol = 1e-4;
for (int i = 0; i < 1000; ++i)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
gravity, omegaCoriolis);
const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);
// Predict
Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol));
const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
const Vector3 v(0, 0, 0), v2;
const NavState actual = pim.predict(NavState(x, v), bias);
const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, actual.pose(), tol));
}
/* ************************************************************************* */

923
gtsam/navigation/tests/testImuFactor.cpp
View File

File diff suppressed because it is too large Load Diff

77
gtsam/navigation/tests/testImuFactorSerialization.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

3
gtsam/navigation/tests/testPoseVelocityBias.cpp
View File

@ -25,6 +25,8 @@
using namespace std;
using namespace gtsam;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
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;
EXPECT(assert_equal(expected, actual, 1e-9));
}
#endif
/* ************************************************************************************************/
int main() {

150
gtsam/navigation/tests/testPreintegrationBase.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

137
gtsam/navigation/tests/testScenario.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

385
gtsam/navigation/tests/testScenarios.cpp Normal file
View File

@ -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;
}
/* ************************************************************************* */

84
gtsam/nonlinear/expressions.h
View File

@ -24,90 +24,6 @@ Expression<T> compose(const Expression<T>& t1, const Expression<T>& 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
typedef Expression<double> double_;
typedef Expression<Vector1> Vector1_;

105
python/gtsam_examples/ImuFactorExample.py Normal file
View File

@ -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()

129
python/gtsam_examples/PreintegrationExample.py Normal file
View File

@ -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()

32
python/gtsam_tests/testScenario.py Normal file
View File

@ -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()

30
python/gtsam_tests/testScenarioRunner.py Normal file
View File

@ -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()

59
python/gtsam_utils/_plot.py
View File

@ -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

20
python/handwritten/exportgtsam.cpp
View File

@ -20,10 +20,10 @@
#include <numpy_eigen/NumpyEigenConverter.hpp>
// Base
// base
void exportFastVectors();
// Geometry
// geometry
void exportPoint2();
void exportPoint3();
void exportRot2();
@ -34,24 +34,29 @@ void exportPinholeBaseK();
void exportPinholeCamera();
void exportCal3_S2();
// Inference
// inference
void exportSymbol();
// Linear
// linear
void exportNoiseModels();
// Nonlinear
// nonlinear
void exportValues();
void exportNonlinearFactor();
void exportNonlinearFactorGraph();
void exportLevenbergMarquardtOptimizer();
void exportISAM2();
// Slam
// slam
void exportPriorFactors();
void exportBetweenFactors();
void exportGenericProjectionFactor();
// navigation
void exportImuFactor();
void exportScenario();
// Utils (or Python wrapper specific functions)
void registerNumpyEigenConversions();
@ -94,4 +99,7 @@ BOOST_PYTHON_MODULE(_libgtsam_python){
exportPriorFactors();
exportBetweenFactors();
exportGenericProjectionFactor();
exportImuFactor();
exportScenario();
}

94
python/handwritten/navigation/ImuFactor.cpp Normal file
View File

@ -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));
}

64
python/handwritten/navigation/Scenario.cpp Normal file
View File

@ -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);
}

5
tests/testExpressionFactor.cpp
View File

@ -605,7 +605,7 @@ TEST(ExpressionFactor, MultiplyWithInverse) {
auto model = noiseModel::Isotropic::Sigma(3, 1);
// Create expression
auto f_expr = MultiplyWithInverse<3>()(Key(0), Key(1));
Vector3_ f_expr(MultiplyWithInverse<3>(), Expression<Matrix3>(0), Vector3_(1));
// Check derivatives
Values values;
@ -638,7 +638,8 @@ TEST(ExpressionFactor, MultiplyWithInverseFunction) {
auto model = noiseModel::Isotropic::Sigma(3, 1);
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
Point2 a(1, 2);