IRI-TR Registration of 3D Points Clouds for Urban Robot Mapping. Ernesto Homar Teniente Avilés Juan Andrade Cetto - PDF

IRI-TR Registration of 3D Points Clouds for Urban Robot Mapping Ernesto Homar Teniente Avilés Juan Andrade Cetto Abstract We consider the task of mapping pedestrian urban areas for a robotic guidance

Please download to get full document.

View again

of 20
All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.


Publish on:

Views: 20 | Pages: 20

Extension: PDF | Download: 0

IRI-TR Registration of 3D Points Clouds for Urban Robot Mapping Ernesto Homar Teniente Avilés Juan Andrade Cetto Abstract We consider the task of mapping pedestrian urban areas for a robotic guidance and surveillance application. This mapping is performed by registering three-dimensional laser range scans acquired with two different robots. To solve this task we will use the Iterative Closes Point (ICP) algorithm proposed in [8], but for the minimization step we will use the metric proposed by Biota et al. [10] trying to get advantage of the compensation between translation and rotation they mention. To reduce computational cost in the original ICP during matching, the correspondences search is done with the library Approximate Nearest Neighbor (ANN). Finally we propose a hierarchical new correspondence search strategy, using a point-to-plane strategy at the highest level and the point-to-point metric at finer levels. At the highest level the adjust error between a plane and it s n adjacent points describing the plane is computed, if this error is bigger than a threshold then we change the level. Institut de Robòtica i Informàtica Industrial (IRI) Consejo Superior de Investigaciones Científicas (CSIC) Universitat Politècnica de Catalunya (UPC) Llorens i Artigas 4-6, 08028, Barcelona Spain Tel (fax): (5751) Corresponding author: Ernesto H. Teniente Avilés tel: http: // Copyright IRI, 2008 CONTENTS 1 Contents 1 Problem Statement 2 2 State of the art 2 3 Range image registration Correspondences Search Registration Step Data association The kd-trees Nearest Neighbor Search using kd-trees A Nearest Neighbor Library Point-to-point association Point-to-plane association Data Reduction Uniform sampling Limit axis distance Filtering the associated data Median filter A Hybrid Hierarchic Approach to Data Association Experiments Data Acquisition Implementation Conclusions 22 7 Acknowledgments 22 2 Registration of 3D Points Clouds for Urban Robot Mapping 1 Problem Statement We consider the task of mapping pedestrian urban areas for a robotic guidance and surveillance application. Multiple 3D scans are necessary to build a map with enough environment information. To create a correct and consistent map, the scans must to have a common reference coordinate system. This process is called registration. If the 3D systems were precisely locate with the robots odometry, the registration could be done directly with this information. However, because the uncertainty on the robot sensors, self localization is erroneous, so a method to have a correct overlap of the 3D scans has to be considered. The mapping is performed by registering 3D laser range scans acquired with two different robots. This mapping application is part of the EU URUS Project (Ubiquitous Network Robotics in Urban Settings). 2 State of the art Scan matching algorithms are often used in mobile robotics to correct the relative motion of a vehicle between two consecutive configurations, by maximizing the overlap between the range measurements obtained at each configuration. The most popular scan matching methods [31] are based on the ICP from Besl and Mckey [8] which is borrowed from the computer vision community. The objective of this algorithm is; to compute the relative motion between two data sets partially overlapped. The algorithm iteratively minimizes the MSE and proceeds as follows: first, for each point in one data set, the closest point in the second one is found or vise versa (correspondence step), then the motion that minimizes the Mean Square Error (MSE) between the correspondences is computed (registration step), finally the data shape is updated (update step). In the registration proposed by Besl and Mckey a point-to-point metric is used to measure the closeness of data, they also suggest an accelerated version of the algorithm by using a linear approximation and a parabolic interpolation with the last three minimization vectors if they are well aligned, which means they have been moving in an approximately constant direction. The use of sampling or tree-based-search to speed up the algorithm are mentioned as future refinements to reduce the computational cost. Chen and Medioni [13] proposed a point-toplane error metric, which makes the algorithm less susceptible to local minima than the metric proposed by Besl and Mackey [8]. The idea in [13] is, that given a so called control point in the Section 2 State of the art 3 first surface, to compute the distance to the nearest tangent plane in the second cloud. Blais [11] suggests a point-to-projection solution computing the sum of the distances between the two range views in the direction of the rays. This has also been called reverse calibration. This approach makes registration very fast because it does not involve any search step to find the correspondences. However, one of its disadvantages is that the resulting registration is not as accurate as the one given in the point-to-point and point-to-plane metrics [31]. Turk and Levoy [35] proposed a point-to-triangle correspondence using meshes which they call zippered meshes finding the nearest position on a mesh to each vertex of an other mesh. They find the rigid transformation that minimizes a weighted least-squared distance between correspondences with a value in the range from 0 to 1 called confidence. For the case of structured light scanners, they measure the the confidence of a point on a mesh to be the angle between the mesh normal and the sensor viewing angle. The confidence is a measure of how certain they are of a given range point s position, which helps to eliminate possible wrong matches. Chetverikov et al. [15] presented a robustified extension of the ICP applicable to overlaps under 50%, robust to erroneous and incomplete measurements, and has easy-to-set parameters called Trimmed ICP (TrICP). The algorithm is based on the consistent use of the Least Trimmed Square [30] in all phases of the operation, on the other hand Yamany et al. [1] used genetic algorithms maximizing an objective function, where the genes are formed by concatenating six binary coded parameters, representing the three angles of rotation and the 3 dof for translations. More recently, a new error metric which explores the compensation between the rotation and translation was proposed by Minguez et al. [23, 24] for the 2D space and Biota et al. [9, 10] extended this metric to the 3D space. They used a point-to-projection minimization using triangles as the projection surface, and perform the Nearest Neighbor (NN) correspondences in the space of the new metric space. They did not tackle the computational complexity of the algorithm in any way, so it is understood they use brute force search to do the matching. The ICP bottleneck in time execution is when searching for point matches. One strategy to reduce the computational complexity is to use tree-based search techniques [28]. Nütcher et al.[25] uses a library called Approximate Nearest Neigborh (ANN), developed by Arya and Mount [5], that uses a balanced kd-tree or box decomposition tree (bd-trees). Also Nütcher et al. [25] proved that kd-trees are faster than bd-trees to the NN problem. Simon et al.[32] used a kd-tree but using a catching points technique, where in the first iteration n neighbors for all the 4 Registration of 3D Points Clouds for Urban Robot Mapping points in the reference cloud are found from the model query and they are cached. It is assumed that after updating the reference cloud, the cached points for each point in the updated cloud should be neighbors. Benjemaa [6] used a double z-buffer structure which provides an explicit space partitioning. Yamany et al. [1] said that the time matching can be significantly reduced by applying a grid closest point (GCP) technique. The GCP is an other sampling scheme which consists of superimposing a 3D fine grid on the 3D space such that the two clouds lie inside the grid, dividing the 3D space on cells, each cell with an index of its closest point in the model set. Greenspan and Godin [17] presented a new solution for the NN search for the matching step which they called Spherical Triangle Constraint. Like Simon et al. [32], they store correspondences at each iteration so that these are available at the next iteration. The Spherical Constraint is applied to determine whether or not the nearest neighbor falls within the neighborhood of each point estimate, and if so, the Triangle Constraint and the Ordering Theorem, are applied to the neighborhood to quickly identify the correspondence. The Ordering Theorem orders a set of points by increasing distance to some point. They shown that after aprox. 20 iterations, their method is more efficient than kd-trees in computational complexity and time execution. More recently Akca and Gruen[2] used a box structure [14] which partitions the search space into boxes, where for a given surface element, the correspondence is searched only in the box containing this element and in the adjacent boxes, the correspondence is searched in the boxing structure during the first few iterations, and in the meantime its evolution is tracked across the iterations. In the end, the searching process is carried out only in an adaptive local neighborhood according to the previous position and change of correspondence. One of the main advantages of the box structure is that it has a faster and easier access mechanism than the tree-based search methods provide. A another common strategy to accelerate the matching process is to reduce the number of points. Sampling the data reduces the match execution time by a constant factor, but retains linear asymptotic computational complexity. Coarse-to-fine strategies haven been used by Zhang [37] and Turk and Levoy [35]. They start using a less detailed description of the data and as the algorithm approaches the solution, the resolution is hierarchically increased. The techniques used for sampling data vary. Turk and Levoy [35] and Blais [11] used uniform sampling. Masuda et al. [22] used intead, random sampling for each iteration. Moreover, Nütcher et al. [27] and Gutmann [18] used a technique best suited to the nature of data for laser range finders so called, Section 2 State of the art 5 reduction filter, which has been shown to work well on realtime applications. However, sampled methods are very sensitive to data content, i.e. noise level, occlusion areas, complexity of the range data, etc. If too many points come from outliers due to sensor errors, this may produce too many wrong correspondences, and may cause the solution to converge on a local minimum leading a poor final overlap, or in the worst case, to divergence. We shall remember that the original algorithm from Besl and Mackay considers data sets without outliers. Several approaches to dismiss possibles outliers have been proposed using rejection strategies. Rejections based on thresholds for the maximum tolerable distance between paired points were implemented by Turk and Levoy[35], the threshold is set as twice the maximum tolerable space between range point meshes; and is adaptively changed when building the the points in the mesh. Rejection that use a statistical method based on the distribution of point distances were used by Zhang [37], and Pulli [29], who used two thresholds for the maximum allowed distance between paired points, one of them dynamic. Masuda et al. [22] also rejects pair matches whose point-to-point distance are larger than some multiple of the standar deviation of distances. Pulli as well as Turk and Levoy [35] use not only statistical reasoning, but also topological information to discard matches. They removed matches that occur on mesh boundaries, and Zhang [37] and Pulli [29] used the angle between the normals of the paired points as a constrain to keep matches. When using a laser range data Nütcher et al. [27] use a median filter to remove the Gaussian noise for each scan row. Another issue on the ICP is the rate of convergence of the minimization step. To accelerate such convergence and to reduce overshoot, some authors propose minor changes to the original extrapolation proposal of Besl and McKay. For instance, Rusinkiewicz and Levoy [31] used the update based on linear zero crossing of the line instead of the extremum of the parabola when quadratic extrapolation is attempted and the parabola opens downwards, and multiply the amount of extrapolation by a dampening factor, arbitrarily set to 0.5 in their implementation. Even when this occasionally reduces the benefit of extrapolation, it also increases stability and eliminates many problems with overshoot. Simon et al. [32] said that while Besl and McKay calculate a single acceleration scale factor for both translation and rotation, they decouple the acceleration of translation and rotation achieving better results. 6 Registration of 3D Points Clouds for Urban Robot Mapping 3 Range image registration In this section, the Iterative Closest Point (ICP) is described in detail. In the minimization step we will use the metric proposed by Biota et al. [10] trying to get advantage of the compensation between translation and rotation they mentioned. To reduce computational cost in the original ICP during matching, the original data is sampled and the NN search is done with the library Aproximate Neares Neighbor (ANN). Finally we propose a new hierarchical correspondence search strategy, using a point-to-plane metric at the highest level and the point-to-point search at finer levels. At the highest level the adjust error between a plane and its n adjacent points describing the plane is computed, if this error is bigger than an user specified threshold we change the level. Given a set of 3D points, S = {p 1,p 2,...,p n } acquired from a reference frame q = [x,y,z,r x,r y,r z ], and S = {p 1,p 2,...,p m } a set acquired from a new frame q = [ x,y,z,r x,r y z],r, we need to estimate the relative displacement between the sensor poses at q and q. The ICP deals with this problem in an iterative process in four steps [8]. At each iteration k, there is a search of correspondences between the points of both scans, then the relative displacement q k is computed by a minimization process. The model scans are updated with the last computed q k ; this is repeated until convergence. 3.1 Correspondences Search The basic idea in correspondence search is to find the closest point p x to a given reference point p q in a reference set S, and according to a specified metric, usually Euclidean distance. It is known as the Nearest Neighbor (NN) problem, also known as closest point search. A correspondence is established by means of the correspondence operator C, which is the closes point operator defined by: C(p q,s ref,k ) = argmin } {{ } p x S ref,k (p x p q ) (1) Specifically our problem is to find for each point p i in S (in case that exist) the closest point p ix in S, resulting in a subset Y of n correspondences (p j,p jx ). Then for each iteration k a subset Y k is given by: Section 3 Range image registration 7 Y k = C(S,S k ) Besl and McKey [8] assume that for each point in reference set must be a correspondence in the data set, in our application this is not the case. 3.2 Registration Step According with Biota et al [10], the registration problem is solved using a Last Square Minimization (LSM) to compute the q that minimize the realtive displacement between the two points sets. Given two associated points p i = (p ix,p iy,p iz ) and p i = (p ix,p iy,p iz ), the next expression needs to be minimized: E dist (q) = n i=1 d ap p (p i,q(p i ) 2 (2) The above equation could be expressed as follows E dist (q) = δ T i (q)mδ i (q) (3) Where δ i (q) = p i qp i p i p i + U(p i )r T and U(p i ) = 0 p iz p iy p iz 0 p ix p iy p ix 0 p 2 ix + L2 p ix p iy p ix p iz M = p ix p iy p 2 iy + L2 p iy p iz p ix p iz p iy p iz p 2 iz + L2 Finally, the q that minimizes equation (3) is given by: 8 Registration of 3D Points Clouds for Urban Robot Mapping q min = n i=1 M MU(p i ) U T (p i )M UT (p i )MU(p i ) 1 n Mδ UMδ i=1 4 Data association In the ICP, finding the correspondences is the most computationally expensive step. Kd-trees are suggested in [8], demonstrated in [37] as an alternative, and later implemented in [32, 27] to speed up this step, 4.1 The kd-trees The kd-trees are a generalization of the binary search trees. The idea behind this data structures (trees) is to extend the notion of a one dimension tree on a recursive subdivision of the space, i.e. for the 2D case the subdivision alternate in using the x or y coordinates to split Fig. 4.1(left). Therefore we first split on x, next on y, then again on x, and so on. In general dimension, the kd-tree cycles among the various possible splitting dimensions. Each partition (of a point set) is represented by a node containing the two successor nodes or by a bounding box that contains the data points for this node Fig. 4.1(right). The root node represents the whole point cloud and the leafs form a disjunct partition of the set. As long as the number of data points associated with a node is greater than a small quantity, called the bucket size (Friedman et al. [16] proved that a bucket size of 1 is optimal ), the box is split into two boxes by an axis-orthogonal hyperplane that intersects this box. Figure 1: A kd-tree example: subdivided data (left) and the corresponding binary tree (right) There are different splitting rules which determine how this hyperplane is selected. The choice Section 4 Data association 9 of the splitting rule affects the shape of cells and the structure of the resulting tree. Standard splitting rule: The splitting dimension is the dimension of the maximum spread (difference between the maximum and minimum values), leading to many cells with high aspect ratio Fig. 2(a). The splitting point is the median of the coordinates along this dimension. A median partition of the points is then performed. This rule guarantees that the final tree has height (log 2 n), also guarantees that every kd-tree entry has the same probability. Friedman et al. [16] introduced this splitting rule in their definition of the optimized kd-tree. Midpoint splitting rule: When splitting the space, to guarantee that the tree is balanced, the most common method is the midpoint splitting rule. The splitting value is the median splitting coordinate Fig. 2(b). As a result, the tree will have O(logn) height. Sliding-midpoint splitting rule: First a midpoint split is attempted. If the data points lie on both sides of the splitting plane then the splitting plane remains here. However, if all the data points lie to one side of the splitting plane, then splitting plane slides toward the data points until it encounters the first point. One child is a leaf cell containing this single point, and the algorithm recurses on the remaining points Fig. 2(c). (a) Standard split (b) Midpoint split (c) Sliding-midpoint split Figure 2: A kd-tree splitting rules example 4.2 Nearest Neighbor Search using kd-trees In section 3.1, we mentioned the need to solve the NN correspondence problem as one of the step of the ICP algorithm. Now we are going to explain how to tackle this issue using kdtrees. A simple and naive way to approach the NN problem is by using brute force search, 10 Registration of 3D Points Clouds for Urban Robot Mapping where the closest point is found computing the distance (given a metric) between each point in S ref and p q, this is highly expensive in computation time, O(n) worst case with expected cost log(n). Friedman et al. [16] showed that O(log n) query time is possible in the average case through the use of kd-trees. Their use ensures that the nearest data point to p q could be found efficiently. High-dimensional (at least three) NN problems arise naturally when complex objects are represented by vectors of d numeric fe
Related Search
Similar documents
View more...
We Need Your Support
Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks