The 2009 IEEE/RSJ International Conference on

Intelligent Robots and Systems

October 11-15, 2009 St. Louis, USA

Fast 3D Mapping by Matching Planes Extracted from

Range Sensor Point-Clouds
Kaustubh Pathak, Narunas Vaskevicius, Jann Poppinga, Max Pfingsthorn, Sören Schwertfeger, Andreas Birk
Dept. of Computer Science, Jacobs University Bremen, 28759 Bremen, Germany
[email protected], [email protected]

Abstract—This article addresses fast 3D mapping by a mobile robot 1.6 GHz computer is much less than the typical time of 30 sec.
in a predominantly planar environment. It is based on a novel pose needed to collect one scan.
registration algorithm based entirely on matching features composed of
2) For small FOV (∼ 50◦ × 50◦ ) TOF sensors, the pose-
plane-segments extracted from point-clouds sampled from a 3D sensor.
The approach has advantages in terms of robustness, speed and storage registration time is typically less than 0.1 sec.
as compared to the voxel based approaches. Unlike previous approaches, In this article, we focus only on the steps marked with a box in
the uncertainty in plane parameters is utilized to compute the uncertainty Fig. 1. The well known EKF-SLAM step can be the same as in [5]
in the pose computed by scan-registration. The algorithm is illustrated
and is outside the scope of this article. A plane extraction step is also
by creating a full 3D model of a multi-level robot testing arena.
common in both approaches and it will not be discussed in detail in
this article. The reader is referred to the authors’ previous work [16],
I. I NTRODUCTION [17] for details.
The work by Kohlhepp et al [2], [19], [13] is also closely related
Encouraged by the success of 2D simultaneous localization and
to ours. In [19], surfaces are extracted from range images obtained by
mapping (SLAM) [1], there have been many recent attempts (ref.
a rotating laser range finder (LRF) and registered together. A local
[2], [3], [4], [5], [6], [7]) to extend the methodology to 3D. Since
module determines the correspondences and computes transforms,
onboard 3D odometry is usually lacking or inadequate, an essential
and a global module detects loop-closure and distributes uncertainty
part of the mapping procedure is finding the relative pose offset
using an elastic graph. For this article, only the local module is
of the robot between two successive range sensor samples using
relevant which is discussed again in [13]. Similar to Sec. II-D1 in
scan-matching. Typical 3D sensors can be categorized as: 1) large
this article, their approach uses feature-directions to compute rotation
field-of-view (FOV) laser range finders (LRF) mounted on a rotating
between successive views. However, no mention is made in their
platforms [3] having a large scanning time of around a minute, and 2)
work of using the uncertainties in the extracted feature-parameters.
time-of-flight (TOF) sensors like the Swiss-ranger [8] and PMD [9]
For estimating translation, they resort back to point features, which
with a much restricted FOV, but being able to provide several scans
is essentially the same as ICP. By contrast, this article uses only
per second. These sensors provide a 3D range scan as a point-cloud
plane-features and does not return to the domain of point-features –
of noisy spatial coordinates, numbering typically between 104 and
even translation is obtained using planes. This allows for detection of
106 .
dominant directions of uncertainties, as detailed in Sec. II-D2. In [13],
The scan-matching algorithms can be broadly classified as 1) point-
many heuristic measures for estimating correspondences between
based, e.g. iterative closest point (ICP) [10], [7], or voxel-based 3D-
features, in particular the ground, across views are discussed. By
NDT [6], and 2) feature-based— where the most popular geometric
contrast, the plane correspondence algorithm presented by us does
features are surfaces represented as planar-patches [11], [12], [5],
not give any special status to the ground.
[13], [14]. Although point/voxel based methods do not assume any The feature-correspondence finding problem has been addressed
structure in the environments, they are computationally expensive in the past using graph-theoretic techniques [20], [21]. However, the
and converge to local minima when the scene overlap is not high, latter work offered scant experimental data in form of only one pair
and sensitivity to outliers [15]. Maps created by these methods are of scans matched and no statistical measures for the variance of
neither easily visualized nor are particularly suitable for 3D path- registration estimation. Another such approach is found in [2], [19],
planning. Planar patches offer good visualization and an impressive [13]. where two sets of planar or quadratic patches are matched using
compression of data (typically 95%) as compared to point-clouds. attribute-graphs . In that work, similarity metrics were formulated
As shown in this paper, the feature-correspondence problem can based on several attributes like shape-factor, area ratio, curvature-
also be solved faster and more robustly due to this compression. histogram, inter-surface relations etc., and a bounded tree search was
As in [5], the approach in our article is also based on plane-features. performed to give a set of correspondences which maximized the
However, our approach obviates the ICP step necessary for the pose metric. The result is refined using an evolutionary algorithm, which
change prediction in their work. The basic steps of the algorithms are is computation-time intensive. An ICP-like algorithm working with
compared in Fig. 1. Combining plane-correspondence determination planar-patch normals instead of points was presented in [15]. Planes
and pose registration in one step and removal of ICP leads to savings were matched based on proximity in terms of inter-normal angle
in computation time and an increase in robustness. In fact, most of and Euclidean distance but the overall consistency of the scene was
the previous 3D mapping approaches have been off-line due to the not considered. Again, no statistical measures of the variance of the
excessive time required by ICP. With the new approach presented in solution were provided.
this article, the pose registration can be computed “fast.” This claim In contrast to the aforementioned approaches, the algorithm pre-
can be made more precise as follows: sented in this paper use only planar patches and maximizes the
1) For large FOV (∼ 270◦ × 180◦ ) actuated LRFs, the pose- overall geometric consistency within a search-space to determine
registration time of 4 − 10 sec. with planes on an AMD Turion correspondences between planes. The search-space is pruned using

978-1-4244-3804-4/09/$25.00 ©2009 IEEE 1150

 Initialization Initialization
Initialization
Point Cloud Sample Point Cloud Sample
Point Cloud Sample
ICP based Pose-Prediction Plane Features Extraction
Odometry Extrapolation
Plane Features Extraction Planes Correspondences Determination &
Scan Registration using ICP Pose Prediction using Planes Registration
Feature-Association / Plane-Pairing

Loop Closure Detection EKF SLAM Update for Pose / Features SLAM Update

Relaxation, and Model Refinement Adding Unpaired Features to Map Adding Unpaired Features to Map
(a) The approach of Nüchter et al [7]. (b) The approach of Weingarten et al [18]. (c) The approach presented in this article.

Fig. 1. Comparison of algorithm structures. The ICP step in Weingarten et al’s approach has been removed in our approach.

criteria such as overlap, size-similarity, and agreement with odometry, plane normal m̂. We see that P (m̂, ρ) , P (−m̂, −ρ). To achieve
if available. For all these tests, only the plane parameter covariance a consistent sign convention, we define planes as P (n̂, d), where,
matrix is employed, without the need to refer back to the original d , |ρ| ≥ 0, and n̂ , σ(ρ) m̂, where, σ(ρ) = −1 if ρ < 0 and +1
point-cloud. Our approach is fast and its reliability as well as otherwise. If ρ = 0, then we choose the maximum component of n̂
computation-time increases with the number of planes. There is the to be positive. The latter case is unlikely to occur in practice in the
option of using additional attributes like intensity, color, etc. for sensor-frame, because such a plane, which is parallel to the line of
making the correspondence-finding step more reliable. Finally, the sight of the range sensor, is unlikely to be detected by it.
covariance matrix of the solution is computed which identifies the For denoting frames and relative transforms, we use the notation
principal uncertainty directions. This information is indispensable for of [22]. The j-th sample is associated with a frame Fj , in which the
subsequent refinement processing like loop-closing or EKF-SLAM, set of extracted planes is denoted as j P. Suppose we are given two
although these are outside the scope of this paper. samples j P and k P. Usually they are successive, but they may be
This paper is organized as follows: Sec. II begins by presenting non-successive, for example during loop-closing. Using the procedure
the basic notation and problem statement. Sec. II-C presents a described in [17], one can compute an isotropic uncertainty measure
new algorithm for finding plane correspondences. Sec. II-D solves σ 2 which is derived (usually, using the trace operation) from the 4×4
the least-squares determination of pose-registration using the found covariance matrix C of the plane parameters n̂ and d associated with
correspondences. Sec. III shows a real-life example of mapping. The it. Thus a plane-set k P is an ordered set of triplets given by
paper is concluded in Sec. IV. k
P , { k Pi h k n̂i , k di , k σi2 i, i = 1 . . . Nk }, (1)
II. P LANE -S EGMENT E XTRACTION AND M ATCHING
B. Decoupling of Rotation and Translation
The scan-matching based on plane-segments consists of the fol-
lowing three steps: If the robot (more precisely, the sensor mounted on the robot)
1) Planes extraction from raw point-clouds: This procedure is moves from Fj to Fk , i.e. rotates by jk R and translates by jk t
based on region-growing in a range-image scan followed by between samples j and k (resolved in Fj ), then the Cartesian
a least-squares estimation of the parameters of planes. The coordinates j p and k p of the same physical point observed from
covariances of the plane-parameters are computed as well. The the two frames are related by
j
details may be found in the previously published work of the p = jk R k p + jk t. (2)
authors [16].
2) Pose-registration by plane-matching: This step consists of Substituting the above in the plane equation, one can derive that if
j
two substeps: Pi and k Pi represent the same physical plane, the plane parameters
a) Finding the correspondences between plane-segments in observed in the two planes are related by
j
the two scans to be matched. These two scans may be n̂i = jk R k n̂i (3)
successive samples for normal registration or may be non- j
n̂Ti jk t j
= di − di k
(4)
successive, if a loop is being closed.
b) After the correspondences have been decided on, finding The above equations show that the rotation and the translation
the optimal rotation and translation which aligns the components have been nicely decoupled.
corresponding set of planes. This gives the pose change
of the robot between the scans. C. Determining Correspondences
3) Polygonization: This step consists of polygonizing each plane- Given two plane-sets j P of size Nj and k P of size Nk , the
segment by finding the boundary of each surface-patch so that problem here is to find which plane indices correspond to each
the surface can be compactly described. This step is crucial for other, i.e. which represent the same physical plane. The overlap
visualization of the result, however, if only pose registration is is not given, and so some planes in one set may not have any
desired, it may be omitted. It is also described in [16]. corresponding plane in the other set. We present a new matching
We now provide an overview of plane-matching, i.e. the second step. algorithm called Plane Registration based on Rotation of a Unit
Sphere (PRRUS). The algorithm scales as O(N4 ), where N is the
A. Notation average of Nj and Nk . Unlike RANSAC [14], this algorithm has
A plane P (m̂, ρ) is given by the equation m̂ · r = ρ, where ρ no random component and relies on two observations 1) the number
is the signed distance from the origin in the direction of the unit of high-evidence (detected using their parameter covariance matrix)

1151
 R2(φ)
This is the well-known Wahba’s problem [23]. To solve this we
R1 k n̂ k n̂ j parameterize the rotation jk R with quaternions jk q̌ and proceed as
ok ok ≡ R1 n̂oj
in [24]. Using the above, one can reformulate (11) as
j n̂
oj
θ 1j T j
max ζr , q̌ K k q̌, (12)
j
k
q̌ 2k

where the matrix K is depends on j n̂i and k n̂i , i = 1 . . . N. The

R1 j n̂ij
k n̂
ik optimum quaternion jk q̌ is then the eigenvector of the matrix K
corresponding to its maximum eigenvalue µ̄(K). The 4×4 covariance
kR = R R
j 2 1 of this optimum jk q̌ can be computed as
j
Fig. 2. Finding correspondences: steps 1 and 2. k C q̌q̌ = − (K − µ̄(K) I4 )+ , (13)

where, X+ represents the Moore-Penrose inverse of the matrix X

planes in a scene, are much less than the number of points, and 2) [25]. This covariance matrix can then be transformed to roll-pitch-
When one pair of corresponding planes is found, only one degree of yaw (RPY) space by using the Jacobian of the transform between
freedom is left for finding the rest of the correspondences. the quaternion and the RPY space. Finally, we note that at least two
PRRUS finds the normal correspondences which give the most non-parallel pairs of planes are required to fully determine rotation.
geometric consistency, i.e. maximizes the satisfaction of (3). As 2) Optimum Translation: Equations (4) can be stacked-up to give
illustrated in Fig. 2, it divides the problem in two steps:
a) Step 1: Find the pair of planes (o∗j , o∗k ) which maximizes M jk t = d, (14a)
the size of the set Ω1 of plane-pairs with the same relative angles to where,
them. In other words, 2j j
n̂T1 d1 − k d1
3 2 3
(o∗j , o∗k ) , max #Ω0 (oj , ok ), (5)
MN×3 , 4 ... 5 , dN×1 ,4 .. (14b)
6 7 6 7
(oj ,ok )
. 5
j j k k j T j k
Ω0 (oj , ok ) , {(ij , ik ) | n̂oj · n̂ij ≈ n̂ok · n̂ik } (6) n̂N dN − dN
2 Due to its intuitive nature and fast closed-form solution, we will solve
The approximate equality-test uses the χ test since the covariances
of the normals is known. In (6), we also remove planes, where the Eq. (14a) with ordinary least squares (LS). A diagonal weighting
dot-product is close to ±1, i.e. planes parallel or anti-parallel to matrix W is defined as
j
n̂oj and k n̂ok . Furthermore, we apply additional checks within the 0j 2 k 2
σ1 + σ1 0
1
maximization step to test for overlap and size-similarity, and prune ..
Σ,@ A, (15)
B C
out pairs in Ω0 (oj , ok ) which fail these checks. This first step takes .
the worst case time of O(N2j N2k ). 0 j 2 k 2
σN + σN
After this step, we compute the rotation matrix R1 , such that ` −1 ´1/2
W, Σ . (16)
R1 j n̂o∗j = k n̂o∗k . (7)
Then the LS solution minimizes kW(M jk t − d)k. If M
We then compute the consensus set of triplets Ω1 , is full rank, the least-squares optimum translation is jk t =
` T 2 ´−1 T 2
M W M M W d.
Ω1 , {(ij , ik , φi ) | (ij , ik ) ∈ Ω0 (o∗j , o∗k )} (8) Unlike rotation, in general we need N ≥ 3 mutually non-
j k k
φi , ∡(R1 n̂ij , n̂ik ) about n̂o∗k . (9) parallel planes to find jk t. The above formula is not a good way
to compute the solution because M may be ill-conditioned, may be
b) Step 2: Now it remains to find the most consistent φ. Find rank-deficient, or N < 3. A more general way to solve the equation
the biggest cluster in the distribution of φi within Ω1 ; call the mean is presented next. We define
of the cluster φ∗ . This gives us the final consensus set
M̂ , WM, d̂ , Wd. (17)
Ω2 , {(ij , ik ) | (ij , ik , φ∗ ) ∈ Ω1 } (10)
Let the singular-value decomposition of M̂ be given by
The set Ω2 is our set of resolved correspondences, to which a least-
UN×N ΛN×3 V3×3 T
. Λ has non-negative singular values λ2i arranged
squares rotation and translation is fitted next.
in descending order. The column unit vectors of U are denoted
D. Pose-registration by plane-matching ui , i = 1 . . . N and the column unit vectors of V are denoted
vi , i = 1 . . . 3.
Statement of the Problem Given noisy plane-sets j P and k P,
Let NM̂ ≤ 3 be the effective rank of M̂. If the largest singular value
with correspondences between planes known and given by the
λ21 < ǫ1 , then the effective rank is 0. The parameter ǫ1 is dependent
common index i, find the optimum rotation jk R and the optimum
on machine accuracy. In our computations, we have taken it as 10−7 .
translation jk t. We provide a solution below.
If λ21 ≥ ǫ1 , then the effective rank is found by finding the count of
1) Optimum Rotation: To find the optimum rotation between j P all singular values λ2i > λ21 /c̄ in the diagonal matrix Λ, where c̄ is
and k P, we maximize the following value-function the maximum allowable condition number of the matrix. We have
N set c̄ ≈ 200. In practice ǫ1 and c̄ are quite important parameters
1 X j 2 k 2 −1 j T j k
max ζr , ( σi + σi ) n̂i k R n̂i (11) for obtaining good translation estimates and also for identifying the
j
R 2 i=1
k directions in which the translation estimate is the most uncertain.

1152
 Then the best rank NM̂ approximation of M̂ is
N
M̂
X
M̃ = λ2i ûi v̂iT , NM̂ ≤ 3. (18)
i=1

The span of the orthogonal unit vectors ûi , i = 1 . . . NM̂ gives

the best approximation for the range-space of M̂. Therefore, the
PNM̂
closest we can get to d̂ is d̃ = i=1 (ûi · d̂)ûi , which gives the
corresponding translation estimate
N
M̂
j
X
kt = λ−2
i (ûi · d̂)v̂i (19)
i=1
N
M̂
+ +
X T
, M̂ d̂, M̂ , λ−2
i v̂i ûi . (20) Fig. 4. The 541 × 361 range-image corresponding to the first scan, with the
i=1 planar regions identified. This image actually lies on the surface of a sphere,
and displaying it as a planar rectangle leads to an obvious distortion.
This is also the minimum 2-norm solution of the LS problem
regardless of the rank of M mentioned in [26].
Note that for directions v̂i , i = (NM̂ + 1) . . . 3, we have The Jacobs University rescue robot is equipped with an actuated
no information about the translation. One option is to keep these laser range-finder (ALRF). The ALRF has a horizontal field of view
components 0 and inject large uncertainty along those directions in of 270◦ of 541 beams. The sensor is rotated (pitched) by a servo from
the covariance matrix. However, if an odometry estimate jk tO , along −90◦ to +90◦ at a spacing of 0.5◦ . This gives a 3D point-cloud of a
with its covariance matrix jk C tt,O is available, we can use it only total size of 541 × 361 = 195301 per sample. The maximum range
for these missing components. In this case we have of the sensor is about 20 meters. The mobile robot was teleoperated
3
X and stopped occasionally to take scans. The time to take one full scan
j
kt = M̂+ d + ( jk t · v̂i )v̂i , is about Tscan ≈ 32 seconds. Every scan corresponds to a 541 × 361
i=NM̂ +1 range-image as shown in Fig. 4. Planar patches were extracted from
3
X these range images using a region-growing algorithm described in
, M̂+ d + MO jk tO , MO , v̂i v̂iT . (21) [16]. For each planar surface, a covariance matrix of plane parameters
i=NM̂ +1 was computed, as described in [17]. A total of 29 usable scans were
The covariance of odometry is only very roughly known because taken as the robot was driven around the multi-level arena shown in
it depends on vehicle model, unknown slippage etc., and a usual Fig. 3. The front camera images of some of these locations are shown
fusion of odometry translation and plane-matching translation using in Fig. 5.
this covariance should be avoided. The nice thing about this solution The result of the plane-matching is shown in Fig. 6 and a view
is that it automatically detects the directions for which the translation of this map from inside the arena is shown in Fig. 6(d). This map
is uncertain, and resorts to odometry only for these directions. is clearly well aligned and is easy to understand by a human. The
Needless to say, if such a case occurs, i.e. the translation is not fully rotation and translation trajectory of the robot computed by plane-
determined, the uncertainty in translation increases, which needs to matching is shown in Fig. 7. This figure also shows the nσ bounds
be then mitigated by loop-closing. Finally, we can write the estimate of the uncertainties in the pose variables, where n = 100, 200
of the covariance matrix for translation as follows respectively. These bounds, like in any other scan-matcher, assume
that all the found plane correspondence are correct, and therefore
j
k C tt = M̂+ WΣWT (M̂+ )T + MO jk C tt,O MTO , tend to be optimistic.
= M+ Σ (M+ )T + MO jk C tt,O MTO (22)
where the last equation comes from simplification using (16) and
(17).
III. M ULTI -L EVEL L AB A RENA E XAMPLE

Fig. 5. The front camera view of the scene at the locations where the robot
took a scan. Only every fourth scan is shown to save space. The full video
may be accessed at [27].

The run-times for the various steps described in Sec. II are shown
in Table I. These are for an AMD Turion 2 × 64 machine 1.6 GHz
with 960 MB RAM running OpenSUSE 10.3 O/S. Compared to the
previously mentioned total time for taking one scan, the registration
(a) (b) (c)
method is indeed “fast”: mean time for extraction and plane-matching
Fig. 3. The robot collecting data in the locmotion test arena in form of a together ≈ 0.23 Tscan . We reiterate that the polygonalization step is
high bay rack needed mainly for visualization. Fig. 8 shows the dependence of the

1153
1154
 [5] J. Weingarten and R. Siegwart, “3D SLAM using planar segments,” in
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), Beijing, 2006.
[6] M. Magnusson, A. Lilienthal, and T. Duckett, “Scan registration for
autonomous mining vehicles using 3D-NDT,” Journal of Field Robotics,
vol. 24, no. 10, pp. 803–827, 2007.
[7] A. Nüchter, K. Lingemann, and J. Hertzberg, “6D SLAM– 3D mapping
outdoor environments,” Journal of Field Robotics, vol. 24, no. 8/9, pp.
699–722, 2007.
[8] CSEM, The SwissRanger, Manual V1.02, CSEM SA, 8048 Zurich,
Switzerland, 2006. [Online]. Available: http://www.swissranger.ch
[9] PhotonIC (R) PMD 3k-S, PMD Technologies, 2008. [Online]. Available:
http://www.pmdtec.com
[10] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,”
IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. 14, no. 2,
pp. 239–256, Feb 1992.
[11] O. D. Faugeras and F. Lustman, “Motion and structure from motion in
piecewise planar environment,” Int. Journal of Pattern Recognition and
Artificial Intelligence, vol. 2, no. 3, pp. 485–508, 1988.
[12] R. Fisher, “Geometric constraints from planar surface patch matching,”
Image and Vision Computing, vol. 8, no. 2, pp. 148–154, 1990.
[13] P. Kohlhepp, G. Bretthauer, M. Walther, and R. Dillmann, “Using
(a) XYZ plot with 100σ ellipsoids. Note how the major axes of ellisoids orthogonal surface directions for autonomous 3d-exploration of indoor
gradually become parallel to corridor directions. environments,” in IEEE/RSJ International Conference on Intelligent
Robots and Systems, Oct. 2006, pp. 3086–3092.
[14] D. Fontanneli, L. Ricciato, and S. Soatto, “A fast RANSAC-based
registration algorithm for accurate localization in unknown environments
using LIDAR measurements,” in IEEE Int. Conf. on Automation Science
and Engineering (CASE), 2007, pp. 597–602.
[15] D. Viejo and M. Cazorla, “3D plane-based egomotion for SLAM on
semi-structured environment,” in IEEE/RSJ Int. Conf. on Intelligent
Robots and Systems (IROS), 2007, pp. 2761–2766.
[16] 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.
[17] 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.
[18] J. Weingarten, “Feature-based 3D SLAM,” Ph.D. dissertation, EPFL,
Lausanne, Switzerland, 2006. [Online]. Available: http://library.epfl.ch/
theses/?nr=3601
[19] P. Kohlhepp, P. Pozzo, M. Walther, and R. Dillmann, “Sequential 3D-
SLAM for mobile action planning,” Intelligent Robots and Systems,
2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Confer-
(b) Roll, pitch, yaw and their 200σ bounds. Note the wrapping of yaw ence on, vol. 1, pp. 722–729 vol.1, Sept.-2 Oct. 2004.
at ±180. [20] W. E. L. Grimson, Object recognition by computer: the role of geometric
constraints. Cambridge: MIT Press, 1990.
Fig. 7. Computed robot trajectory and uncertainty. [21] W. He, W. Ma, and H. Zha, “Automatic registration of range images
based on correspondence of complete plane patches,” in Fifth Int. Conf.
on 3-D Digital Imaging and Modeling (3DIM’05). IEEE, 2005, pp.
470–475.
20 [22] J. J. Craig, Introduction to robotics – Mechanics and control. Prentice
Hall, 2005.
18 [23] M. D. Shuster, “The generalized Wahba problem,” The Journal of the
Astronautical Sciences, vol. 54, no. 2, pp. 245–259, April-June 2006.
16
[24] B. K. P. Horn, “Closed-form solution of absolute orientation using unit
14 quaternions,” Journal of the Optical Society of America, vol. 4, no. 4,
pp. 629–642, 1987.
12
Time [sec.]

[25] K. Kanatani, Statistical Optimization for Geometric Computation. New

10 York: Dover Publications, Inc., 2005.
[26] G. H. Golub and C. F. V. Loan, “An analysis of the total least squares
8 problem,” SIAM Journal of Numerical Analysis, vol. 17, no. 6, pp. 883–
6 893, 1980.
[27] K. Pathak and A. Birk, “3D map of Jacobs University locmotion
4 test arena in form of a high bay rack,” 2009. [Online]. Available:
http://robotics.jacobs-university.de/datasets/Lab3D-2009/
2
[28] J. Poppinga, N. Vaskevicius, K. Pathak, and A. Birk, “Plane matching
0 using Swiss-Ranger and comparison to ICP,” 2009. [Online]. Available:
8 10 12 14 16 18 20 22 24 26
Average nr. of planes in scan−pair
http://robotics.jacobs-university.de/datasets/SRLab

Fig. 8. The dependence on the computation time of the plane-matching

algorithm on the average number of planes in the scan-pair after filtering out
low evidence planes.

1155