Sparser Relative Bundle Adjustment:
constant-time maintenance and local
optimization of arbitrarily large maps
José-Luis Blanco-Claraco
Javier González-Jiménez
Juan-Antonio Fernández-Madrigal
May 7, 2013
Karlsruhe – ICRA 2013
Long-term goal: truly autonomous robots
Most roboticians agree a robot must autonomously
learn how the world looks like and where it is:
SLAM
1/4: Introduction
2
Ideal SLAM pipeline… for a life time?
Sensory data
SLAM
front-end
SLAM
back-end
Data
association
The map
Where am I?
1/4: Introduction
World
model
3
Ideal SLAM pipeline… for a life time?
Sensory data
SLAM
front-end
SLAM
back-end
Data
association
The map
Where am I?
1/4: Introduction
World
model
3
Ideal SLAM pipeline… for a life time?
Sensory data
SLAM
front-end
SLAM
back-end
Data
association
The map
Where am I?
1/4: Introduction
World
model
3
That’s the question
SLAM
back-end
Rational decision towards O(1)?
To use global coordinates
Not to use global coordinates
1/4: Introduction
World
model
4
SLAM in relative coordinates
1
?
?
0
?
?
2
a
b
?
?
c
5
3
?
4
?
1/4: Introduction
5
SLAM in relative coordinates
1
?
?
Unknowns:
• Keyframe-to-keyframe poses
0
?
?
?
2
a
• Landmark relative positions
?
b
?
?
c
5
3
Known data:
• Observations
?
4
?
1/4: Introduction
5
SLAM in relative coordinates
1
?
?
0
?
?
2
Key for O(1): Locally consistent maps
a
b
?
?
c
5
3
?
4
?
Introduced by Gabe Sibley and colleagues:
• G. Sibley, “Relative bundle adjustment,” Department of Engineering
Science, Oxford, 2009.
• G. Sibley, C. Mei, I. Reid, and P. Newman, “Adaptive relative bundle
adjustment”, RSS 2009.
1/4: Introduction
6
Loop closing in relative SLAM
1
?
1
?
?
?
0
0
?
?
?
2
a
?
b
vs.
a
?
b
?
?
c
2
5
?
?
c
5
3
?
4
?
2/4: Loop closures in RBA
3
?
4
?
7
Edge creation and the locally-consistent area
✓ Fewer unknowns
✓ Consistent maps are larger
Tradeoff ?
1
?
1
?
?
?
0
0
2
2
vs.
?
?
5
?
5
3
?
4
?
2/4: Loop closures in RBA
3
?
4
?
8
A totally new problem: edge creation
The problem of edge-creation:
Given a set of observations, how many and which edges
should be created?
2/4: Loop closures in RBA
9
A totally new problem: edge creation
The problem of edge-creation:
Given a set of observations, how many and which edges
should be created?
Optimal solution? We still don’t know
2/4: Loop closures in RBA
9
The power of the edge creation policy
Different policies:
• The “intuitive” linear graph policy: (
2/4: Loop closures in RBA
RBA [G.Sibley et al.])
10
The power of the edge creation policy
Different policies:
• The “intuitive” linear graph policy: (
RBA [G.Sibley et al.])
• All edges to the same keyframe: (
becomes global SLAM)
2
1
3
4
0
2/4: Loop closures in RBA
10
The power of the edge creation policy
Different policies:
• The “intuitive” linear graph policy: (
RBA [G.Sibley et al.])
• All edges to the same keyframe: (
becomes global SLAM)
2
1
3
4
0
• Something in between?
2/4: Loop closures in RBA
10
Our proposed policy
Inspired by hierarchical submapping methods:
1
2
5
4
3
6
0
7
8
16
12
15
14
11
9
13
2/4: Loop closures in RBA
10
11
Our proposed policy
Inspired by hierarchical submapping methods:
1
2
5
4
3
6
0
7
8
16
12
15
14
11
9
13
10
Probably, the first framework that seamless integrate global and relative coordinates.
2/4: Loop closures in RBA
12
The need for spanning trees in RBA
1
2
?
5
4
3
6
0
7
Observation model of
landmark include the path
from:
observer KF
base KF
8
16
12
15
14
11
9
13
10
3/4: Incrementally building spanning trees
13
The need for spanning trees in RBA
1
2
?
5
4
3
6
0
7
Observation model of
landmark include the path
from:
observer KF
base KF
15
8
16
12
12
15
14
11
9
13
10
13
Keep STs up to a maximum topological depth Dmax
3/4: Incrementally building spanning trees
14
16
0
8
1
14
An algorithm for updating STs: basic idea
New node
New edge
n
ik
…
…
Distance:: ST.D[r][n]
Distance:: ST.D[s][i
ST.D[s][ k]
ST.N[s][ik]
ST.N[r][n]
r
Spanning
tree of n
?
s
Spanning
tree of ik
3/4: Incrementally building spanning trees
15
An algorithm for updating STs: basic idea
New node
For all r and s:
Is the new path shorter?
Is a new path in range of
Dmax?
New edge
n
ik
…
…
Distance:: ST.D[r][n]
Distance:: ST.D[s][i
ST.D[s][ k]
ST.N[s][ik]
ST.N[r][n]
r
Spanning
tree of n
?
Update the STs accordingly:
• r s: Go towards “n”
• s r: Go towards “ik”
s
Spanning
tree of ik
3/4: Incrementally building spanning trees
15
An algorithm for updating STs: complexity
Computational complexity:
O ( N log N R )
2
R
1
?
?
0
2
?
?
5
NR
3
?
4
?
3/4: Incrementally building spanning trees
16
An algorithm for updating STs: complexity
Computational complexity:
O ( N log N R )
2
R
1
?
?
0
In practice: O(1)
2
?
?
5
NR
3
?
4
?
3/4: Incrementally building spanning trees
16
Experiments
4/4: Results
17
Experiments: (1) Monocular SLAM
4/4: Results
18
Experiments: (1) Monocular SLAM
4/4: Results
18
Experiments: (1) Overall processing time
Total: 55,000 keyframes, 4,000,000 observations, 400,000 landmarks
4/4: Results
19
Experiments: (2) 2D graph-SLAM demo
4/4: Results
20
Experiments: (2) 2D graph-SLAM demo
4/4: Results
21
Conclusions
4/4: Results
22
Conclusions
• Contributions:
Proposal of edge-creation policies as worthy of
research.
Blended global-relative coordinates, similar to
submapping.
O(1) algorithm for online updating of spanning trees.
4/4: Results
22
Open source release
Public C++ implementation.
Policy-based design
flexibility
4/4: Results
23
Open source release
Public C++ implementation.
Policy-based design
flexibility
SLAM and BA-like problems
4/4: Results
Relative Graph SLAM
23
Open source release
Available in Ubuntu 13.04 official repository (other distros
can use PPA)
$ sudo apt-get install libmrpt-dev mrpt-apps
$ srba-slam --help
More online:
http://www.mrpt.org/srba
4/4: Results
24
Sparser Relative Bundle Adjustment:
constant-time maintenance and local
optimization of arbitrarily large maps
Thanks
for your
attention!
More info online:
http://www.mrpt.org/srba