Active SLAM : a FrameworkMy, on-going, PhD Research

Henry Carrillo LindadoAdvised by:

José A. Castellanos

Bio – Academic Background

Name: Henry David Carrillo Lindado. Hometown: Barranquilla – Colombia. Academic:

PhD in Computer Science and System Engineering (2010 -2014) University of Zaragoza - Spain

M.Sc. in Computer Science and System Engineering M.Sc. in Electronics Engineering B.Eng. in Electronics Engineering

Funding: FPI scholarship by the Ministry of Science and Innovation of Spain. 2010-2014.

Contact: Here: 0.59 Cartesium hcarri@unizar.es http://webdiis.unizar.es/~hcarri/pmwiki/pmwiki.php

1

So, What is my PhD about? Objective: To build an active SLAM framework. Why?:

Where should I go in order to improve my localization and map representation?

If I go from A to B, will I be lost (e.g. Unable to localize)?

What movements should I make in order to keep my metrical error below X mm?

Aim at: Metrical representations. Topological representations. Metrical+Topological representations.

2

What have I done?

Metrical

3

Preliminaries – SLAM H0: A model of the operative environment is an

essential requirement for an autonomous mobile robot.

Three basic tasks: Where am I? What does the world look like? Where do I go?

SLAM => Joint of two tasks. SLAM => Does not define

the path-trajectory of the robot. Integrated approach => On the way to autonomy.4 Exploration and Mapping with Mobile Robots. Cyrill Stachniss. 2006.

Preliminaries – Active SLAM (I) Active SLAM => To integrate path planning into

a SLAM process. To explorer more area. Navigate safely. Reduce uncertainty.

Algorithms 1º Alg. [Feder, Leonard](99)

Active perception [Bajacksy](86) Infinite Horizon and MPC [Leung, Dissanayake](06)

5

Preliminaries – Active SLAM (II)

Pseudo-code:

Set of trajectories Assign a score to each trajectory

Uncertainty of map+robot Trajectory constraints

Execute the trajectory with the optimum .

6

Preliminaries – Active SLAM (II)

Pseudo-code:

Set of trajectories Assign a score to each trajectory

Uncertainty of map+robot Trajectory constraints

Execute the trajectory with the optimum .

6

Preliminaries – Active SLAM (II)

Pseudo-code:

Set of trajectories Assign a score to each trajectory

Uncertainty of map+robot Trajectory constraints

Execute the trajectory with the optimum .

6

J1 J2 J3 J4 J51 1,5 1,9 0,8 3

Preliminaries – Active SLAM (II)

Pseudo-code:

Set of trajectories Assign a score to each trajectory

Uncertainty of map+robot Trajectory constraints

Execute the trajectory with the optimum .

6

J1 J2 J3 J4 J51 1,5 1,9 0,8 3

Uncertainty Criteria for Active SLAM (I) Uncertainty/Inform. Criteria =>

In the TOED, a design (i.e. ), is better than a design, if:

The above does not allow to quantify the improvement, therefore is desirable to:

It permits to quantify the uncertainty in .

7

• Theory of Optimal Experiment Design (A-opt, D-opt, E-opt…).

• Information Theory ( Entropy, MI…).

Uncertainty Criteria for Active SLAM (II) Some possible uncertainty criteria for active SLAM

are:

Previous works ([Sim and Roy, 2005], [Mihaylova and De Schutter, 2003]) report A-opt as the best criterion and that D-opt gives null values. A-opt, widely used: [Kollar2008] [MartinezCantin2008]

[Meger2008] [Dissanayake2006]. Although D-opt is commonly used in the TOED

because it is optimal.8

Determinant (D-opt)

Trace (A-opt)

max (𝜆1 ,…,𝜆𝑘)

Max (E-opt)

trace (Σ )= ∑𝑘=1 ,… , 𝑙

𝜆𝑘det (Σ )= ∏𝑘=1 ,…, 𝑙

𝜆𝑘

Uncertainty Criteria for Active SLAM (III) It is indeed possible to use D-opt in the Active

SLAM context: The structure of the problem needs to be taken into account

(i.e. The covariance matrix varies with time). It is not informative to compare the determinant of a matrix l x

l with a m x m. det(l x l) is homogeneous of grade l.

The computation of the determinant of a highly correlated matrix (e.g. SLAM) is prone to round-off errors. Processing in the logarithm space

D-opt for a l x l covariance matrix:

Stem from [Kiefer, 1974] :9

First experiment First experiment: on the computation

Is it possible to compute D-opt from a robot doing SLAM?

Execute a SLAM algorithm (e.g. EKF-SLAM, iSAM). Compute in each step: A-opt, E-opt , D-opt,

Determinant, entropy and mutual Information.

• Simulated Robot indoor environment : MRPT/C++

• Real Robot indoor environment : Pioneer 3 DX - Ad-hoc

• Real Robot indoor environment : DLR dataset• Real Robot outdoor environment : Victoria

Park dataset10

1E - Simulated Robot indoor environment (I)

Scenario: Area of 25x25 m 2D EKF-SLAM Sensor: Odometry +

Camera (360º - 3m range)

180 landmarks - DA Known.

Gaussian errors: Odometry + Sensors

11

1E-Simulated Robot indoor environment (II) Qualitative results

(a)-(f) A-opt, E-opt, D-opt, determinant, entropy and MI.12

1E-Real Robot indoor environment @ DLR

Scenario: Area 60x40 m Sensor: Odometry + Camera

2D EKF-SLAM 576 landmarks – DA known.

13

1E-Real Robot indoor environment @ DLR Qualitative results

(a)-(f) A-opt, E-opt, D-opt, determinant, entropy and MI.14

First experiment – Quantitative analysis Average correlation between the uncertainty

criteria:

Variance: A-E (0,0002) / A-D (0,0540) / D-E (0,0481).

A-opt y E-opt => High correlation. E-opt is guided by a single eigenvalue.

A-opt y D-opt => Medium correlation. H0: D-opt take into account more components than A-opt.

A-opt E-opt D-optA-opt 1 0,9872 0,6003E-opt 0,9872 1 0,5903D-opt 0,6003 0,5903 1

15

Second Experiment Second experiment: Active SLAM

What is the effect of the uncertainty criteria in active SLAM?

Active SLAM => Unitary horizon (greedy). Uncertainty criteria => A-opt, D-opt and

Entropy. Effect => MSE y .• Simulated Robot with unitary horizon: MRPT /

C++

16

2E-Simulated Robot indoor environment (I)

Scenario: Area of 20x20m and

30x30m 2D EKF-SLAM Sensor: Odometry +

Camera (360º - 3m range)

Gaussian errors: Odometry + sensors.

Path planner: Discrete (A*) and continuous (Attract-Repel).

17

2E-Simulated Robot indoor environment (II)

Resulting paths for each uncertainty criterion: (a) D-opt, (b) A-opt y (c) Entropy. Each colour represents an executed path. 20 x 20 m map.

• Qualitative analysis

18

2E-Simulated Robot indoor environment (III)

Resulting trajectories for 10000 steps active SLAM simulation. (a). Initial trajectory. (b) A-opt. (c). D-opt.

• Qualitative analysis.

19

2E – Quantitative Analysis 30x30 m

Evolution of MSE ((a)-(c)) y chi2 ((d)-(f)) ratio. Average of 10 MC simulations.

20

Take home message D-opt is the optimum criterion to measure

uncertainty according to the TOED (i.e. better than A-opt (Trace)).

It is possible to obtain useful information regarding the uncertainty of a SLAM process with D-opt.

D-opt shows better performance than A-opt in our simulated experiments of active SLAM.

To compute D-opt in the context of a SLAM process => use the formulation presented here.

21

What have I done?

Metrical: an example using D-

opt22

FaMUS: Fast Minimum Uncertainty Search

17

• Minimum uncertainty path between A to B in a graph.

• Exhaustive search.

FaMUS: Fast Minimum Uncertainty Search

17

• Minimum uncertainty path between A to B in a graph.

• Exhaustive search.

FaMUS: Fast Minimum Uncertainty Search

24

Experiment: Are the minimum uncertainty path and the shortest path necessarily equal? Select two points A and B, and compare the final

uncertainty. 1000 times x 4 datasets. (Biccoca, Intel , New

colleges and Manhattan).

FaMUS: Fast Minimum Uncertainty Search

25

Examples of paths.

FaMUS: Fast Minimum Uncertainty Search

26

Summary of results

• Improvement of a least 50% in timing respect to the state of the art. [Valencia2011]

What have I done?

Topological

27

Topological Guiding question:

Where should I go in order to improve my topological map?

Challenges: well-posed and egocentric images. Execute a SLAM algorithm (e.g. EKF-SLAM, iSAM). Compute in each step: A-opt, E-opt , D-opt,

Determinant, entropy and mutual Information.

28

Topological One solution:

Textons (a.k.a gist)- Undelaying Structure- Probabilistic decision

29

What have I done?

TBD

30

TBD Which are the confidence intervals in the active

predictions? When do I stop the active behaviour?

Find a relationship between uncertainty and metrical error.

Use other constraints other than uncertainty. Speed up the decision process.

Real experiments.

31

Active SLAM : a FrameworkMy, on-going, PhD Research

Thanks!!!hcarri@unizar.es

http://webdiis.unizar.es/~hcarri

32

Experimentos Primer experimento : acerca del cálculo

Segundo experimento : SLAM activo

• Robot simulado ambiente interior : MRPT / C++

• Robot real ambiente interior : Pioneer 3 DX - Ad-hoc

• Robot real ambiente interior : DLR dataset• Robot real ambiente exterior : Victoria Park

dataset

• Robot simulado con horizonte unitario : MRPT / C++

7

1E-Robot en ambiente exterior @ VP (I)

Escenario: Área de 350 x 350 m iSAM Sensor: Odometría +

Laser 150 landmarks – DA

conocida.

13

1E-Robot en ambiente exterior @ VP (II) – Resultados cualitativos

(a)-(f) A-opt, E-opt, D-opt, determinante, entropía y MI.14

1E-Robot en ambiente interior ad-hoc (I)Escenario:

Área 6x4 m 2D EKF-SLAM Sensor: Odometría +

Kinect 5 landmarks – DA

conocida

15

1E-Robot en ambiente interior ad-hoc (II) – Resultados cualitativos

(a)-(f) A-opt, E-opt, D-opt, determinante, entropía y MI.16

2E - Análisis cuantitativo 20x20 m

Evolución del MSE ((a)-(c)) y chi2 ((d)-(f)). Promedio de 10 MC.

18

DeterminanteOperación algebraica que transforma una matriz en un escalar. Propiedades (matriz n x n)

Geométrica: Volumen del paralelepípedo definido en el espacio n-dimensional.

Homogéneo de grado n. Si,

15

Artículos “Experimental Comparison of Optimum

Criteria for Active SLAM”. Oral presentation in the “III Workshop de Robótica: Robótica Experimental (ROBOT’11)”.

“On the Comparison of Uncertainty Criteria for Active SLAM”. Submitted to ICRA’12.

“Planning Minimum Uncertainty Paths Over Pose/Feature Graphs Constructed Via SLAM” . Submitted to ICRA’12.

18

On the Comparison of UncertaintyCriteria for Active SLAM

Thanks!!!hcarri@unizar.es

http://webdiis.unizar.es/~hcarri

19

FaMUS: Fast Minimum Uncertainty Search

17