www.lr.tudelft.nl
Transcription
www.lr.tudelft.nl
2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland Automated Localization of a Laser Scanner in Indoor Environments Using Planar Objects Kourosh Khoshelham Optical and Laser Remote Sensing, Dept. of Remote Sensing, Delft University of Technology, Delft, The Netherlands. Email: [email protected] Abstract— A method is presented for automated localization of a laser scanner in indoor environments by matching planar features extracted from range data. Plane correspondences are used in a linear least-squares adjustment model to estimate the relative scanner positions in consecutive scans. The performance of the method is demonstrated using two datasets of building interiors. Accuracy assessment of the computed positions shows localization errors of a few centimeters. Keywords—Navigation; plane matching; search; registration. I. INTRODUCTION A new trend in terrestrial laser scanning is the development of an autonomous system that is able to scan an indoor environment from a number of predefined positions, register the scans, and provide an accurate and complete point cloud of the scene. Such a system would require the automation of two main processes: scanner localization and the registration of the scans. In theory, automated registration precedes the localization problem, because a correct registration of consecutive scans provides the relative position and orientation of the scanner in every pair of scans. In practice, however, existing registration methods are slow and iterative, and require an approximate estimate of the transformation between the two scans. Alternatively, if the motion of the scanner from one scan position to another can be estimated from the data, it can serve as an initial approximation to perform an iterative fine registration of the scans, which will in turn lead to an improved accuracy for the localization of the scanner. The registration of laser scanner data has been a topic of intensive research in the past. Salvi et al., [1] provide a review of the more recent methods. Perhaps the most common approach is the so-called iterative closest point (ICP) algorithm [2]. The ICP algorithm uses an iterative estimation model to find a transformation that minimizes the sum of the squared distances between a set of corresponding points in the two scans. To establish correspondence between the points the ICP algorithm requires a good approximation of the transformation between the two scans. The concept of point-to-point correspondences was employed in a number of other methods [3-6], which all rely on the availability of an initial approximation. Another type of correspondence was proposed by Chen and Medioni [7], who developed an estimation model based on minimizing point-to-plane distances. The point-to-plane correspondences lead to a more robust estimation model, which converges in fewer 978-1-4244-5864-6/10/$26.00 ©2010 IEEE iterations [1]. However, this method also requires an initial approximation of the transformation. The problem of initial approximation can be dealt with in different ways. A straightforward solution is to directly measure the scanner’s position and orientation using GPS and navigation sensors [8]. This solution is applicable only in outdoor scenes where GPS signals can be received. A more general approach would be to estimate the motion between a pair of consecutive scans using the properties of the scanned surface itself. Chung et al., [9] used principal component analysis to find the relative orientation between two overlapping scans. This coarseregistration approach is likely to fail when complete 360degree scans are to be registered. Brenner and Dold [10] proposed the use of plane correspondences for coarse registration, and investigated the combinatorial complexity of the search for corresponding planes. In a previous work, we proposed a method to estimate the relative position of the scanner by intersecting corresponding planes extracted from two overlapping range images [11]. The relative positions make up the trajectory of the scanner, but are insufficient for a full registration of the scans. This paper extends the plane-based localization method to enable a complete pair-wise registration of overlapping scans. This is done by inferring a transformation from a set of plane correspondences within a linear least-squares plane matching model. The benefit of a linear estimation model is that it requires no initial approximations, and leads to a more efficient search for correspondences. The correspondence problem is approached with an initiateand-extend search strategy, which begins with initial correspondence hypotheses and extends the correspondences to all plane pairs that can contribute to an accurate estimation of the transformation. The paper proceeds with a description of the plane intersection and the extended plane matching model in Section 2. Section 3 presents the initiate-and-extend strategy for finding the largest set of corresponding planes. Experiments and results are presented in Section 4. The paper concludes with some remarks in Section 5. II. LOCALIZATION AND REGISTRATION USING PLANE CORRESPONDENCES The basic principle of plane-based localization and registration is that a minimum of three plane correspondences sufficiently constrain the motion in the 3D space, subject to the condition that the planes have an intersection point. In the following, first the method for motion estimation by intersecting the planes is described. Then the method is extended to a more general plane 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland matching model, which forms the basis for inferring a complete transformation from two sets of planes. A. The Plane Intersection Method Suppose a minimum of three planes that intersect in a point appear in two consecutive scans. The motion between the two scans can be computed from the differences in the measured ranges to the planes. Figure 1 illustrates this concept. Distances to three planes are measured from the scanner positions S1 and S2. If planes P1, P2, and P3 in the coordinate system of S1 are shifted by -ρ12, -ρ22, and –ρ32 respectively in the direction of their normals, they intersect at point S2. Let a plane be defined with a normal vector n = (n1, n2, n3)T and a perpendicular distance ρ to the origin of the coordinate system. The condition that the plane contains a point x is described by ρ = nTx. Now, the intersection of the three shifted planes can be expressed as: ⎧ ρ 1 − ρ 2 = n 1T s 1 1 1 2 ⎪ 1 ⎪ 1 2 1T 1 − = ρ ρ n s ⎨ 2 2 2 2 ⎪ 1 2 1T 1 ρ ρ − = n s ⎪⎩ 3 3 3 2 where x = (x1, x2, x3, 1)T is the homogenous representation of a point. The transformation of a set of m points from the coordinate system of a second scan to that of a first scan is expressed as: 2 x14 xm = H 4x 4 x 4x m where superscripts denote the coordinate system, and subscripts show the size of the matrices. In principle, H can be a similarity transformation: ⎡ sR H = ⎢ T3x 3 ⎣ 01x 3 t 3x1 ⎤ 1 ⎥⎦ π1j = H − T π 2j π1 x1 = ( H − T π 2 ) Hx 2 = π 2 H −1Hx 2 = π 2 x 2 = 0 T T T B. Extension to Plane Matching To estimate a complete transformation from a set of corresponding planes all the plane parameters should be used in the adjustment. Let π = (nT, –ρ)T denote a plane, and the point-on-plane condition be expressed as: πTx = 0 (2) T (6) Equation (5) can be written as: T This plane intersection solution does not provide any information about the relative rotation between the two scans. The rotation parameters can be derived from the directions of the plane normals, which are absent in the system of equations (1). (5) because the points-on-plane condition is invariant to the transformation, i.e. : π1j H = π 2j Figure 1. Scanner localization by plane intersection. (4) consisting of a scale s (normally equal to 1), a 3D rotation R and a translation vector t. The corresponding transformation for transforming the plane j that contains the points from the second scan to the first is: (1) where n and ρ are plane parameters, subscripts denote plane numbers, and superscripts denote the coordinate system in which the plane parameters are defined. Solving the system of equations given in (1) for s21 gives the location of s2 in the coordinate system of s1, which is the motion between the two scans. (3) T (7) Equation (7) is the basic observation equation in the plane matching estimation model. Given a set of corresponding planes a similarity transformation can be T T estimated such that the norm || π2 – π1 H || is minimized. To obtain an estimation of the transformation without iteration we may ignore the special orthogonality of R and assume that H is an affine transformation. In effect, we over-parameterize the transformation to make the estimation model linear. The result is the familiar direct linear transform (DLT), which is commonly used in photogrammetry and computer vision [12, 13]. The application of this model has been shown also for the registration of aerial laser strips [14]. To solve for the transformation, Equation (7) is rearranged so as to have the entries of H stacked in a vector: ⎡⎣ n11 j I 4 n12 j I 4 ⎡ n12j ⎤ ⎡ h1 ⎤ ⎢ ⎥ 2 ⎢h ⎥ = ⎢ n2 j ⎥ n31 j I 4 ⎤⎦ 2 ⎥ 4 x12 ⎢ ⎢ n2 ⎥ ⎢⎣ h 3 ⎥⎦12 x1 ⎢ 2 3 j 1 ⎥ ⎢⎣ − d j + d j ⎥⎦ 4 x1 (8) where h1T, h2T, and h3T are the first three rows of the transformation matrix H. For k planes we will have a system of equations of the form given in (8), for which the least-squares solution is: 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland A 4k x12 ⋅ X12 x1 = B4k x1 → T −1 T X = ( A WA) A WB (9) where W is a weight matrix to assign appropriate weights to the two observation types in B. It can be seen that the plane matching equation given in (7) is a general form that contains the plane intersection model as a special case in which only distances are used as observables. Let us decompose the plane parameters in (7) such that: ⎡n1T ⎣⎢ j ⎡ R t ⎤ ⎡ 2T − ρ 1j ⎤⎥ ⎢ = n ⎦ ⎣01x 3 1⎥⎦ ⎣⎢ j − ρ 2j ⎤⎥ ⎦ (10) where R is the rotation and t = s21 is the translation from the second scan to the first scan. We have: ⎡n1T R n1T s1 − ρ 1 ⎤ = ⎡n 2T j 2 j⎥ ⎢⎣ j ⎦ ⎢⎣ j − ρ 2j ⎤⎥ ⎦ (11) which gives rise to two equations: ⎧ n1T R = n 2T j ⎪⎪ j ⎨ ⎪ 1 2 1T 1 ⎪⎩ ρ j − ρ j = n j s 2 (12a) by imposing tight constraints one can substantially reduce the search space. Such tight constraints would then increase the risk of eliminating a correct combination due to small errors in plane parameters. In addition, the constrained search does not result in a single combination containing correct correspondences, but a set of combinations. That is, it may end up with a set of correct yet slightly different transformations. To deal with the combinatorial complexity of finding plane correspondences we use a variant of the hillclimbing algorithm [15], which we call the initiate-andextend search. It works in two steps. In the first step, initial combinations of three (or four) plane pairs are created using a small subset of the planes in each scan. At this stage, loose constraints are imposed to further reduce the number of initial combinations while maintaining the correct correspondences. A transformation is estimated for each remaining combination that now we call an initial match set. In the second step, each initial match set is extended with new planes in the two scans that fit to the estimated transformation. Figure 2 illustrates the initiateand-extend search. A main aspect of this method is that a large number of planes in an extended match set contribute to the estimation of the transformation. In addition, extending the initial match sets provides a straightforward method for finding the correct transformation by picking the largest extended match set (the winning match set). The following sections describe the initiate-and-extend search method in more details. (12b) As it can be seen, Equation (12b) is the plane intersection equation as in (1). Note that in the intersection model only the distance parameter of a plane is used as observable, whereas in the general plane matching model all the plane parameters take part. As a result, the solution to the general model still requires a minimum of three corresponding planes even though the number of unknown variables is raised to 12. III. INITIATE-AND-EXTEND SEARCH FOR CORRESPONDENCES Using the plane matching equation a transformation can be estimated between two sets of corresponding planes in two consecutive scans. A major issue now is how to establish correspondence between the planes. The correspondence problem is usually approached with a search algorithm. Since a minimum of three planes is sufficient for estimating the transformation, a simple search strategy could be to create all combinations of three planes out of all planes in the two scans, and then estimate a transformation between every pair of these plane triplets. The correct transformation can then be identified using an objective score function. Such an exhaustive search strategy will be enormously expensive as the number of planes increases. For example, with 50 planes per scan more than one billion combinations can be created, which should all undergo the transformation estimation and the scoring process. To reduce the computational cost of the search, Brenner and Dold [10] suggested a hierarchy of constraints to progressively prune the search tree. A drawback of such a constrained search strategy is that only Figure 2. The initiate-and-extend search for plane correspondences. A. Creating Initial Match Sets In principle, the total number of combinations of k plane pairs in two sets of m and n planes is expressed as: nc = Ckm ⋅ Pkn = m! n! × k !(m − k )! (n − k )! (13) where C and P denote combination (unordered selection) and permutation (ordered selection) respectively. It can be readily seen that by starting with a small subset of the planes (small m and n) the number of initial match sets can be reduced dramatically. For example, with 10 planes per scan the number of combinations of three plane pairs will be in the order of 105. Imposing a loose relative orientation constraint [16] further reduces the number of initial combinations, typically by a factor of 10~102. The computational cost of processing this number of initial 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland combinations is far more affordable than that of, for instance, 109 combinations when starting with 50 planes. An important issue in initiating with a small number of planes is that the initial match sets should contain at least one combination that includes correct correspondences, and can, therefore, lead to a correct transformation. B. Extending Initial Match Sets If an initial match set contains correct correspondences, then the estimated transformation can be used to find other correct correspondences in the two scans. Extending the plane matching estimation model with these new correspondences will result in a more precise estimation of the transformation. In contrast, if a transformation is estimated from wrong correspondences in an initial match set, the likelihood of other planes randomly fitting in the transformation is very small. Therefore, the number of planes correspondences in an extended match set is an indication of the correctness of the transformation. The extension of the initial match sets follows the concept of the hill climbing algorithm. Each initial match set is extended by taking a new plane from the first scan and transforming it to the second scan (using the transformation estimated by the initial match set). In the second scan, a nearest neighbor operation is performed to find the nearest plane (having the most similar parameters) to the transformed plane. This plane and the plane taken from the first scan are added to the match set, and a new transformation is estimated. If the estimation yields residuals smaller than a threshold, then the extension is accepted; otherwise, the two new planes are removed from the match set. The extension is tried with all the planes in the two scans, which are not already in the match set; the process terminates when no new planes can be added to the match set. Once the extension process is performed for all the initial match sets, a winning match set is identified as one that contains the largest number of plane correspondences. The number of planes in the winning match set is an indication of the reliability of the estimated transformation. A winning match set with too few planes might imply that none of the initial match sets contained a correct set of correspondences. In such case, the matching should be initiated with a larger number of planes. IV. EXPERIMENTS The performance of the plane matching method was tested using two indoor datasets. The first dataset included a set of seven scans acquired in a corridor of approximately 2x5x3 meters dimension. The second dataset was obtained in an interior of the Aula building of TU Delft with a volume of about 8x25x3 meters, and Figure 3. included a set of six panoramic scans. Figure 3 shows reflectance images pertaining to one scan in each dataset. Both datasets were acquired by a FARO LS 880 laser scanner. The position of the scanner at each scan was manually measured to serve as reference in the evaluation of the localization results. In the corridor dataset, 2D measurements were made by a tape, while in the Aula interior a total-station was used to make accurate measurements of the scanner positions in 3D. Planar segments were extracted from all scans using a range image segmentation algorithm as described in [11]. The plane matching process was performed with the planes extracted from the corridor scans in a pair-wise fashion. From the estimated transformation the position of the scanner at each scan was computed. Figure 4 depicts the computed trajectory of the scanner plotted together with the manual measurements of the scanner positions. As can be seen, discrepancies are within a few centimeters. Table I summarizes the results of the corridor test. The values δx and δy are discrepancies between the computed and measured coordinates of the position of the scanner at each scan. The root-mean-squared distance residuals and of normal residuals are derived from the estimation model (of the winning match set) in each registration, and indicate the precision of the registration. The precision measures given in Table I as the RMS residual of the distance parameters of planes range between 1.7 cm and 2.4 cm. Considering the nominal range precision of the scanner (1.1 to 4.2 mm at 25 m) these residuals are noticeably large. The low precision of the registration process represented by large residuals is possibly due to the low precision of the plane parameters obtained by the segmentation algorithm. In the regiongrowing segmentation, the plane parameters are computed within a 5x5 neighborhood around every point, and once a region is fully grown the average of the parameters over the region is associated to the plane. Such a procedure is local and sensitive to noise, and may not provide sufficient accuracy for the plane parameters. Figure 5 shows the number of planes involved in the matching process for the registration of each pair of consecutive scans in the corridor test. A total of 20 largest planes in each scan were used for the registration, out of which 10 were used for the creation of the initial match sets. Each initial match set contained 4 plane pairs. It can be seen from Figure 5 that the winning match sets contain between 10 to 15 planes, which is an indication that a correct set of correspondences was present in the initial match sets, and that a large number of plane pairs contributed in the estimation of the transformation in all registrations. Reflectance images of a scan in the corridor (left) and a scan in the Aula interior (right). 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland Figure 6 shows in a magnified box the closing error of the entire set of estimated registrations as the distance between the initial position of point p1 (which was measured manually) and its computed position obtained from all six pair-wise registrations. Table II summarizes the discrepancies between the measured coordinates of the scanner positions and the computed ones obtained from the estimated registration parameters. As it can be seen, the discrepancies are slightly smaller than those of the corridor test, except for the points p4 and p5, which have larger discrepancies. These points were the farthest to the total-station, and it is likely that their reference measured coordinates were less accurate. The discrepancies at P1 represent the closing error, which is 2.7 cm as the distance between the reference and the computed position of the point. s9 440 420 s8 400 s7 380 s6 Y 360 340 320 s5 s4 300 Total nr planes Nr planes winning matchSet Nr planes initial matchSets 280 24 22 260 s3 40 60 80 20 100 X 120 140 160 Figure 4. The computed trajectory of the laser scanner in the corridor test. The manually measured positions are shown with red marks. The coordinates are in centimeters. TABLE I. ACCURACY OF REGISTRATION AND LOCALIZATION IN THE CORRIDOR TEST. 18 16 Nr planes 240 20 14 12 10 8 6 Scanner position Localization δx (cm) δy (cm) s3 0 0 s4 3.6 2.8 s5 2.2 1.0 s6 1.0 -2.6 s7 -1.8 -0.3 s8 -1.5 1.8 s9 -3.4 -0.5 Mean 0.0 0.3 RMS 2.3 1.7 Registration 4 RMS residual of normals RMS residual of distances (cm) 0.010 2.4 0.029 1.7 0.011 2.3 0.024 2.1 0.021 2.3 0.024 2.0 To verify the influence of the accuracy of plane parameters on the precision of the registration, in the Aula-interior dataset a least-squares plane fitting was applied to the points within the regions of the segmented range images. The plane matching process was carried out with the plane parameters from this fitting procedure to yield the registration parameters as well as the motion of the scanner. Figure 6 shows the positions of the scanner derived from the estimated transformation parameters in the Aula-interior test. As it can be seen, the scanner trajectory forms a polygon so that both sides of a wall in the middle of the area can be scanned. This setting made possible a registration of the last scan and the first scan. 2 0 s3_s4 s4_s5 s5_s6 s6_s7 Registration s7_s8 s8_s9 Figure 5. The number of planes involved in the plane matching process in the corridor test. The results of the Aula-interior test shown in Table II exhibit a noticeable improvement over those of the corridor test in terms of the internal precision of the registrations. Here the RMS residuals of the distance parameters of the planes range from 0.4 to 1.0cm. Also, the RMS residuals of the normal vector parameters are significantly smaller than those of the corridor test. These results indicate that a precise fine registration can be performed using planar segments if the parameters of the planes are sufficiently accurate. Figure 7 shows the registered point cloud of the Aula interior obtained by applying the registration parameters estimated in the pairwise application of the plane matching process. The processing time needed for the registration of all scans was measured in both tests. The algorithms were run on a desktop PC with 3.2 GHz CPU speed and 2.00 GB memory. In the corridor test, where initial match sets were created with four plane pairs out of 10 planes in each scan, the entire registration process took 38 seconds. In the Aula-interior test, where initial match sets contained three plane pairs selected out of 7 planes in each scan the process took 155 seconds. The faster processing of the corridor dataset is due to the fact that initial match sets contained four plane pairs. This allowed for a quick identification of wrong correspondences (using the residual values), which in turn speeds up the extension process. 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland 11 p4 10 p5 9 8 p3 7 Y 6 5 4 p2 1.32 3 p6 1.31 Y 2 1.3 p1 1 1.29 -4 -3 -2 -1 0 1 p1 2 1.28 1.29 X 1.3 1.31 1.32 1.33 X Figure 6. The computed trajectory of the laser scanner in the Aulainterior test. The manually measured positions are shown with red marks. The coordinates are in meters. TABLE II. ACCURACY OF REGISTRATION AND LOCALIZATION IN THE AULA-INTERIOR TEST. Localization Scanner position correspondences are effectively eliminated from the estimation model by rejecting plane pairs whose parameters have large residuals. This results in a high level of robustness against noisy and outlier planes as these normally cannot survive the extension process. The plane matching approach to pointcloud registration and scanner localization requires that a sufficient number of planes are available in each scan, and that at least three of these planes intersect in a point so as to have a unique solution for the estimation model. A degenerate configuration of the planes can be thought of as a set of only vertical walls in two scans, which can not constrain the motion of the scanner in the direction of Z axis. Such a constraint can be provided by including the planes of the floor or ceiling in the transformation estimation model. Another requirement for a fine registration using planar objects is the high accuracy of the input plane parameters. Generally, a segmentation algorithm does not provide highly accurate plane parameters, and a least-squares fitting process is necessary. It was shown that plane parameters from the segmentation lead to a rather coarse registration, which may only be sufficient as an initial approximation for an iterative fine registration method such as ICP. However, with accurate plane parameters derived from a least-squares fitting process, a fine registration can be performed in a single step, with a precision better than 1 cm, which obviates the need for an iterative second registration. Registration δx (cm) δy (cm) δz (cm) p1 0 0 0 p2 -0.7 1.0 -0.6 p3 -0.6 1.5 -0.7 p4 1.0 -13.8 0.5 p5 0.6 -4.8 -4.7 p6 -0.7 -1.3 0.0 p1 -2.3 -0.7 1.2 Mean -0.4 -2.6 -0.6 RMS 1.1 5.6 1.9 RMS residual of normals RMS residual of distances (cm) 0.002 0.4 0.003 0.7 0.002 0.7 0.001 0.0 0.003 1.0 0.003 0.9 V. CONCLUDING REMARKS The paper introduced a plane matching method for pointcloud registration and scanner localization. The transformation between two overlapping scans is estimated in a linear least-squares adjustment model, which is independent of an initial approximation. Plane correspondences are found using an initiate-and-extend search strategy that begins with matching initial combinations of three or four plane pairs, and extends when more planes fit into the estimated transformation. This way, a correct transformation can be found by looking for the largest extended match set, while incorrect Figure 7. The registered point cloud of Aula interior. The ceilings have been removed to provide a better view of the interior. The circles represent scanner positions. ACKNOWLEDGMENT The author would like to thank Ben Gorte for segmenting the range images, and Jochem Lesparre and Giorgi Mikadze for their assistance during the scanning of the Aula interior. REFERENCES [1] [2] J. Salvi, C. Matabosch, D. Fofi, and J. Forest, "A review of recent range image registration methods with accuracy evaluation," Image and Vision Computing, vol. 25, pp. 578-596, 2007. P. J. Besl and N. D. McKay, "A method for registration of 3-D shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, pp. 239-256, 1992. 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 15-17 September 2010, Zürich, Switzerland [3] M. Greenspan, G. Godin, and I. C. S. Ieee Computer Society, "A nearest neighbor method for efficient ICP," in 3rd International Conference on 3-D Digital Imaging and Modeling, Quebec City, Canada, 2001, pp. 161-168. [4] T. Jost and H. Hugli, "A multi-resolution scheme ICP algorithm for fast shape registration," in 1st International Symposium on 3D Data Processing Visualization and Transmission, Padua, Italy, 2002, pp. 540-543. [5] G. C. Sharp, S. W. Lee, and D. K. Wehe, "ICP registration using invariant features," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 24, pp. 90-102, Jan 2002. [6] E. Trucco, A. Fusiello, and V. Roberto, "Robust motion and correspondence of noisy 3-D point sets with missing data," Pattern Recognition Letters, vol. 20, pp. 889-898, Sep 1999. [7] Y. Chen and G. Medioni, "Object modeling by registration of multiple range images," Image and Vision Computing, vol. 10, pp. 145-155, Apr 1992. [8] S. Schuhmacher and J. Bohm, "Georeferencing of terrestrial laser scanner data for applications in architectural modeling," in 3DARCH 2005: Virtual Reconstruction and Visualization of Complex Architectures, Mestre-Venice, Italy, 2005. [9] D. H. Chung, I. D. Yun, and S. U. Lee, "Registration of multiplerange views using the reverse-calibration technique," Pattern Recognition, vol. 31, pp. 457-464, 1998. [10] C. Brenner and C. Dold, "Automatic relative orientation of terrestrial laser scans using planar structures and angle [11] [12] [13] [14] [15] [16] constraints," in ISPRS Workshop on Laser Scanning 2007 and SilviLaser 2007, Espoo, Finland, 2007, pp. 84-89. B. Gorte, K. Khoshelham, and E. Verbree, "Indoor navigation by using segmentation of range images obtained by laser scanners," in International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences. Vol. XXXVII. Part B1., J. Chen, J. Jiang, and A. Baudoin, Eds. Beijing, China, 2008, pp. 971-976. K. B. Atkinson, Close Range Photogrammetry and Machine Vision. Caithness: Whittles Publishing, 1996. R. Hartley and A. Zisserman, Multiple view geometry in computer vision, 2nd edition. Cambridge, UK: Cambridge University Press, 2003. C. v. d. Sande, S. Soudarissanane, and K. Khoshelham, "Assessment of Relative Accuracy of AHN-2 Laser Scanning Data Using Planar Features," Sensors, vol. 10, pp. 8198-8214, 2010. S. S. Skiena, The algorithm design manual, 2nd edition, 2nd ed. London: Springer, 2008. K. Khoshelham and B. G. Gorte, "Registering point clouds of polyhedral buildings to 2D maps," in Proceedings of the 3rd ISPRS International Workshop 3D-ARCH 2009: "3D Virtual Reconstruction and Visualization of Complex Architectures". vol. XXXVIII-5/W1, F. Remondino, S. El-Hakim, and L. Gonzo, Eds. Trento, Italy, 2009.