A complete Example: SLAM with outliers¶
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 Functions and Constraints;
build basic contractors (
CtcFwdBwdin 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:
CtcFwdBwdCtcCompoCtcFixPointCtcQInterCtcInverse.
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:
MyClass* x = new MyClass(...)
just to avoid potential memory fault when pointing to temporary objects. Of course, all these objects should be disallocated afterwards.
Download¶
The full code can be found under .../examples/slam (this subfolder is included in the Ibex package).
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.
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 (\(\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:
/*================================ data ==================================*/
extern const int N; // number of beacons
extern const int T; // number of time steps
extern const double L; // limit of the environment (the
// robot is in the area [0,L]x[0,L])
extern const int NB_OUTLIERS; // maximal number of outliers per
// time units
extern IntervalMatrix beacons; // (a Nx2 matrix) beacons[i] is the
// position (x,y) of the ith beacon
extern IntervalMatrix d; // (a TxN matrix) d[t][i]=distance
// between x[t] and the ith beacon
extern IntervalMatrix v; // (a Tx2 matrix) v[t] is the delta
// vector between x[t+1] and x[t].
/*========================================================================*/
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.
Entering equations and functions¶
A measurement is an equation.
To enter an equation in Ibex, we use the NumConstraint class (see Constraints).
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:
The corresponding code in Ibex is:
Variable x(T,2); // create a Tx2 variable
for (int t=0; t<T; t++) {
if (t<T-1) {
NumConstraint* c=new NumConstraint(x,x[t+1]-x[t]=v[t]);
...
}
}
note Here, v is not a variable but a constant (see data.h).
Sometimes, different constraints are based on the same pattern. It is then often convenient
to declare first a Function object.
Example: For distance constraints, we may first declare the distance function:
// create the distance function beforehand
Variable a(2); // "local" variable
Variable b(2);
Function dist(a,b,sqrt(sqr(a[0]-b[0])+sqr(a[1]-b[1])));
and then the equation for each time step and each beacon:
for (int t=0; t<T; t++) {
for (int i=0; i<N; i++) {
NumConstraint* c=new NumConstraint(
x,dist(x[t],beacons[i])=d[t][i]);
...
}
}
Creating basic contractors¶
We can create now contractors.
To create a contractor with respect to an equation we
use the CtcFwdBwd class (see Forward-Backward).
Example: With the constraint x=1:
Variable x;
NumConstraint* c=new NumConstraint(x,x=1);
Ctc* ctc=new CtcFwdBwd(*c);
Note: The Ctc prefix indicates that this class is a contractor (i.e.,
it can be composed with other contractors). Ctc is also the name of
the generic contractor class.
Combining contractors¶
We are now ready to build our first strategy. We create all the contractors
and push them in a vector ctc (this vector will be necessary for the composition):
vector<Ctc*> ctc;
for (int t=0; t<T; t++) {
vector<Ctc*> cdist;
for (int b=0; b<N; b++) {
// Push the contractor corresponding to
// the detection of beacon n°b at time t
NumConstraint* c=new NumConstraint(
x,dist(transpose(x[t]),beacons[b])=d[t][b]);
ctc.push_back(new CtcFwdBwd(*c));
}
if (t<T-1) {
// Push the contractor corresponding to
// the speed measurement at time t
NumConstraint* c=new NumConstraint(x,x[t+1]-x[t]=transpose(v[t]));
ctc.push_back(new CtcFwdBwd(*c));
}
}
Now, we can create the composition of all these contractors using CtcCompo
(the vector ctc being given in argument) and a fixpoint of the latter using CtcFixPoint. This gives:
// Composition
CtcCompo compo(ctc);
// FixPoint
CtcFixPoint fix(compo);
We are done. We just have to call the top-level contractor on the initial box.
// the initial box [0,L]x[0,L]x[0,L]x[0,L]
IntervalVector box(T*2,Interval(0,L));
cout << endl << " initial box =" << box << endl;
fix.contract(box);
cout << endl << " final box =" << box << endl << endl << endl;
Result¶
The execution shows that the final box contains the real trajectory:
initial box =([0, 10] ; [0, 10] ; [0, 10] ; [0, 10] ; [0, 10] ; [0, 10])
final box =([8.592079632938807, 9.009246227143752] ; [0.4364101205434934, 0.8936036705218675] ; ... )
The real positions are:
Second strategy (with outliers)¶
We consider now that at most NB_OUTLIERS outliers may occur for each time step.
To contract rigorously despite of outliers, we must use the “q-intersection” operator that basically consider all possible combinations of (N-NB_OUTLIERS) contractors among N:
Ibex provides the CtcQInter contractor.
Q-intersection¶
We must only place all the contractors
related to the same time step in another temporary vector (called cdist)
and give this vector in argument of CtcQInter:
Let us see what happens if we do this.
Let us replace:
if (t<T-1) {
// Push the contractor corresponding to
// the speed measurement at time t
NumConstraint* c=new NumConstraint(x,x[t+1]-x[t]=v[t]);
ctc.push_back(new CtcFwdBwd(*c));
}
by:
// create a temporary subvector
// for collecting all the contractors corresponding
// to the detections at time t
vector<Ctc*> cdist;
for (int b=0; b<N; b++) {
NumConstraint* c=new NumConstraint(
x,dist(x[t],beacons[b])=d[t][b]);
// push the detection of beacon n°b
cdist.push_back(new CtcFwdBwd(*c));
}
// Push the q-intersection of all
// the contractors in "cdist" in the main
// vector "ctc"
ctc.push_back(new CtcQInter(cdist,N-NB_OUTLIERS));
Problem: the program runs almost endlessly! … Why?
… because the q-intersection runs exponentially in the dimension of the input box, which is 2T.
Of course, the implementation should take advantage of the fact that only 2 variables are actually impacted. But the current code is not optimized in this way.
Anyway, it is often necessary to apply a contractor strategy to only a subset of variables (here, to the two components of x[t]).
For this end, we will make use of the Inverse contractor.
Projection using the inverse contractor¶
Applying the q-intersection on the subset of variables x_t amounts to apply the inverse of this contractor by the projection function \(x\mapsto x[t]\).
We replace:
// create a temporary subvector
// for collecting all the contractors corresponding
// to the detections at time t
vector<Ctc*> cdist;
for (int b=0; b<N; b++) {
NumConstraint* c=new NumConstraint(
x,dist(x[t],beacons[b])=d[t][b]);
// push the detection of beacon n°b
cdist.push_back(new CtcFwdBwd(*c));
}
// Push the q-intersection of all
// the contractors in "cdist" in the main
// vector "ctc"
ctc.push_back(new CtcQInter(cdist,N-NB_OUTLIERS));
By:
vector<Ctc*> cdist;
for (int b=0; b<N; b++) {
// Create the distance constraint with 2
// (instead of 2*T) variables
Variable xt(2);
NumConstraint* c=new NumConstraint(
xt,dist(xt,beacons[b])=d[t][b]);
cdist.push_back(new CtcFwdBwd(*c));
}
// q-intersection with 2 variables only
CtcQInter* q=new CtcQInter(cdist,N-NB_OUTLIERS);
// Push in the main vector "ctc" the application
// of the latter contractor to x[t]
ctc.push_back(new CtcInverse(*q,*new Function(x,x[t])));
And now, the program terminates instantaneously.
With NB_OUTLIERS set to 1, the dispaly shows a slightly larger box:
initial box =([0, 10] ; [0, 10] ; [0, 10] ; [0, 10] ; [0, 10] ; [0, 10])
final box =([8.542599451371126, 9.032225305125761] ; [0.3807126686643456, 1.002241041162326] ; ...)
Third strategy (how can this scale?)¶
The program we have proposed so far does not really scale. For example, setting T=200000 in data.cpp will make
the program run for a long time and crash after a memory overflow.
We see now a more efficient variant. This variant, however, will be less concise. We will also partially lose the
elegancy of contractor programming. In particular, we will do ourselves the loop that compose the contractors
as time increases. But, after all, a programming language is always a compromise between efficiency and elegancy
so if you really look for efficiency, you should accept to sacrify a little bit of elegancy.
Let us first explain why the current program does not scale. In the program, we build a NumConstraint object for most of the contractors and each of these NumConstraint objects builds (silently) a Function object.
For instance, by writing:
NumConstraint* c=new NumConstraint(xt,dist(xt,beacons[b])=d[t][b]);
The following function is created somewhere
Now, you must be aware that the construction of Function objects is both time and memory consuming. The good point however is that, once built, these objects are fast to use (evaluation, gradient, etc.).
The CtcQInter objects are also costly because each contains a set of N references.
It is clear in our context that the we keep on creating the same contractors again and again, with only one of the parameters changing with time (in our previous example, d[t][b]). The key idea is to factorize all these contractors and create a “parametrized” contractor where the time is set dynamically. Let us start with the detection constraints.
Detection contractor¶
The following class declares a contractor for the detection of a given beacon. This is a handcrafted contractor so we need to create a new class that extends Ctc and implements the contract() function. Time (contrary to the beacon number) is not set at construction so that one instance of this contractor can be used
for any time:
/*
* Contractor for the detection of beacon n°b.
*
* This is a contractor parametrized by the time "t".
* It means that a call to contract() must be
* preceded by a call to set_time(t).
*/
class Detection : public Ctc {
public:
/*
* The contractor is for a specific beacon "b" which
* is specified in argument of the constructor.
*/
Detection(int b) : Ctc(2), b(b) {
Variable x(2);
// This function will be created once for the T time steps!
dist = new Function(x,sqrt(sqr(x[0]-beacons[b][0])+sqr(x[1]-beacons[b][1])));
}
/*
* Allow to set the time dynamically
*/
void set_time(int t) {
this->t=t;
}
void contract(IntervalVector& x) {
// by simplicity, we call the backward
// operator on the function directly
dist->backward(d[t][b],x);
}
protected:
int b; // beacon number
int t; // time number
Function* dist; // distance function
};
Speed contractor¶
We do the same with the second set of time-dependant constraints, namely the “speed” or “delta” constraints between two consecutive time steps.
/*
* Contractor for the "speed" constraint.
*
* This is a contractor parametrized by the time "t".
* It means that a call to contract() must be
* preceded by a call to set_time(t).
*/
class Speed : public Ctc {
public:
Speed() : Ctc(2) {
Variable a(2);
Variable b(2);
delta = new Function(a,b,b-a);
}
void contract(IntervalVector& x) {
delta->backward(v[t],x);
}
void set_time(int t) {
this->t=t;
}
protected:
int t;
Function* delta;
};
Scan contractor (q-intersection)¶
Again, we create a parametrized contractor for the q-intersection of the N detections occuring at a given time.
This set of measurements somehow forms a scanning of the environment so we name this contractor Scan:
/*
* Scanning contractor that aggregates
* the N detections occurring at a given time t.
*/
class Scan : public Ctc {
public:
Scan() : Ctc(2) {
// The N detections
detect = new Detection*[N];
// The q-intersection is created as before,
// using a temporary vector "cdist"
vector<Ctc*> cdist;
for (int b=0; b<N; b++) {
cdist.push_back(detect[b]=new Detection(b));
}
qinter = new CtcQInter(cdist,N-NB_OUTLIERS);
}
void contract(IntervalVector& x) {
qinter->contract(x);
}
void set_time(int t) {
// we set the time of each sub-contractor
for (int i=0; i<N; i++)
detect[i]->set_time(t);
}
protected:
Detection** detect;
CtcQInter* qinter;
int t;
};
Trajectory¶
We can create now the final contractor that calls 2T times an instance of the Scan and Speed contractors (there is only one instance of each). A call to set_time() precedes every call to contract(). Note that a fix-point would not be reasonable here.
/*
* The contractor for the whole trajectory.
*
* It will contract every positions of the robot using
* detections and speed data, in a single pass (no
* fixpoint).
*/
class Trajectory : public Ctc {
public:
Trajectory() : Ctc(2*T) { }
void contract(IntervalVector& x) {
for (int t=0; t<T; t++) {
// Get a copy of the domain of x[t]
IntervalVector xt=x.subvector(2*t,2*t+1);
// Set the time
scan.set_time(t);
// Contract with the scanning
scan.contract(xt);
// Update the box "x" with the new domain for x[t]
x.put(2*t,xt);
if (t<T-1) {
// Get a copy of the domain of x[t] and x[t+1]
IntervalVector xtt1=x.subvector(2*t,2*(t+1)+1);
// Set the time
speed.set_time(t);
// Contract with the speed vector
speed.contract(xtt1);
// Update the box
x.put(2*t,xtt1);
}
}
}
Scan scan;
Speed speed;
};