=======================================
A complete Example: SLAM with outliers
=======================================
.. _slam-intro:
---------------
Introduction
---------------
The goal of this example is to implement a simple contractor strategy
for a SLAM problem with the IBEX library. SLAM means *simultaneous localization and map building*
and is a classical problem in mobile robotics.
We will see that contractor programmming with Ibex basically amounts to:
- enter your mathematical model using :ref:`mod-func` and :ref:`mod-sys-ctrs`;
- build basic contractors (``CtcFwdBwd`` in general) with respect to the equations;
- apply operators to these contractors to yield new (more sophisticated) contractors.
The code we build here will eventually involve 5 different contrators:
- ``CtcFwdBwd``
- ``CtcCompo``
- ``CtcFixPoint``
- ``CtcQInter``
- ``CtcInverse``.
We shall implement a strategy that is similar to a predictor-corrector approach (like the Kalman filter for instance)
in the sense that we also use odometry and observation to reduce the uncertainty on the robot's position.
However, both information as considered on the same footing and there is no distinction such as *prediction* versus *correction*.
They are just contractors that can be used in any order and we will even calculate a fixpoint of them (so the strategy
is not a *recursive* filter).
**note** For the sake of simplicity, we shall always use dynamic *allocation*:
.. code-block:: cpp
MyClass* x = new MyClass(...)
just to avoid potential memory fault when pointing to temporary objects.
Of course, all these objects should be disallocated afterwards.
.. _slam-intro-download:
^^^^^^^^^^^^^^^^^^^^
Download
^^^^^^^^^^^^^^^^^^^^
The full code can be found under ``.../examples/slam`` (this subfolder is included in the Ibex package).
.. _slam-intro-prob-desc:
^^^^^^^^^^^^^^^^^^^^
Problem description
^^^^^^^^^^^^^^^^^^^^
The goal is to charaterize the trajectory of an autonomous robot by enclosing in a box its position x[t]
for each time step t=0...T.
.. image:: images/loc.png
:width: 400 px
We have no direct information on its position (including the initial one) but the robot measures
at each time step:
- its distance from a set of N fixed beacons (:math:`\rightarrow` N measurements) as if it was equipped with a telemeter;
- its "speed" (delta) vector v[t]=x[t+1]-x[t].
Each measurement is subject to uncertainty: *distances and speed vector*
but also the position of the beacons, that is supposed to be measured a priori.
Furthermore, we shall consider outliers.
First of all, let us assume that the measurements are all simulated in a seperate unit.
The header file of this unit contains:
.. literalinclude:: ../examples/slam/data.h
:language: cpp
:start-after: [data]
:end-before: [data]
.. _slam-strat1:
------------------------------
First strategy (no outlier)
------------------------------
First, we consider no outlier. A simple strategy consists in :
- creating a contractor for each measurement,
- calling all these contractors in sequence (composition)
- performing a fixpoint loop
Let us start by creating contractors for measurements,
that is, those related to equations.
.. _slam-strat1-eq:
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Entering equations and functions
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
A measurement is an equation.
To enter an equation in Ibex, we use the ``NumConstraint`` class (see :ref:`mod-sys-ctrs`).
A ``NumConstraint`` object contains a mathematical condition, or "constraint".
To define a constraint mathematically, we must specify how many variables it relates
and in which order these variables must be taken.
That is why we need to create first some ``Variable`` objects.
But keep in mind that these objects are just a C++ trick for the only purpose of
declaring a constraint.
Once declared, a constraint is self-contained and depends on nothing else.
*Example:* For creating the equations:
.. math::
\forall t