# Lesson D: Building our own contractor¶

This lesson will extend the previous exercise by dealing with a **data association** problem together with localization: the landmarks perceived by the robot are now **indistinguishable**: they all look alike. The goal of this exercise is to develop our own contractor in order to solve the problem.

Content of this lesson

## Introduction: indistinguishable landmarks¶

As before, our goal is to deal with the localization of a mobile robot for which a map of the environment is available.

It is assumed that:

the map is static and made of point landmarks;

**the landmarks are indistinguishable**(which was not the case before);the position of each landmark is known;

the position of the robot is not known.

This problem often occurs when using sonars for localization, where it is difficult to distinguish a landmark from another. The challenge in this type of scenario is the difficulty of **data associations**, *i.e.* establishing feature correspondences between perceptions of landmarks and the map of these landmarks, known beforehand. This is especially the case of challenging underwater missions, for which one can reasonably assume that the detected landmarks cannot be distinguished from the other in sonar images. For instance, two different rocks may have the same aspect and dimension in the acquired images, as illustrated by the following figure.

Due to these difficulties, we consider that we cannot compute reliable data associations that stand on image processing of the observations. We will rather consider the data association problem **together with state estimation**.

## Data association: formalism¶

Up to now, we assumed that one perceived landmark was identified without ambiguity.
However, when several landmarks \(\mathbf{m}_{1}\), \(\dots\), \(\mathbf{m}_{\ell}\) exist, the observation data may not be *associated*: we do not know to which landmark the measurement \(\mathbf{y}\) refers. We now consider several measurements denoted by \(\mathbf{y}^i\). Hence the observation constraint has the form:

with \(\mathbf{m}^{i}\) the unidentified landmark of the observation \(\mathbf{y}^i\). The \(\vee\) notation expresses the logical disjunction *or*.
Equivalently, the system can be expressed as:

where \(\mathbb{M}\) is the bounded map of the environment: the set of all landmarks represented by their bounded positions. In other words, we do not know the right parameter vector \(\mathbf{m}_{1}\), \(\dots\), \(\mathbf{m}_{\ell}\) associated with the observation \(\mathbf{y}^i\). This is a problem of data association. The following figure illustrates \(\mathbb{M}\) with the difficulty to differentiate landmarks in underwater extents.

In this exercise, the data association problem is solved together with state estimation, **without image processing**. The constraint \(\mathbf{g}\big(\mathbf{x},\mathbf{y}^{i},\mathbf{m}^{i}\big)=\mathbf{0}\) has been solved in Lesson C, it remains to deal with the constraint \(\mathbf{m}^{i}\in\mathbb{M}\). We will call it the *constellation constraint*.

## The constellation constraint and its contractor¶

When solving a problem with a constraint propagation approach, we may not have the contractors at hand for dealing with all the involved constraints. In this lesson for instance, we assume that we do not have a contractor for dealing with this constellation constraint. The goal of the following exercise is to see how to build our own contractor. We will see that it is easy to extend our framework with new contractors, without loss of generality.

**We now focus on the constellation constraint without relation with the other equations.** The constraint is expressed by \(\mathcal{L}_{\mathbb{M}}\left(\mathbf{a}\right)~:~\mathbf{a}\in\mathbb{M}\).

Let us consider a constellation of \(\ell\) landmarks \(\mathbb{M}=\{[\mathbf{m}_{1}],\dots,[\mathbf{m}_{\ell}]\}\) of \(\mathbb{IR}^{2}\) and a box \([\mathbf{a}]\in\mathbb{IR}^2\). Our goal is to compute the smallest box containing \(\mathbb{M}\cap[\mathbf{a}]\). In other words, we want to contract the box \([\mathbf{a}]\) so that we only keep the best envelope of the landmarks that are already included in \([\mathbf{a}]\). An illustration is provided by the following figures:

The definition of the contractor is given by:

where \(\bigsqcup\), called *squared union*, returns the smallest box enclosing the union of its arguments.

Exercise

**D.1.** We can now build our own contractor class. To do it, we have to derive the class `Ctc`

that is common to all contractors. You can start from the following new class:

```
class MyCtc(Ctc):
def __init__(self, M_):
Ctc.__init__(self, 2) # the contractor acts on 2d boxes
self.M = M_ # attribute needed later on for the contraction
def contract(self, a):
# Insert contraction formula here (question D.2)
return a
```

```
class MyCtc : public Ctc
{
public:
MyCtc(const std::vector<IntervalVector>& M_)
: Ctc(2), // the contractor acts on 2d boxes
M(M_) // attribute needed later on for the contraction
{
}
void contract(IntervalVector& a)
{
// Insert contraction formula here (question D.2)
}
protected:
const std::vector<IntervalVector> M;
};
```

`M`

represents the set \(\mathbb{M}\), made of 2d`IntervalVector`

objects;

`a`

(in`.contract()`

) is the box \([\mathbf{a}]\) (2d`IntervalVector`

) to be contracted.

**D.2.** Propose a simple implementation of the method `.contract()`

for contracting \([\mathbf{a}]\) as:

The user manual may help you to compute operations on sets such as unions or intersections, appearing in the formula. Note that you cannot change the definition of the `.contract()`

method.

**D.3.** Test your contractor with:

a set \(\mathbb{M}=\big\{(1.5,2.5)^\intercal,(3,1)^\intercal,(2,2)^\intercal,(2.5,3)^\intercal,(3.5,2)^\intercal,(4,1)^\intercal,(1.5,0.5)^\intercal\big\}\), all boxes inflated by \([-0.05,0.05]\);

three boxes to be contracted: \([\mathbf{a}^1]=([1.25,3],[1.6,2.75])^\intercal\), \([\mathbf{a}^2]=([2,3.5],[0.6,1.2])^\intercal\), and \([\mathbf{a}^3]=([1.1,3.25],[0.2,1.4])^\intercal\).

You can try your contractor with the following code:

```
# M = [ ...
for Mi in M:
Mi.inflate(0.05)
# a1 = IntervalVector([ ...
# a2 = IntervalVector([ ...
# a3 = IntervalVector([ ...
ctc_constell = MyCtc(M)
cn = ContractorNetwork()
cn.add(ctc_constell, [a1])
cn.add(ctc_constell, [a2])
cn.add(ctc_constell, [a3])
cn.contract()
```

```
vector<IntervalVector> M;
// M.push_back(IntervalVector( ...
for(auto& Mi : M)
Mi.inflate(0.05);
// IntervalVector a1( ...
// IntervalVector a2( ...
// IntervalVector a3( ...
MyCtc ctc_constell(M);
ContractorNetwork cn;
cn.add(ctc_constell, {a1});
cn.add(ctc_constell, {a2});
cn.add(ctc_constell, {a3});
cn.contract();
```

## Application for localization¶

We will localize the robot in the map \(\mathbb{M}\) created in Question **D.3**.

Exercise

**D.4.** In your code, define a robot with the following pose \(\mathbf{x}=\left(2,1,\pi/6\right)^\intercal\) (the same as in Lesson C).

**D.5.** Display the robot and the landmarks in a graphical view with:

```
beginDrawing()
fig_map = VIBesFigMap("Map")
fig_map.set_properties(100,100,500,500)
fig_map.draw_vehicle(x_truth,0.5)
for Mi in M:
fig_map.draw_box(Mi, "red[orange]")
# Draw the result of next questions here
fig_map.axis_limits(fig_map.view_box(), True, 0.1)
```

```
vibes::beginDrawing();
VIBesFigMap fig_map("Map");
fig_map.set_properties(100,100,500,500);
fig_map.draw_vehicle(x_truth,0.5);
for(const auto& Mi : M)
fig_map.draw_box(Mi, "red[orange]");
// Draw the result of next questions here
fig_map.axis_limits(fig_map.view_box(), true, 0.1);
```

You should obtain this result.

**D.6.** We will generate a set of range-and-bearing measurements made by the robot. For this, we will reuse a function of Lesson B:

```
v_obs = DataLoader.generate_static_observations(x_truth, M, False)
```

```
vector<IntervalVector> v_obs =
DataLoader::generate_static_observations(x_truth, M, false);
```

where `x_truth`

is the actual state vector of the robot and `M`

the set of landmarks \(\mathbb{M}\). The returned value `v_obs`

is a vector of [range]×[bearing] interval values.

Display the measurements `v_obs`

in the view with `.draw_pie()`

, as we did in the previous lesson. You should obtain a figure similar to this one:

You can easily add uncertainties on these measurements, for instance with:

```
# Adding uncertainties on the measurements
for obs in v_obs:
obs[0].inflate(0.02) # range
obs[1].inflate(0.02) # bearing
```

```
// Adding uncertainties on the measurements
for(auto& obs : v_obs)
{
obs[0].inflate(0.02); // range
obs[1].inflate(0.02); // bearing
}
```

## State estimation without data association¶

We will go step by step and not consider the data association problem for the moment.

We will first reuse the Contractor Network developed in the previous Lesson for range-and-bearing localization, and apply it on this set of observations.

Exercise

**D.7.** With a Contractor Network, perform the state estimation of the robot (the contraction of \([\mathbf{x}]\)) by considering simultaneously all the observations. You can reuse the CN previously implemented in Lesson C for this purpose.

We will assume that:

the identity (the position) of the related landmarks is known;

the heading \(x_3\) of the robot is known.

```
# Define contractors
# ...
cn = ContractorNetwork()
for i in range(0,len(v_obs)): # for each measurement
# Define intermediate variables
# ...
# Add contractors and related domains
# ...
# Note: v_obs[i] is a 2d vector [y^i]
# Note: M[i] is a 2d box of M
cn.contract()
```

```
// Define contractors
// ...
ContractorNetwork cn;
for(int i = 0 ; i < v_obs.size() ; i++) // for each measurement
{
// Define intermediate variables
// ...
// Add contractors and related domains
// ...
// Note: v_obs[i] is a 2d vector [y^i]
// Note: M[i] is a 2d box of M
}
cn.contract();
```

If you are using C++, please read the paragraph related to intermediate variables in C++, at the end of this page.

In the code, the *identity* of each landmark is known in the sense that a measurement `v_obs[i]`

refers to a landmark `M[i]`

.

**D.8.** Display the resulted contracted box \([\mathbf{x}]\) with:

```
fig_map.draw_box(x.subvector(0,1)) # displays ([x_1],[x_2])
```

```
fig_map.draw_box(x.subvector(0,1)); // displays ([x_1],[x_2])
```

You should obtain this result in black (considering uncertainties proposed in Question **D.6**):

## State estimation with data association¶

We now assume that the identities of the landmarks are not known. This means that the `M[i]`

variables will not appear anymore in the resolution of the problem, except for creating the constellation contractor.

Exercise

**D.9.** Outside the resolution part, we create a list of boxes for representing **possible identities** of the landmarks for each measurement.

```
# Association set (possible identities)
m = [IntervalVector(2) for _ in v_obs] # unknown associations for each observation
```

```
// Association set (possible identities)
vector<IntervalVector> m(v_obs.size(), IntervalVector(2));
// unknown association for each observation
```

Each `m[i]`

object is a 2d box corresponding to the \(\mathbf{m}^i\) vector of the equations in the above Section Formalism.

**D.10.** Update the Contractor Network for solving the state estimation together with the data association problem. The CN should now involve the `m[i]`

objects as well as the new contractor you developed in the Section The constellation constraint and its contractor.

```
ctc_asso = MyCtc(M)
# ...
cn.add(ctc_asso, [m[i]])
```

```
MyCtc ctc_asso(M);
// ...
cn.add(ctc_asso, {m[i]});
```

We recall that one contractor can be added to the CN several times together with different domains.

You should obtain exactly the same result as in Question **D.8**.

**D.11.** *Same result* means that the data association worked: each measurement has been automatically associated to the correct landmark without ambiguity.

We can now look at the set `m`

containing the associations. If one \([\mathbf{m}^i]\) contains only one item of \(\mathbb{M}\), then it means that the landmark of the measurement \(\mathbf{y}^i\) has been identified.

**D.3**, we defined \(\mathbb{M}\) as a set of boxes with a width of \(2\times 0.05\). Then, if one box \([\mathbf{m}^i]\) has a maximal diameter that is less than \(0.1\), it means that it contains one single item of \(\mathbb{M}\).

```
for mi in m:
if mi.max_diam() <= 0.10001: # if identified
fig_map.draw_circle(mi[0].mid(), mi[1].mid(), 0.02, "blue[blue]")
```

```
for(const auto& mi : m)
if(mi.max_diam() <= 0.10001) // if identified
fig_map.draw_circle(mi[0].mid(), mi[1].mid(), 0.02, "blue[blue]");
```

Expected result:

## Conclusion¶

This association has been done only by merging all information together. We define contractors from the equations of the problem, and let the information propagate in the domains. This has led to a state estimation solved concurrently with a data association.

In the next lessons, we will go a step further by making the robot move. This will introduce differential constraints, and a new domain for handling uncertain trajectories: tubes.

Underwater application

We could apply this solver on actual data, such as the underwater application presented in the introduction of this lesson. The identification of the rocks on the seafloor would be done by the fusion of all data, without image processing. The same algorithm also provides a reliable way to gather different views of a same object, without using data processing.

This topic of data association with underwater robotics has been the subject of a paper proposed for this ICRA 2020 conference:

**Set-membership state estimation by solving data association**

*ICRA 2020*

Here is a video providing an overview of the problem and how to solve it. This is mainly the object of this Lesson D and the next one, that will deal with the dynamics of the robot.

## End of second step!¶

That’s about all for this second step.

See you next step for discovering tubes!

## Appendix¶

Important

**The case of intermediate variables in C++.**

In C++, if we create intermediate variables in a different block of code than the `.contract()`

method, then the C++ variables are destroyed at the end of their block and so the CN lose the reference to them. For instance:

```
ContractorNetwork cn;
CtcFunction ctc_plus(Function("a", "b", "c", "b+c-a"));
Interval x(0,1), y(-2,3);
if(/* some condition */)
{
Interval a(1,20);
cn.add(ctc_plus, {x, y, a}); // constraint x+y=a
} // 'a' is lost here
cn.contract(); // segmentation fault here
```

Instead, we must create the domains inside the CN object in order to keep it alive outside the block. This can be done with the method `.create_interm_var()`

:

```
ContractorNetwork cn;
CtcFunction ctc_plus(Function("a", "b", "c", "b+c-a"));
Interval x(0,1), y(-2,3);
if(/* some condition */)
{
Interval& a = cn.create_interm_var(Interval(1,20));
cn.add(ctc_plus, {x, y, a});
} // 'a' is not lost
cn.contract(); // the contraction works on [x], [y] and [a] that still exists
```

Therefore, the following function:

```
IntervalVector& d = cn.create_interm_var(IntervalVector(2));
```

creates for instance an intermediate 2d variable with a 2d box domain: \([\mathbf{d}]=[-\infty,\infty]^2\). Its argument defines the type of domain to be created (`Interval`

, `IntervalVector`

, *etc.*).

Technically, `create_interm_var()`

returns a C++ *reference* (in the above example, of type `IntervalVector&`

). The reference is an alias on the intermediate variable, that is now inside the Contractor Network. It can be used in the same way as other variables involved in the CN.