IEEE Robotics & Automation Magazine - December 2013 - 91

correct the robot trajectory (that is represented based on a
hierarchical map structure), and its performance depends on
the visual compass accuracy.
Localization
The simple addition of new poses to the graph causes the system to grow unacceptably large over time. Reducing the graph
size is necessary if large-scale map making is to be complete in
a reasonable amount of time. The basic idea for reducing the
map size is to uniformly distribute the poses to be estimated in
a space to spatially minimize the number of overlapping poses.
To achieve this objective, an occupancy grid map is used. A
new pose is added to the graph when the robot is located in an
unoccupied grid. This approach is possible because feature
matching between two adjacent images captured from an
upward-looking camera is possible regardless of a rotation of
the robot about the z-axis of the robot base reference frame. If
the robot moves in occupied grids, the localization process is
performed instead of the pose graph optimization process
described in the "Pose Graph Optimization" section.
Shown in Figure 9 is an example of localizing the overlapping pose x k . Figure 9(a) is a given graph to correct the pose
x k using the loop closure constraint z k, j -2 . In this study, the
overlapping pose x k is updated based on the Kalman filter
concept. Figure 9(b) describes the prediction and observation
for the pose x k with respect to the base reference frame. The
prediction of the pose x k and its covariance matrix are given by
x k (pred.) = x k -1 5 z k -1, k
1
= W k--1 1 + J 5 W k--1 1, k J T5,
W k-(pred.)

(14)

where J 5 is the Jacobian matrix of compounding operation
5 . In a similar manner, with the loop closure constraint
z k, j -2 and the pose x j -2 , the observation of the pose x k and
1
its covariance matrix W -k (observ.)
can be computed as
x k (observ.) = x j -2 5 (6 z k, j -2)
1
= W -j -1 2 + J 5 (J 6 W k-,1j -2 J T6) J T5,
W k-(observ.)

In a real scenario, the robot may move to its destination far
from the current submap. Before closing the current submap,
the robot pose as a locally referenced pose is updated in the
current submap using (14)-(17). However, the robot is gradually far from the current submap, and the pose correction in
the current submap is finally impossible. In this case, the current submap is automatically closed when the uncertainty of
the robot pose with respect to the base reference of the current
submap reaches a limit. After that, the robot tries to make the
loop closure constraint between the current robot pose and
any pose stored in the map. To update the robot pose with the
loop closure constraint, poses expressed in (14)-(17) are represented as globally referenced poses.
If the pose x k +p is located in an unoccupied grid and is
thus permanently added to the graph, the incremental constraint z k -1, k +p between the two poses x k +p and x k -1 is
obtained by simply computing x k +p 6 x k -1 with the two
poses x k +p [estimated using (16) and (17)] and x k -1 [corrected using (1)]. In this study, the covariance matrix
W -k -1 1, k +p of the incremental constraint z k -1, k +p is heuristically defined by the error covariance matrix predicted from
the odometry motion model without regard to W -k +1 p, updated
using (16) and (17). This scheme prevents the covariance
matrix W -k -1 1, k +p from becoming smaller than those for other
incremental constraints. In our tests, the incremental constraint z k -1, k +p with an overly optimistic covariance matrix
sometimes resulted in an inconsistent global map. This problem was more serious when a trajectory between the two poses
x k +p and x k -p was long. If the pose x k +p is used as a base reference of new submap, the incremental constraint between the
just previous and new submaps can be obtained by representing the pose x k +p as a locally referenced pose in the just previous submap with the aforementioned z k -1, k +p and W -k -1 1, k +p.

xk

zk-1, k

xk-1

xk-2

(15)

where J 6 is the Jacobian matrix of 6 (which is the inverse of
compounding operation 5). After computing (14) and (15),
the pose x k and its covariance matrix are updated using the
standard Kalman filter formulation:

zk-2, j

zk, j-2
xj-2

zj-2, j-1

The prediction of the next pose x k +1 and its covariance
matrix are also given by

zj-1, j

xj

xk (Prediction)

xk

Base
Reference

xk (Observation)

(17)

Two types of overlapping poses can be considered: one for
local localization before closing the current submap and the
other for global localization after closing the current submap.

xj-1
(a)

1
^ W k (pred.) x k (pred.) + W k (observ.) x k (observ.) h
x k (update) = W -k (update)
1
W k-(update)
= ^W k (pred.) + W k (observ.) h-1 .
(16)

x k +1(pred.) = x k (update) 5 z k, k +1
1
W k-+1 1(pred.) = W k-(update)
+ J 5 W k-,1k +1 J T5 .

zk-2, k-1

(b)
Figure 9. An example of localizing the overlapping pose x k
using the loop closure constraint z k, j -2 . (a) Given graph.
(b) Prediction and observation for x k .

DECEMBER 2013

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

91
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2013
