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.