IEEE Robotics & Automation Magazine - June 2013 - 47

each setting. It can be seen that for case
A, a very high percentage of accurate or
Accuracy (M, RAD)
very accurate solutions is reached,
whereas in the second case B, the accuy 1 0.025
y 1 0.05
y 1 0.2
racy seems lower. Only the Y and i
i 1 0.045
i 1 0.09
i 1 0.16
Case, Noise (M, RAD)
final values are checked, as the nature
A x, y, i ! ! 0.5
99%
100%
-
of the environment does not provide
A x, y ! ! 1.5, i ! ! 0.38
98%
98%
98%
any information about X.
B x ! ! 1.5 y ! 5 ! 1 i ! ! 0.38 62%
73%
93%
It is important to highlight the fact
that inaccurate solutions are not necessarily incorrect, although they preswould arrive at such an uninformative minimum. To take ent a poorer final alignment between the corridors. In any
into account the fact that the desired solution is the closest case, the algorithm still outputs a physically feasible conto the initial position r 0AB , a weighted term can be added to figuration that is more likely than the initial one, as can be
(7), resulting in
seen in Figure 11.
The main contribution of this section is not only the
0
novel
minimization method proposed, but also its
h (M A, M B, rAB, r AB) =
0
potential
applicability to online mapping problems
= e (M A, M B, rAB) + K rAB - r AB , (8)
because of GPU computing, which was taken into
account while designing this algorithm. Most of the prowhere rAB _ x 2 + y 2 + L2 i 2 is the weighted Euclidean cessing time consumed by the CPU is taken by (7),
norm that uses L to normalize angular into distance units. which is a large summation over all the cells of a grid
With these modifications, the cost function depicted in map and has to be repeated every time the cost needs to
Figure 7 becomes more adequate for minimization pur- be evaluated. This summation can be parallelized in a
poses, as shown in Figure 9.
CUDA kernel as shown in Figure 12. The grid is comOnce the cost function is defined and can be evaluated posed by m # n blocks, each one with 256 threads. The
using (8) for every possible relative pose, the Broyden- parallelization uses a hierarchical summation scheme to
Fletcher-Goldfarb-Shanno (BFGS) algorithm, which is avoid excessive waits because of synchronization locks
known to perform well in many situations, is applied. At that appear in the last line of the function SumThread,
each iteration, the value of the cost function and its gradi- which must be necessarily performed as atomic. If just
ent are computed according to the four-point formula one single variable was used as in the sequential version,
and, afterwards, they are fed into the open-source imple- all the threads of every block would always collide, as
they are executed simultaneously. Consequently, using
mentation of [26].
To analyze the performance of
the algorithm in terms of accuracy,
the following experiment is con0,18
ducted with two grid maps corresponding to the same corridor. As
0,16
shown in Figure 10, there are different physical feasible configura0,14
tions or solutions depending on
0,12
the initial pose; if the initial pose is
close to the center, then the most
0,1
likely solution is that both corridors are the same, but if the initial
0,08
position is not so close to the cen0,06
ter, then it could be more likely that
the corridors are actually parallel.
0,04
The results in terms of accuracy
are summarized in Table 5, where
0,02
different initial pose intervals are
0
used. The first two lines correspond to the matching case A, and
Y Displacement (m)
the last one to the parallel corridors
case B. The experiment is repeated
with 100 random initial poses for Figure 9. Final cost function with (8) and submaps blurring.
128

X Displacement (m)

Cost (Error)

Table 5. Accuracy (M, RAD).

4

1
-1 2
-11
0
-9
-8
-7
-6
-5
-4
-3
-2
-1
0
1
2
3
4
5
6
7
8
9
10
11
12

0 4
- 8 2
- 1
-

june 2013

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

47
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2013
