IEEE Robotics & Automation Magazine - December 2013 - 90

between the two locally referenced poses is computed using
Algorithm 1 and is then converted to the relative transformation between the global reference poses of the two submap coordinate frames.
To reduce the computational complexity of SLAM, we
introduce two strategies. First, the current pose stored in
the map can have a single loop closure constraint using
Algorithm 1. Second, the largest loop closure constraint
among multiple loop closure candidates is selected for the
current pose. These strategies are necessary to apply a
pose graph optimization technique in a low-cost embedded
system because the computational complexity is proportional to the number of poses and the number of loop closure constraints.
In this study, the visual compass output obtained in a certain short trajectory {x i, ..., x i +k} is used to construct a loop
closure constraint that describes only a rotational angle
between the two poses x i and x i +k . This is because the proposed visual SLAM algorithm corrects the robot trajectory to
be estimated as nodes of the graph using the spatial constraints identifying the relative transformation between poses.
Assume that a set of correction angles of the visual compass
{Di VC corr.(x idx(j)), j = 1, f, n} is obtained in a part of the short
trajectory {x i, ..., x i +k} , where idx (j) is an index of the pose
with the correction angle obtained by the visual compass.
Here, the average correction angle Di VC avg.corr. for the set of
correction angles {Di VC corr.(x idx(j)), j = 1, f, n} is used to
obtain the loop closure constraint that describes a rotational
angle Di (z i +k, i) between the two poses x i and x i +k , as
Di (z i +k, i) = i (x i) - (i (x i +k) + Di VC

),

avg.corr.

(10)

where i (x i) and i (x i +k) are the orientations of the two poses x i
and x i +k , respectively. In our tests, the weight of the rotational
angle Di (z i +k, i) required to compute (1) was set to a relatively
high value compared to that of the rotational angle term in the
loop closure constraint obtained using Algorithm 1 because
the visual compass orientation was comparatively accurate.
This scheme allows the visual compass to smoothly reduce
the orientation errors of the robot trajectory to be estimated
as nodes of the graph.
Pose Graph Optimization
Whenever the current submap is closed, the alignment of
local submaps is performed by minimizing a sum of
weighted square costs expressed by spatial constraints
between the global reference poses of the submap coordinate frames. Such a formula is equal to (1) because all of the
constraints between the submaps mean the relative transformations between poses. Therefore, the locally referenced
poses in each submap and the global reference poses of the
submap coordinate frames can be corrected by the same
pose graph optimization method.
In (1), 6 (which is the inverse of compounding operation
5) [16] is a nonlinearity function. In this article, the constraint
z i, j and the nonlinearity function h (x i, x j) are given by
90

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

DECEMBER 2013

Dx (z i, j)
z i, j = >Dy (z i, j)H
Di (z i, j)
DX i, j cos (i (x i)) + DYi, j sin (i (x i))
+ h (x i, x j) = >-DX i, j sin (i (x i)) + DYi, j cos (i (x i))H, (11)
i (x j) - i (x i)
where x i =[x (x i), y (x i), i (x i)] T , x j =[x (x j), y (x j), i (x j)] T , DX i, j =
(x (x j) - x (x i)), and DX i, j = (y (x j) - y (x i)) . The nonlinearity of
(11) arises from the orientation i (x i) of the pose x i . An
approach for efficiently solving (1) is to linearize a constraint z i, j
with respect to the orientation i (x i) [23]. The linearized form of
(11) is given by
Dx (z i, j) cos (i (x i)) - Dy (z i, j) sin (i (x i))
z i, j (linearized) = >Dx (z i, j) sin (i (x i)) + Dy (z i, j) cos (i (x i))H
Di (z i, j)
x (x j) - x (x i)
(12)
+ h (x i, x j) (linearized) = > y (x j) - y (x i)H .
i (x j) - i (x i)
The covariance matrix W -i, 1j (linearized) of z i, j (linearized) can also be
represented by
W -i, 1j (linearized) = J z W -i, 1j J Tz ,

(13)

where J z is the Jacobian matrix 2z i, j (linearized) /2z i, j , and W -i, 1j
is the covariance matrix of z i, j .
Using (12) and (13), (1) is represented in a linearized version and can be solved by a linear solver that handles a linear
least-square problem such as AX = B [23]. The covariance
matrix of the solution is used to define the covariance matrix
W -i, 1 of any pose x i in the graph. There is a major problem in
using the linearized version of (1), which is that the linearization errors cannot be avoided due to orientation errors of the
poses. The orientation errors will have a severe effect on the
performance of SLAM. Therefore, the linearized version of
(1) will hold for real-world applications, when the orientation
errors are small. To overcome this problem, the visual compass has been developed using the orthogonal structure characteristics of an indoor environment and is applied to accurately make the loop closure constraint that represents a
rotational angle between nonconsecutive poses.
To more efficiently solve the linearized version of (1), the
location and orientation terms of the poses are separately estimated in this article. First of all, the orientation terms of the
poses in (1) are directly corrected by a linear solver because
the correction of the orientation terms in this article is a linear
estimation problem. The corrected orientations are again
used to compute (12) and (13), and the linearized version of
(1) is obtained. Here, the location terms are only considered
in estimation and are corrected by a linear solver that computes the linearized version of (1).
In summary, the pose graph optimization method presented in this section solves two linear estimation problems to
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2013
