Consistent Pose Estimation CPE

1

• Consistent Pose Estimation (CPE)
• SLAM
• SRI Kurt Konolige, Keith Nicewarner, Benson
  Limketkai
• SONY Steffen Gutmann
• U of Washington Dieter Fox, Jonathan Ko,
  Benjamin Stewart, Dirk Schulz

2
(No Transcript)
3
An Example of CPE in Action
4
Outline of Talk

• Matching via Correlation
• Constraints and Covariances
• General Least Squares optimization
• Non-linear case
• Practical considerations, incremental method
• Computational issues Complex Gradient
• General Linear Markov-Gauss (Kalman Filter) and
  its relation to GLSQ

5
Scan Matching as Markov Localization

• One scan or set of scans is the reference map
• Robot is localized within the map using an
  observation scan.

6
Markov Localization CorrelationKonolige and
Chou 1999

• Discretize the map and the observation scan
  (sensor patch)
• Correlate the sensor patch against the map grid

7
Correlation computation

• Coarse search
• Map 10 mm resolution
• Patch 2 deg resolution
• Search area 600 mm x 600 mm
• 40K poses of 100 readings
• 1 ns / pose-reading
• Log form of Corr uses addition of small integers
• Efficient SIMD implementation
• 4 ms

8
Outline of Talk

• Matching via Correlation
• Constraints and Covariances
• General Least Squares optimization
• Non-linear case
• Practical considerations, incremental method
• Computational issues Complex Gradient
• General Linear Markov-Gauss (Kalman Filter) and
  its relation to GLSQ

9
Constraints as Random Variables

• Sensor readings are matched to yield constraints
  between poses
• Constraints are between local frames
• Constraints are represented by random variables

x,y,?
x,y,?
10
Robot Poses and Scans Lu and Milios 1997

• Successive robot poses are connected by odometry
• Sensor readings are matched to yield constraints
  between poses
• Constraints are represented by

11
Outline of Talk

• Matching via Correlation
• Constraints and Covariances
• General Least Squares optimization
• Non-linear case
• Practical considerations, incremental method
• Computational issues Complex Gradient
• General Linear Markov-Gauss (Kalman Filter) and
  its relation to GLSQ

12
Consistent Pose Estimation (CPE-SLAM)

• Network of constraints Dij
• Poses are free variables Xi
• Constraints are covariances between poses
• What is the globally optimal estimate for the
  poses?

Gaussian errors, mutual independence
13
Incremental Consistent Pose Registration

• Construct local incidence net (scan match
  odometry other relations)
• Optimise using LuMilios scheme
• Whats the best neighborhood?

14
Local Neighborhood Error

• Anecdotal evidence for a single environment

15
Map Correlation

• Identify when were in an area weve been in
  before, and not well-localized
• Compute scalar product of patch with map for
  different poses.
• Result is a pose prob. grid.
• Search area deteremined by relative uncertainty
• Efficient Implementation (IJCAI99)

16
Closing the Loop

• Use scan patches to increase selectivity
• Identify a common tack point between scan patches
• Deform the network based on covariances of matches

17
Multi-Robot Mapping

• Add new constraint into a partial map from any
  robot
• Three types of pose/constraint addition

18
Frontier Based Exploration
Yamauchi et al. 96, Thrun 98
19
Coordinated Exploration
Burgard et al. 00, Simmons et al. 00
20
Multi-Robot Exploration and Mapping

• We know how to perform
• robot localization Fox et al. 99, Jensfelt
  et al. 01 .
• single robot mapping and exploration Gutmann
  and Konolige 99, Thrun 98.
• multi-robot mapping and exploration if we
  have knowledge of the robots start locations
  Burgard et al. 00, Thrun et al. 00,Howard
  et al. 02, Konolige et al. 00,Fenwick et al.
  02Williams et al. 02,Zlot et al. 02.
• Distributed multi-robot exploration with
  arbitrary start locations has not been solved.

21
Why is This Hard?
Robot A
Robot B
Robot C
22
Exploration with Unknown Start Locations

• Problem
• Complexity grows exponentially in the number of
  robots.
• False map merges are hard to undo.
• Approach
• Initially robots explore individually
• Whenever robots can communicate they
• estimate their relative locations
• physically verify hypotheses before map merging
• coordinate exploration after map merging

23
(No Transcript)
24
(No Transcript)
25
Coordination for Map Merging
26
Centibots Mapping Evaluation
27
Centibots Mapping Evaluation
28
Centibots Mapping Evaluation

• Unofficial runs
• 3 robots 16 mins
• 2 robots, unknown start locations 26 min

29
Scaling to Large Environments
30
Scaling to Large Environments

• Results
• Simple loop closure is linear in the size of the
  loop
• General registration grows as N log N
• 1K poses in 5 seconds10K poses in less than 1
  minute

Solve a linear system
31
Visual Odometry