REGISTRATION OF TERRESTRIAL MOBILE LASER DATA ON 2 D OR 3 D GEOGRAPHIC DATABASE BY USE OF A NON-RIGID ICP APPROACH

This article presents a generic and efficient method to register terrestrial mobile data with imperfect location on a geographic database with better overall accuracy but less details. The registration method proposed in this paper is based on a semi-rigid point to plane ICP ("Iterative Closest Point"). The main applications of such registration is to improve existing geographic databases, particularly in terms of accuracy, level of detail and diversity of represented objects. Other applications include fine geometric modelling and fine façade texturing, object extraction such as trees, poles, road signs marks, facilities, vehicles,etc. The geopositionning system of mobile mapping systems is affected by GPS masks that are only partially corrected by an Inertial Navigation System (INS) which can cause an important drift. As this drift varies non-linearly, but slowly in time, it will be modelled by a translation defined as a piecewise linear function of time which variation over time will be minimized (rigidity term). For each iteration of the ICP, the drift is estimated in order to minimise the distance between laser points and planar model primitives (data attachment term). The method has been tested on real data (a scan of the city of Paris of 3.6 million laser points registered on a 3D model of approximately 71,400 triangles).


Context
Over the last years, a growing number of mobile mapping systems have been developed in order to obtain large amounts of accurate georeferenced data on urban canyons.In this context, a number of problems arise especially in georeferencing because buildings cause GPS masks which are partially corrected by the inertial measurements.That is why, it is not possible to use these data with a high level of detail directly without making a registration pass.In this paper, we propose not to find the ideal absolute position, but to register such data on the geographical database that it is supposed to help improve.In the following, the term model will denote any such 2D or 3D geographical database.Such models give a rough and generalized representation of reality.They have a reliable georeferencing (even if not perfectly accurate ) because they are built on points measured by human operators.For instance the one we used in this study has a precision around 30cm.However, they usually have a low level of detail (details smaller than 1m were not represented in our database).A model is constituted of geometrical primitives which can be: punctual (0D, such as a levelling network point, apex of polyhedral objects, tree trunks position or posts in 2D), linear (1D, such as edges of 2D polygons, edges of 3D polyhedral objects or linear objects in 3D, land registry, topographic database or fragmented data in 2D representation and curbs), or surface (2D, such as sides of polyhedral objects).In this paper, the term mobile data will denote any data (image or laser) acquired by a terrestrial mobile mapping system.A mobile mapping system is a vehicle which integrates camera and/or laser sensors in order to perceive the environment and a positioning system that allows to localize the data acquired by these sensors.These systems allow to move the sensor closer to the observed data.The level of detail of these acquisitions has been increased compared to airborne imaging.This advantage has led to fast development of these types of system over the last years.Mobile mapping location is generally define by firstly a global position-ing system (GPS) allowing to obtain the position of an object with varying accuracy.The latter depends on the acquisition conditions and the system which lead to an accuracy of a few meters to 0.10 m.Secondly, it is supported by an inertial measurement unit (IMU) consisting of accelerometers measuring the vehicle acceleration, gyrometers measuring the angular acceleration and magnetometers to obtain the position of the geographic true north at all time of whose the accuracy depends on the used system and thirdly sometimes an odometer measuring the distance crossed by the vehicle.The data stemming from these sensors are integrated to compute the precise position of the vehicle at all times.However, this geolocation maybe disrupted by two phenomena.On the one hand the multiple path denotes the fact that the GPS sensor receives the same signal several times, either directly, or indirectly (reflected by façades for example) which disturbs it.On the other hand the GPS masks are characterised by a loss of information.The part of the sky that is visible is relatively small and the sensor does not see satellites well enough to deduct its correct position.These disturbances leads to an absolute error in the trajectory estimation of the vehicle which is partially compensated for by the information of the IMU.This partial compensation generally infers a gap which can reach several meters in the case of a GPS mask that lasts several minutes.In this article, we call drift this gap between the trajectory supplied by the system and the real (ideal) trajectory.The registration method aims to estimate this drift.The drift depends not only on the quality of the positioning but also on the algorithm of integration of the sensor data.It must be considered as non-linear (non-rigid) according to the time (the drift occurs even if the vehicle is at a standstill).However, good IMU quality guarantees a very slow variation of the drift as well as a very good orientation of the vehicle.The drift mainly consists in slow drift in translation of our registration and in particular the drifting model based on these two hypotheses.

Motivation
For all these reasons, it is interesting to register mobile data on models, particularly for applications such as change detection, updating geographical databases, increasing their level of detail (geometry and texturing) and making all datasets correspond to a common standard.The underlying motivation is twofold: • combine the model robustness (less detailed) with a high level of detail of mobile data (less precise), • make the model and the mobile laser data compatible in order to improve the model geometry, and its texture because if the vehicle is equipped with cameras, this compatibility will also be beneficial to the registration of the laser data on the model.
The method described here can be used for any positioning system for whose drift variations are slow enough (which is the case with all the systems integrating IMU or precise accelerometers).
The major obstacles in this registration are: • the incompatible level of detail between the model (a few tens of centimetres) and the mobile data (one centimetre), • the positioning error of the mobile mapping system linked to a strong non-linear drift in time.
The main objective is not to obtain an absolute centimetre geolocation of the mobile data, but to ensure its relative consistency with the model to make them readily usable for further applications.

STATE OF THE ART REGISTRATION
Data registration is a topic that has been studied for many years by various scientific communities (computer vision, computer graphics, photogrammetry, medical imaging, artificial intelligence).
The main objective consists in registering at least one dataset on an other.One of the dataset usually serves as a reference on which the second will be overlapped.The purpose is to determine the transformation (rotation and translation) necessary to bring them as close as possible.Data registration technique combine two important steps.The first one consists in comparing all the datasets to extract common characteristics for each of them.The second uses these characteristics to determine the optimal transformation to use.These methods, summarized in (Gressin et al., 2013) may be rigid or non-rigid, 2D or 3D, and use point to point or point to plane distances.Currently, the reference technique which is most often used (Salvi et al., 2007) for registration problems is the ICP (Iterative Closest Point).This registration method has the advantage of being simple to use while giving very good results.The major drawback is it requires a good initial estimation of the relative position of the objects to register (Chen and Medioni, 1992).It works in a iterative way and consists in minimizing a distance until convergence to determine the optimal transformation between the objects.The ICP was introduced for the first time by (Besl and McKay, 1992) and worked by finding matches between two initial entities.This matching was simple and consisted in looking for the closest point to point match in both datasets.Then, numerous researches has been carried out to improve this technique.
A few registration methods have been developed apart from ICP.
To quote a few, (Pottmann et al., 2004) used a local quadratic approximation, (Ripperda and Brenner, 2005) used techniques based on the distributions of the normal field named NDT for "Normal Distribution Transform ", (Tsin and Kanade, 2004) performed the registration process by using nucleus correlation, (Chen et al., 1999) used an approach based on "RANSAC" (Random Sample Concensus), (Jian and Vemuri, 2005) used a gaussian mixture, and (Wolfson and Rigoutsos, 1997) used a registration method based on geometrical hashing.
Positioning The prior on the drift mentioned before allowed us to turn to an ICP registration method.However, we saw that not only was there a drift but that it was non-linear in time.The initial strategy was to use a simple and robust registration method based on the ICP between geometrical primitives of the model and the 3D points of the mobile data.This correspondence technique was characterized by a non-rigid drift model defined piecewise linearly between different control times throughout the trajectory.
The main contribution of the article with regard to the state of the art is the definition of this drift model, particularly well adapted to mobile mapping and therefore to the drift that we try to estimate.
The other characteristics of our method are: 1. point selection by a local geometrical descriptor inspired by (Demantké et al., 2012), 2. using the geometry of acquisition in order to define compatibility between the normal of the model and the point cloud.This compatibility allows for more robustness in the matching as demonstrated by (Rusinkiewicz and Levoy, 2001).

DRIFT MODEL
We are now going to present the choices made for our drift model denoted as D.

Model Choice
As explained in section 1, our choice of model was guided by three characteristics of the positioning system drift.Firstly, the drift variation was slow.Secondly, it had a good orientation quality (a small rotation drift compared to the translation drift).Thirdly, the drift was independent from the speed of the vehicle (there is drift even if the vehicle is at a standstill).So, we chose a drift model defined by a piecewise linear translation according to time t.The choice of the temporal dependence was natural in view of the third characteristic, and also allowed to manage the difference of vehicle drift in case of loops (scanning the same place at different times).Finally, the choice of linear interpolation rather than cutting into rigid blocks (Gressin et al., 2013) allows to model the real drift more finely.

Formalism
The drift of the initial trajectory can be written as a translation D : t −→ R (3) which defines the real trajectory registration of the vehicle: Where Pinit is the initial position of a point of the cloud (calculated by the system of positioning), Prec is its position after registration and t is the acquisition time.This information is given by the GPS with a precision of the order of a hundredth of a second.Drift D is discretized by a piecewise linear function.For point Pinit acquired at time t, the corresponding point Prec is determined by interpolating Pinit (cf.equation 2) between two drift value δc given at the upper and lower control time Tc spaced out regularly by an interval of time ∆t (cf.Figure 1): where variables known at time t The step of temporal discretisation ∆t must be chosen as rather small in order to approximate the non linear drift accurately enough.Thus, the unknowns determined by ICP will be the three compo- It allows to obtain the new trajectory of the vehicle by equation (1) hence, allowing for the complete cloud registration by application of the drift for every point.In general, in this article, trajectory registration T rajrec will always be defined by T rajrec = T rajinit + D δ (t) where T rajinit is the initial trajectory and δ the vector concatenating the drift δc for each control time Tc and which defines it in a differential way with regard to the initial mobile data (cf.Figure 2).It particularly allows us to define a simple distance between two trajectories defined by δ i and δ j which we shall call "average drift" and denoted as DM : where NT c represents the number of Tc along the trajectory.This allows us to take into account not only the geometry of the point clouds but also the time dimension because the distances were measured between points acquired at the same time (at control time Tc) and not between the closest points to both cloud.Also please note that this distance can be seen at the same time as a distance between point clouds and between the corresponding trajectories because the drift D δ (t) applies to both.

Rigidity
As this model can be deformed, it is necessary to parametrize its rigidity.We did this defining a deformation energy corresponding, by analogy to mechanical deformation, with the amount of energy to be provided to the system in order to deform it (in particular this energy must be null in a rigid transformation).Thus, we are going to define a deformation energy denoted as E def : Minimizing this energy will bring the drift at time Tc to be close to the drift at time Tc+1, which allows to introduce the a priori mentioned in section 1 i.e. the variation of the drift is slow.This will also help convergence by propagating the constraints along the trajectory.

ICP (ITERATIVE CLOSEST POINT) 4.1 Principle
The ICP is a registration technique that works by matching different datasets thanks to an iterative method.Every iteration tries to move the datasets closer to each other by minimizing an energy Ereg.The iterative process is stopped when a criterion is reached.
The key of this method is that it can be adapted to various types of metrics such as point to point, point to primitive and primitive to primitive distances.The major drawback is that it needs a good initial guess of the drift values (rotation and translation).This is not limiting in our case where the acquisition system ensures a correct geolocalization of the mobile data.

The six steps of ICP
The ICP algorithm described in (Rusinkiewicz and Levoy, 2001) is decomposed in six steps: (1) selection of characteristic primitives on each dataset, (2) match these selected primitives, (3) matching agreement, (4) weight of the matches,(5) term definition, (6) system minimization.In this paper, the method is explained following these steps.

Selection of characteristic primitives
Point selection is an essential step of the registration.In our study, the models used describe the geometry of buildings in an urban environment by means of geometrical 3D primitives (triangles).Therefore, to perform a registration, it is necessary to determine which points in a laser point cloud correspond to a building façade.In our experiment, the ground was not scanned, and the roofs were not visible from the ground.Thus, we limited the selection to façade points.It would thus be possible to work in 2D but we chose to keep to 3D for two reasons: firstly to make the method extendible to scans observing the ground (thus allowing for altimetric correction) and secondly to tackle special cases such as a small building in front of a bigger one or to vertically unhook façades.We use a local geometric descriptor, introduced by (Demantké et al., 2012), to describe the probability of a point P to belong to a façade (cf.Figure 3).The selection of façade point is done by thresholding the local geometrical descriptor computed from every 3D laser point (cf.Figures 4 and 5a).

Matching and agreement
Once the façade points selected, it is then possible to match them with the model's primitives.We shall say that the match is good if the data point results from a laser echo on a real object which is represented the matching primitive.In all other cases, we shall say that the match is bad.Bad matches may be due to two factors.Firstly, a sufficiently large drift causing that a point is attributed to the wrong primitive.Secondly, the existence of objects that are not modelled in the scene (trees, facilities, etc.).Laser points corresponding to such objects do not correspond to a primitive of the model but may be matched to one.In our case, the selection reduced the number of points of this type but could not eliminate them completely.
Bad matches can lead to large errors in drift estimation if their proportion is too significant.So, the objective was to generate as many good matches and as few bad ones as possible.For that purpose, points are usually matched to the closest primitive provided that the distance from the point to the primitive is lower than a fixed threshold dmax.In our case, the mobile mapping system records the spatial position of the laser center, denoted as C, for every point P , which allowed to define the laser beam R = CP .In our study, the direction of the normal nP was computed using the method of (Demantké et al., 2012), and was oriented in the direction of the laser center.Every primitive P rim of the model also has a normal nP rim directed from the inside to the outside, so we could refine the process of agreeing the matches by adding a normal compatibility criterion.Finally, we used beam R = CP to match a point P to the first primitive P rim intersected by R meeting the following two conditions (cf.Figure 5b): 1. Normal compatibility: nP rim.nP > 0 (cf.Figure 5c), 2. Distance of acceptance: dist(P, P rim) < dmax (cf.Figure 5d), If no primitive is intersected or they do not meet the conditions, no match is made.This additional information improves the robustness of the matching thanks to the contribution of the compatibility of the normal, and thus the robustness of the ICP.Moreover, using ray-tracing to select primitive candidate can be performed by highly optimized libraries reducing drastically the processing time as the matching is by far the most time consuming step.If the laser center position is not known, the presented method can be used by returning to a more classical matching, to the detriment of losing the two advantages mentioned above.In the rest of this article, we will index the matches by i: match i pairs the beam Ri = PiCi acquired at time ti to primitive P rimi.nP i and nP rim i will denote the normals associated to Pi and P rimi.

Weight of the matches
We also use a normal compatibility criterion to weigh the different matches: This weight is always positive as the selection rejects the matches for which wi ≤ 0.
4.2.4Data term Every iteration of the ICP aims at moving the data closer to the model.This is done by minimizing a data term.In our case, we can simply define it as: where Pi(δ) = Pi + D δ (ti) is a matched point to which we applied the drift defined by δ (cf.equation 2), Napp is the total number of matchings and Qi is any point belonging to the primitive P rimi.

System minimization
The semi-rigid registration problem (for a given matching) consists in finding the drift D δ (t) defined by the set of δc which minimizes: where λ rigid is a rigidity parameter chosen by the user (cf.Section 3.3).We are now going to write (7) in matrix form, by separating both energies.According to equation ( 2) each squared term of E model in ( 6) is equal to: dist(Pi(δc), P rimi) E model can be written in matrix form (weighted least square): ) concatenating the drift vectors δc for each control time Tc, -b model is a dimension Napp vector whose i th term is: -A model is a matrix of dimension (Napp × 3NT c ), The i th line of this matrix contains only six non null terms starting from index 3c − (ti): the three components of (1 − α(ti)) nP rim i then those of α(ti) nP rim i , -W model is a diagonal matrix of dimension Napp and i th diagonal term wi.
In the same way, according to the equation ( 4) we can write E def in the following way:  Finally, according to (7), we obtain the matrix expression: with: This optimal vector δ of dimension 3 * NT c defines the drifts δc associated with control times Tc.Consequently, we can use its values to compute the new trajectory and new points positions.
The registration by ICP is made by iterating the matching and minimization steps until a stopping criterion is reached (cf.Figure 6): This model was generated according to the method described in (Durupt and Taillandier, 2006).
The mobile data: The mobile data that we used was produced by the terrestrial mobile mapping vehicle Stéréopolis II ((Paparoditis et al., 2012)).Two fixed RIEGL lasers simultaneously acquire the scene on each side of the road with a rate of 10 000 pts/sec each.These lasers cover an angular sector from the horizontal to 80 • upward in a plane orthogonal to the trajectory thus covering most of the façades, but not the ground.The data used for the evaluation consists of 3.6 million laser points extending over a distance of about 400 meters and were acquired in 3 minutes.This data is well localized and close to the model (0.5 m on average) as shown in Figure 7.
The selection step kept 54, 7% of these points, some of which correspond however to planar vertical surfaces that were not represented in the model (bus shelters, trunks, etc.) as shown in figure 8.

Results
This method was used to register the data presented in section 5.1.At the end of the registration process, 93, 89% of the selected points were matched and the point to primitive distance decreased from 0.5 to 0.095 m.The final distance must not be interpreted as a registration error as this remaining distance comes mainly from façades details laying outside of the main façade plane (windows, balconies).At visual inspection, façade planes from the model always exactly coincide, so we consider this registration result as nearly optimal.The registration lasted 182s all in all.The process stopped after 16 iterations of approximately 11s each (matching, system resolution and application of the drift).The number of iterations depend on the initial distance from the data to the model (the greater the distance, the more iterations are necessary to re- trieve good matches) and on the threshold used as a stop criterion (the lower the threshold, the more iterations are necessary to reach it).To test the robustness of our method, we amplified the deformation 40 times (thus with an initial precision of 20m instead of 0.5m) and still obtained an excellent result as shown in figure 9. We could even obtain a correct registration with an amplification of the real drift up to 80 times.

CONCLUSION AND PERSPECTIVES
The purpose of our approach was to define a process capable of registering mobile laser data on a geometrical database by taking into account the non-linearity of the deformations in time.A registration algorithm was set up based on a point to plane ICP method and a piecewise linear deformation model according to time.The obtained results are satisfactory because the evaluation of the robustness shows that the algorithm can register drifts up to 80 times higher that of our system which guarantees its reliability.The registration method described in this article is easy to parametrize because it only has two influential parameters: the rigidity term λ rigid and the maximal matching distance dmax which are both simple to interpret.The method is also generic as it is applicable to both 2D or 3D models, and because the hypotheses on the real drift are not too limiting; the most binding one is probably the requirement for a highly accurate orientation as it is not re-estimated.Finally, the proposed algorithm is sufficiently fast for our needs as it takes 182 s to register data acquired in 3 minutes.In the context of a production field, we can thus hope to process data acquired in the day time during the following night.
Our main leads for improvement are: -Matching the points with other types of objects (ground or other urban objects in addition to façades) to increase the robustness of our method.
-Evaluating accuracy in terms of repeatability of the method.
-Using the trajectory resulting from registration to re-estimate the orientation of the vehicle to remove the major limitation of current usability (precision of the orientation).
are: c + (t) the index of the first control time after t, c − (t) the index of the first control time before t and α(t) = (t − T c − (t) )/∆t the interpolation coefficient.The unknowns that define the drift are: δ c + (t) the drift at the control time T c + (t) and δ c − (t) the drift at the control time T c − (t) .

Figure 1 :
Figure 1: Cutting of the trajectory in different control time Tc.nents of the vectors δc which define the drift by equation (2).It allows to obtain the new trajectory of the vehicle by equation (1) hence, allowing for the complete cloud registration by application of the drift for every point.In general, in this article, trajectory registration T rajrec will always be defined by T rajrec = T rajinit + D δ (t) where T rajinit is the initial trajectory and δ the vector concatenating the drift δc for each control time Tc and which defines it in a differential way with regard to the initial mobile data (cf.Figure2).It particularly allows us to define a simple distance between two trajectories defined by δ i and δ j which we shall call "average drift" and denoted as DM :

Figure 2 :
Figure 2: Illustration of the distance d separating the drift δ n c at control time Tc between two mobile data for the average drift computation DM .

Figure 3 :
Figure 3: Probability of a laser point belonging to a vertical plane.

Figure 4 :
Figure 4: Binary threshold.Points belonging to a façade in green and points not belonging to a façade in black.
11) where A def is a matrix of ((NT c − 1) × NT c ) 3 by 3 blocks.The block of coordinates (c, c ) is Id if c = c, −Id if c = c + 1 and zero otherwise.

Figure 5 :
Figure 5: Illustration of the selection point process.(a) All façade points selected by the local geometrical descriptor.(b) Ray-tracing step: a point is selected only if the beam intersects a primitive.(c) Normal compatibility step: a point is selected if its normal is consistent with the primitive normal.(d) Distance acceptance: a point is selected only if its distance to the primitive is lower than a threshold value.
Figure 6: Overview of the method.

Figure 7 :
Figure 7: Overview of the geometric descriptor.

Figure 8 :
Figure 8: Zoom of part of the geometric descriptor in the scene emphasizing other vertical objects than building façades (poles, trees, panels, etc.).

Figure 9 :
Figure 9: Initial (top) and final (bottom) steps of the registration of an artificially degraded terrestrial laser point cloud with around 20 meters precision on three different area of interest.