Content uploaded by Andreas Birk
Author content
All content in this area was uploaded by Andreas Birk on Aug 23, 2022
Content may be subject to copyright.
Relaxing Loop-Closing Errors in 3D Maps
Based on Planar Surface Patches
Kaustubh Pathak, Max Pfingsthorn, Narunas Vaskevicius, and Andreas Birk
Robotics Group, Jacobs University Bremen, 28759 Bremen, Germany
Abstract—3D mapping using large planar patches results in a
compact and easily understandable representation of the environ-
ment as compared to point-cloud or voxel based representations.
In this work, relaxation of loop-closing errors in a 3D map
is presented. It is based on a plane-matching algorithm for
correspondence-finding and least-squares registration of large
planar patches fitted on 3D range-images. This method also
provides covariance matrices for the pose-registration result.
Based on this registration algorithm, a relaxation method utilizing
these covariances is formulated. In particular, we exploit the
plane-matcher properties that it provides an accurate rotation
estimate, and that it can easily identify principal translational
directions of uncertainty, to relax only the translation errors. This
results in a closed-form solution that can be computed in a very
fast manner, namely within a few milliseconds, as demonstrated
by experimental results in an indoor locmotion test arena in form
of a high bay rack.
FINA L VER SION:
IEEE International Conference on Advanced Robotics (ICAR),
2009
@inproceedings{PlaneSLAM-LoopClosing-ICAR09,
author = {Pathak, K. and Pfingsthorn, M. and
Vaskevicius, N. and Birk, A.},
title = {Relaxing loop-closing errors in
3D maps based on planar surface patches},
booktitle = {IEEE International Conference
on Advanced Robotics (ICAR)},
pages = {1-6},
year = {2009},
type = {Conference Proceedings}
}
I. INTRODUCTION
With the increasing availability of 3D range sensors, spatial
mapping is coming into its own. 3D sensors typically return
point-clouds, i.e. a dense sampling of range in several direc-
tions (∼104−106) in a single sensor-sample. Due to the
size of this data, many voxel-based mapping techniques like
occupancy-grids become inefficient with respect to storage,
computation, as well as visualization. The latter becomes
obvious on comparing Figs. 2 and 3 which show the result
of a matched pair of samples taken around the test arena as
shown in Fig. 1.
Existing work in point-cloud based mapping uses the ICP
algorithm for registration [1] followed by map refinement
using simultaneous multi-loop relaxation. There exist other
approaches which use planar surfaces, but again employ
ICP for registration [2], followed by EKF-SLAM for map
refinement using planes as features. Recently, the authors
(a) (b) (c)
Fig. 1. The robot collecting data in the locmotion test arena in form of a
high bay rack
Fig. 2. The point-cloud based map for two matched samples collected in the
multi-story arena shown in Fig. f:robotinlab. It corresponds to the planar-patch
map of Fig. 3.
presented an approach [3] wherein the scan-registration can
be done using planar-patches only, thus making the point-
cloud based ICP step unnecessary. For a review of additional
related literature, the reader is referred to the aforementioned
publication. The details of planar patch extraction from point-
clouds is presented in [4], [5]. Here, it is shown how the
approach can be extended to map-refinement using multi-loop
relaxation.
The idea of map-refinement using loop-closing has been in
use in 2D mapping for some time [6], [7], [8]. Computational
efficiency for very large 2D data-sets has been studied in
[9], [10]. Relaxation has been used by [1] for 3D point-
clouds based maps. The basic idea can be simply illustrated by
Fig. 3. A matching of two samples. The robot location is shown by a red
circle at the height of the sensor. Corresponding planes are drawn with the
same color and grayed-out planes were unmatched. Planar surface patches
are drawn semi-transparent to show internal detail. This map is much more
understandable compared to 2
Fig. 4 which shows the path taken by a robot, where adjacent
sampling positions have been joined by lines. The arrows show
non-adjacent samples which were also successfully matched
and registered by our planar patches based scan-matcher. The
whole sequence can be represented as a graph, called the pose-
graph, where each node represents a sample, and each edge,
the inter-node pose registration and its covariance computed
by the scan-matcher. A relaxation algorithm looks at the
global picture and tries to maximize the overall consistency
of the map by finding all the loops existing in this graph
using a breadth-first search, and minimizing a Mahalanobis
metric computed using the pose discrepancies along with their
uncertainties.
The most general formulation relaxes both rotation as well
as translation errors. However, even in relatively general
approaches formulated in 2D, some simplifications of the
rotational coupling are done to speed up the computation–
e.g. neglecting the contribution of the rotation matrix in the
computation of Jacobians [7], [10]. As shown in [3], plane-
matching can decouple rotation and translation determination.
Additionally, only two pairs of non-parallel corresponding
planes need to be found to determine the rotation registration
between two samples, whereas for translation, this number
is three. Typically, for a large FOV sensor, the number of
high-evidence corresponding plane pairs [3] found between
two samples is ∼10. Therefore, in general, the rotation is
much more accurately determined than translation in plane-
matching, especially in man-made environments with several
parallel planes. In this paper, we exploit the accuracy of
rotation determination to relax only the translation error. Since
this part is linear, a fast non-iterative closed-form least-squares
solution can be obtained. We experimentally validate the
accuracy of rotation determination in Table I, which justifies
relaxing only the translational part. A similar approach was
taken in 2D by [11], where ignoring rotation relaxation was
justified by assuming the availability of a global orientation
sensor, e.g. a compass. In our work, the justification is based
on the rotational accuracy of the plane-matcher.
This paper is structured as follows: In Sec. II, we provide a
mathematical formulation of the problem and its solution. The
plane-matching itself is described in [3] and is not discussed
in this article. Sec. III provides experimental results for a 3D
map created by plane-matching and subsequent relaxation for
an indoor multi-story robot rescue arena located at the Jacobs
University Bremen. Sec. III-A provides experimental evidence
for our claim of high accuracy of the rotation determination
by the plane-matcher. The cost-function reduction and im-
provement of map quality after relaxation are discussed in
Sec. III-B. The paper is concluded in Sec. IV.
II. PRO BLE M FORMULATION
We illustrate the problem by an experimental example
shown in Fig. 4. The robot takes 3D samples from several
locations i= 0 . . . around arena as shown in Fig. 1. Each
location defines a robot-attached frame Fiwith origin Oi. For
each pair of adjacent frames Fiand Fi+1, planes are extracted
from the point-clouds and matched [3], thus providing the pose
registration between these frames. These matches are written
i→i+ 1.
It is sometimes also possible to match certain non-adjacent
frames Fi,Fjsuch that |j−i|>1. These are called loop-
closing matches, and are written in reverse order as j→i,
where j > (i+ 1). The pose-registration computed by plane-
matching consists of a relative rotation R
j
iand translation
t
j
i≜−−−→
OjOi, resolved in frame Fj. Here, we have adopted the
left superscript/subscript notation of [?].
The result of pair-wise matching can be represented as a
pose-graph [10]. This graph consists of nodes Ni, i = 0 . . .,
and directed edges j
iE, i =j, also denoted as j→i. Each
node Nicorresponds to a sensor frame Fiwith origin Oi.
Each directed edge j
iEcontains the certain relative rotation
R
j
iand the uncertain relative translation t
j
i. An example is
shown in Fig. 4, where the loop-closing edges are shown by
a pair of arrows, and normal (adjacent nodes) edges by lines.
Proceeding as in [6], the translation t
j
ican be thought of
as a random vector with mean j
i¯
tand the inverse of its 3×3
covariance matrix being denoted as C
j−1
itt. This covariance
comes from the plane-matcher. If the scans corresponding
to two nodes are not matchable, we set the inverse of the
covariance C
j−1
itt ≡0.
To relax the graph we need to minimize the cost
min
t
j
iX
j=0 X
i=j+1 t
j
i−j
i¯
tT
C
j−1
ittt
j
i−j
i¯
t(1)
with the additional constraints on t
j
ifor all possible inde-
pendent loops containing it. As j
iEand i
jEcontain the same
information, only one is considered in the summation– which
is considered, depends on the direction the graph is traversed.
Since unconstrained optimization is easier to solve, we re-
formulate the problem by resolving all quantities with respect
Fig. 4. The XY path before relaxation with the five loop-closing match pairs
shown by arrows. The longest loop-closing edge (27 →0) is shown by red
arrows and the corresponding plane-matching result is shown in Fig. 3.
to the global frame— since the rotations are considered known
and certain, this is possible. The position of Niresolved in
global coordinates is denoted by g
it. Furthermore,
j
i¯
g≜R
g
j
j
i¯
t(2)
j
iΣ−1
tt ≜R
g
jC
j−1
itt R
g
j
T(3)
Then the previous cost function can be made unconstrained as
follows
min
g
⋆tX
j=0 X
i=j+1 g
it−g
jt−j
i¯
gTj
iΣ−1
tt g
it−g
jt−j
i¯
g
We further define
x≜
g
1t
.
.
.
gt
∈R3,(4)
and also,
j
iI≜03×3· · · −I303×3· · · I303×3· · ·(5)
In the above, I3is the identity matrix, −I3appears in the jth
3×3block of j
iI, and I3appears in the ith block. Note that
the size of j
iIis 3×3. The cost function can again be rewritten
as
min
xX
j=0 X
i=j+1 j
iI x −j
i¯
gTj
iΣ−1
tt j
iI x −j
i¯
g(6)
Differentiating the above by x, setting the resulting expression
to zero and solving for xgives
G x =b,where, (7)
G≜X
j=0 X
i=j+1
j
iITj
iΣ−1
tt
j
iI,(8)
b≜X
j=0 X
i=j+1
j
iITj
iΣ−1
tt
j
i¯
g.(9)
After solving for x, we can back-calculate
t
j
i=R
g
j
Tg
it−g
jt.(10)
III. EXP ERI ME NTAL RE SULTS
One of the Jacobs University robots is equipped with
an actuated laser range-finder (ALRF). The ALRF has a
horizontal field of view of 270◦of 541 beams. The sensor
is rotated (pitched) by a servo from −90◦to +90◦at a
spacing of 0.5◦. This gives a 3D point-cloud of a total size of
541 ×361 = 195301 per sample. The maximum range of the
sensor is about 20 meters. The mobile robot was teleoperated
and stopped occasionally to take scans. The time to take one
full scan is about Tscan ≈32 seconds. Every scan corresponds
to a 541 ×361 range-image. Planar patches were extracted
from these range images using a region-growing algorithm
described in [4]. For each planar surface, a covariance matrix
of plane parameters was computed, as described in [5]. A
total of 29 usable scans were taken as the robot was taken
around the multi-level arena, and their plane-matching results
are described in detail in [3]. Here, we will concentrate on the
loop-closing and relaxation aspects of the problem.
As shown in Fig. 4, the graph has 29 nodes with 28 normal
edges representing matching of adjacent scans. There are also
5loop-closing edges which match non-adjacent nodes; the
longest of these edges in terms of the in-between robot-
movement is 27 →0.
A. Rotational Errors
Fig. 5 shows the rotational pose of the robot computed by
plane-matching. To experimentally validate the claim that the
plane-matching leads to accurate rotation results, we compile
the rotation computations for the loop-closing edges in Table I.
As an example, for the loop 27 →0, the row marked “direct”
shows the result of of matching F27 with F0directly. The
row marked “Cumul.” shows the result of finding the relative
transform between F27 and F0by cumulatively computing
the relative rotation by traversing all the intermediate edges as
follows
R
0
27 =R
0
1R
1
2. . . R
25
26 R
26
27 (11)
We see that the maximum difference is ≈2◦, and the
Mahalanobis distance χ2based on the covariance matrix of
the direct transform is quite small. We have thus verified our
claim.
B. Cost Function Reduction After Relaxation
Fig. 6 shows the mechanism by which translational errors
may creep in due to insufficient number of matching planes
found in certain directions. The matching of F22 and F23 leads
to one principal direction of uncertainty which is reflected in
the long uncertainty ellipse shown in Fig. 7(a). The corrected
path of the robot after relaxation is shown in Fig. 7(b). The
corresponding 3D models are shown in Fig. 8, which show
the improvement in the overall map after relaxation. Further
multimedia relating to this data-set may be accessed at [12].
Fig. 9 shows the point-cloud based 3D map reconstructed
Fig. 5. Roll, pitch, yaw and their 200σbounds. Note the wrapping of yaw
at ±180.
Loop Type Roll Pitch Yaw χ2dist.
4→2Direct −1.383◦0.404◦20.315◦
Cumul. −2.189◦0.172◦21.414◦5.8 10−4
10 →8Direct −0.641◦0.011◦11.954◦
Cumul. −0.697◦−0.035◦12.086◦6.9 10−6
17 →15 Direct −0.683◦−0.602◦4.389◦
Cumul. −0.078◦−0.986◦−2.121◦1.8 10−3
27 →25 Direct −1.405◦−0.195◦26.265◦
Cumul. −1.054◦0.019◦26.614◦8.8 10−5
27 →0Direct 1.992◦1.693◦−46.092◦
Cumul. 2.916◦0.508◦−48.367◦2.3 10−3
TABLE I
ROTATIO N DETE RMIN ATION ER ROR INLOO PS
using the pose registrations found by plane-matching. This
map is clearly very hard to interpret without a 3D viewer and
illustrates again the advantage of surface based maps in terms
of both size and ease of visualization.
Table II summarizes the reduction in the cost function (6)
after relaxation with an increasing number of loops taken into
account. Since the covariance matrices in equation (6) need
to be in the global frame, the quality of the rotation to the
global frame and therefore the final covariance matrix itself
depends on the path taken through the pose-graph.Directed
and Undirected denote different breadth-first traversals used
to compute the rotated covariance matrices. During Directed
traversals, edges are always traversed in the direction they
were recorded. This means that only the path of there robot is
used to calculate the rotations for the covariance matrices, loop
edges are ignored because they only link to previous nodes
in the path. Undirected traversals use any available edge and
therefore the shortest path to compute the global rotations.
This of course leads to much more accurate global covariance
matrices, and thus, theoretically, to a better final result. The
other effect is, that the initial error will largely depend on the
covariance matrix of the edge where the tips of the breadth-
first traversal meet. This fluctuation of the initial error can be
seen in the lower half of Table II.
(a) Match for samples 21 and 22. This is a good match, with
matching pairs found in all directions.
(b) Match for samples 22 and 23. The arrow shows the
direction of uncertainty.
Fig. 6. An example of the source of translational uncertainty. The dashed
circle shows the patches which was not matched in the second sample due
to a sudden increase in its size beyond the threshold of size-similarity. This
causes no patches to be found perpendicular to the corridor direction, and
therefore, odometry was resorted to only for this direction, along with a high
uncertainty.
The runtimes reported in Table II were recorded on a
Core2Duo 2.8 GHz machine with 4GB RAM. All runtimes
are very similar, as a matrix with the same size has to be
inverted in each case.
For both Directed and Undirected traversal methods, an
increasing number of loops were added to show how the final
error is continuously reduced as more information is available.
Our method was able to correct 98.74% of the error in the
Undirected case. The remaining error is almost five times
bigger in the Directed case, which was able to correct 94.68%
of the error. The rightmost column called Loop shows the
loop added for that row. The error is reduced significantly
after adding the large loop between number 27 and 0. The
error drops significantly a second time with the introduction
of the loop between number 4 and 2. It is interesting to see
that even though the last two loops do not influence the final
result significantly, the overall error function still decreases
further.
(a) The estimated path before relaxation with 500σellipses. The uncertainty
ellipse at the node 23, shown by an arrow, is relatively much longer in one
direction, as explained in Fig. 6.
(b) Estimated robot path after relaxation. Note how the loop is closed much
better.
Fig. 7. The robot path before and after relaxation.
Directed
# Loops Initial χ2Final χ2Comp. Time Loop
0 99.378 10699.378 1063.52 ms
1 99.378 10628.048 1063.56 ms 27 →0
2 99.378 10620.993 1063.56 ms 17 →15
3 99.378 1065.4205 1063.58 ms 4→2
4 99.378 1065.4030 1063.63 ms 27 →25
5 99.378 1065.2918 1063.61 ms 10 →8
Undirected
# Loops Initial χ2Final χ2Comp. Time Loop
0 99.378 10699.378 1063.49 ms
1 1143.9 10621.925 1063.77 ms 27 →0
2 127.53 10613.193 1063.69 ms 17 →15
3 499.92 1061.3458 1063.68 ms 4→2
4 371.74 1061.3317 1063.78 ms 27 →25
5 1196.7 1061.2519 1063.76 ms 10 →8
TABLE II
DEV ELO PMEN T OF ER ROR E STI MATES W ITH D IFFER ENT N UMB ER OF
LO OPS A ND TRAVE RSA L MET HOD S. RUNTIMES ARE FROM A CORE2DUO
2.8 GHZ WITH 4GB RAM.
IV. CONCLUSIONS
A pose-graph based relaxation method was presented for
a 3D map comprised of large planar surface patches. The
method derives its computational speed from the fact that
only the translational error needs to be relaxed because the
plane-matcher is able to decouple rotation and translation
determination and the rotation determination is quite accurate.
These claims were verified in an indoor scenario for a multi-
story robot rescue arena.
V. AC KN OWL EDGME NTS
This work was supported by the German Research Founda-
tion (DFG).
REFERENCES
[1] D. Borrmann, J. Elseberg, K. Lingemann, A. N¨
uchter, and J. Hertzberg,
“Globally consistent 3d mapping with scan matching,” Robotics and
Autonomous Systems, vol. 56, no. 2, pp. 130–142, 2008.
[2] J. Weingarten and R. Siegwart, “3D SLAM using planar segments,” in
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), Beijing, 2006.
[3] K. Pathak, N. Vaskevicius, J. Poppinga, M. Pfingsthorn, S. Schwertfeger,
and A. Birk, “Fast 3D mapping by matching planes extracted from
range sensor point-clouds,” (submitted), 2009. [Online]. Available:
http://robotics.jacobs-university.de/datasets/Lab3D-2009/
[4] J. Poppinga, N. Vaskevicius, A. Birk, and K. Pathak, “Fast plane
detection and polygonalization in noisy 3D range images,” in IEEE Int.
Conf. on Intelligent Robots and Systems (IROS), Nice, France, 2008.
[5] K. Pathak, N. Vaskevicius, and A. Birk, “Revisiting uncertainty analysis
for optimum planes extracted from 3D range sensor point-clouds,” in
IEEE Int. Conf. on Robotics and Automation, Kobe, Japan, 2009.
[6] F. Lu and E. Milios, “Globally consistent range scan alignment for
environment mapping,” Autonomous Robots, vol. 4, no. 4, pp. 333–349,
1997.
[7] E. Olson, J. Leonard, and S. Teller, “Fast iterative alignment of pose
graphs with poor initial estimates,” Robotics and Automation, 2006.
ICRA 2006. Proceedings 2006 IEEE International Conference on, pp.
2262–2269, May 2006.
[8] M. Golfarelli, D. Maio, and S. Rizzi, “Correction of dead-reckoning
errors in map building for mobile robots,” Robotics and Automation,
IEEE Transactions on, vol. 17, no. 1, pp. 37–47, Feb 2001.
[9] U. Frese, P. Larsson, and T. Duckett, “A multilevel relaxation algorithm
for simultaneous localization and mapping,” Robotics, IEEE Transac-
tions on, vol. 21, no. 2, pp. 196–207, April 2005.
[10] G. Grisetti, C. Stachniss, S. Grzonka, and W. Burgard, “A tree param-
eterization for efficiently computing maximum likelihood maps using
gradient descent,” in Proceedings of Robotics: Science and Systems,
Atlanta, GA, USA, June 2007.
[11] T. Duckett, S. Marsland, and J. Shapiro, “Fast on-line learning of
globally consistenst maps,” Autonomous Robots, vol. 12, no. 3, pp. 287–
300, 2002.
[12] K. Pathak and A. Birk, “3D map of Jacobs University locmotion
test arena in form of a high bay rack,” 2009. [Online]. Available:
http://robotics.jacobs-university.de/datasets/Lab3D-2009/
(a) Before relaxation: top view. (b) Before relaxation: side view
(c) After relaxation: top view. (d) After relaxation: side view
Fig. 8. The lab model created by plane-matching before and after relaxation. The side view shows the good alignment of the wall with the windows after
the relaxation. The robot starts at the left bottom diagonal corner and went clockwise around the arena as shown in Fig. 7. The left wall in Fig. 8(a) and
8(b), shows some translation offset where the loop was closed , but almost no rotational misalignment. The translation misalignment is then corrected by
loop-relaxation. An X3D model and video files for this dataset can be found at [12].
Fig. 9. Point-cloud map based on the registration computed by plane-matching
followed by relaxation. Such a map can show more details, but is difficult to
interpret without a 3D viewer.
