A Low-Cost Consistent Vehicle Localization Based on Interval Constraint Propagation

,


Introduction
Using sensory information to locate the robot in its environment is one of the most fundamental problem providing a mobile robot with autonomous capabilities.A vast number of works [1,2] dedicated to such problems use both proprioceptive and exteroceptive sensors.Proprioceptive sensors such as odometers and gyrometers are used to calculate the elementary movements, which are used to estimate the robot pose (position and orientation).However, this generates cumulative errors as the robot moves [3,4].To cope with this problem, exteroceptive sensors (lasers, sonars, GPS, cameras, etc.) are used to gather information either from the surrounding environment or from a global reference, aiming at eliminating the accumulated errors and improving the estimation of the robot pose [5,6].
Probabilistic techniques have long been used in solving robotic localization and mapping problem.The most commonly used methods are Kalman filtering and Particle filtering [7,8].These methods associate a probability to a position, but nothing ensures that the robot is indeed in the position with the highest probability.It has long been noticed by the research community that probabilistic methods suffer the inconsistency problem [9].It is due to the fact that they are based on the assumption that the sensor noises are described by probability density function.However, in real world, many sensors do not possess Gaussian noise model or their distributions are simply not available [10].
Interval analysis [11] is an alternative and less known method which allows us to solve nonlinear problems in a guaranteed way.Instead of making hypothesis on the probability distribution of sensor noises, interval methods take the assumption that all the noises are bounded within known limits.This seems to be a realistic representation as most sensor manufacturers provide the maximum and minimum measurement errors under suitable working conditions.These extreme values of error can then be regarded as the error bounds.Interval based algorithms can be used to recursively propagate such bounded errors by using consistency techniques and systematic search methods.Contrary to probability methods, interval methods provide guaranteed sets which enclose the real state, without losing any feasible value.
Interval methods have achieved promising results in parameter and state estimation tasks [12][13][14], and they found their way to the mobile robotic localization and mapping area.Instead of considering Gaussian noise, all model and measurement noises are assumed to be bounded.The aim is to guarantee the configurations of the robot pose that are consistent with the given measurements and noise bounds.Kieffer [15] and Jaulin [16] provided simulation results for a robot equipped with a belt of on-board ultrasonic sensors.The feasibility of interval methods on localization applications had been shown.Followed-up researchers focused on real-time implementation of such localization technique.Kieffer [17] and Seignez [18] presented results for a mobile robot navigating in an indoor environment, with odometers mounted on each rear wheel to measure the motion and sonars located in different directions to perceive the environment.The ability of interval method to cope with erroneous data and to obtain accurate estimations of the robot pose was demonstrated.Lambert [19] extended such work by providing results for an outdoor vehicle equipped with odometers, gyro, and GPS receiver.Comparison was made with the Particle Filter localization, showing the better performance of interval method in terms of consistency.Gning [2] and Kueviakoe [20] dealt with the outdoor localization problem in the framework of Interval Constraint Satisfaction Problem (ICSP).Those works used constraint propagation techniques to fuse the redundant data of sensors.Drevelle [21] uses relaxed constraint propagation approach to deal with erroneous GPS measurements.Bonnifait [22] combined constraint propagation and set inversion techniques and presented a cooperative localization method with significant enhancement in terms of accuracy and confidence domains.Experimental results illustrate that constraint propagation techniques are well adapted to real-time implementation and provide consistent result for localization.Some other researchers proposed specific contractors [23] or separators [24] to solve the localization problem.
Indeed, when considering consistency issue, interval methods are advantageous in vehicle localization.However, they are prone to large output imprecision, which can make them unsuitable for high level task.We improve on it and present an extension of others' works dealing with the localization problem by using ICP.Instead of using ultrasonic sensors [16], sonar [18,25], or GPS [19,26] as exteroceptive sensors, we propose to use a monocular camera.The aim is to achieve both consistent and low imprecision localization result.We propose to drive a vehicle embedding precise but expensive sensors around the environment in order to build up a map represented in an interval way.Then the proposed method enables a set or a single vehicle to navigate consistently in such environment using this map and low-cost sensors.By constructing the problem as an ICSP, the proposed method is able to handle both mapping and localization in a unified form using Interval Constraint Propagation techniques.
The paper is organized as follows.Section 2 introduces the basics of interval analysis.Section 3 illustrates how the mapping and localization problem can be formulated as an Interval Constraint Satisfaction Problem.Sections 4 and 5 present the simulation and experimental results in terms of mapping and localization.Section 6 draws a conclusion of the paper.

Overview of Interval Analysis and Constraint Propagation
Interval analysis and constraint propagation are the theoretical tools used in interval based methods for robotic localization and mapping.In this section, we give a brief overview of these tools.

Interval Analysis.
Interval analysis [11,27] is a numerical method which allows us to solve nonlinear problems in a guaranteed way.One of the pioneers of interval analysis, Ramon E. Moore, proposed to represent a solution of a problem by an interval in which the real solution is guaranteed.
An interval [] is a connected subset of R, defined by its lower bound  and upper bound : The width of a nonempty interval ) can be taken as an estimation of  and ([])/2 can be regarded as the estimation error [28].
Let IR be the set of the intervals of R. A  (also called V V) [x] ([x] ∈ IR  ) is a generalization of the interval concept.It is a vector whose components are intervals: For instance, the configuration of a vehicle's pose usually contains 3 parameters: position , position , and heading angle .Consequently, for vehicle localization, the solution is a three-dimensional box: The width of a nonempty box [x] can be computed by ([x]) = max =1... ([  ]).We define the volume of [x] as The volume of the box is usually used to evaluate the estimation uncertainty of a state vector [28].

Operations of Interval Arithmetic.
Rules have been defined to apply the basic arithmetical operations on intervals [27].Let us consider two intervals [] and [] and a binary operator ⬦ ∈ {+, −, ×, ÷}; the smallest interval which contains all feasible values for [] ⬦ [] is defined as follows: The set-theoretic operations can be applied to intervals.The  of two intervals is defined by Figure 1: Intersection and interval hull of interval boxes.
[x] The result of  is always an interval, but it is not the case for their Consequently, an V ℎ is defined as the smallest interval that contains all the subsets of [] ∪ [], denoted by For instance, the interval hull of [2,4] ∪ [5,8] is the interval [2,8].Figure 1 gives an example of s and  for 2 interval boxes; it is realized by applying the intersection and interval hull operator to each component of the box.

Inclusion Function. The image of a vector function 𝑓 :
IR  → IR  (defined by arithmetical operators and elementary functions) over an interval box [x] can be evaluated by its inclusion function  [] , whose output contains all possible values taken by (⋅) over [x] (see Figure 2): The simplest way to create an inclusion function is to replace all the variables and operators by their interval equivalents.
The resulting interval function is called "natural inclusion function".For instance, has the natural inclusion function: Inclusion function can also be used to represent equations defined by interval variables.Such equations, also called constraints, are the core of Interval Constraint Satisfaction Problems.

Contractor.
The concept of contractor is directly inspired from the ubiquitous concept of filtering algorithms in constraint programming.Given a constraint  relating a set of variables , an algorithm C is called a contractor (or a filtering algorithm) if C returns a subdomain of the input domain [x] and the resulting subdomain C([x]) contains all the feasible points (the gray domain in Figure 3) with respect to the constraint :

Interval Constraint Satisfaction Problem (ICSP).
An ICSP is a mathematical problem whose solution is the minimal domain of the variables satisfying all the constraints.

Interval Constraint Propagation (ICP).
To solve an ICSP, an Interval Constraint Propagation (ICP) algorithm iterates domain reductions until no domain can be contracted.To satisfy the set of  constraints, it removes, from the domain of the variable, every value that is not compatible with the constraints and the other variables.ICP reduces the size of the domains in a consistent way by repeating this removal operation.This solving process is called  and the corresponding algorithm (e.g., Forward/Backward) used for contraction is regarded as a .
Let us consider a simple ICSP instance which is defined by 3 variables and 2 constraints: The domains of , , and  are, respectively, [], [], and [].An inclusion function of ( 12) is The Forward/Backward Propagation (FBP), an ICP algorithm, can be applied to solve the ICSP.FBP executes the contraction process in two phases: firstly, the Forward propagation reduces the left terms of ( 13) via Secondly, Backward propagation reduces the right terms of ( 13) by Forward and Backward equations, obtained from the constraints, are computed one after the other (here we compute from (14a) to (15c)).When all the equations have been computed, the algorithm restarts from the beginning equation until the domains of [], [], and [] are no more contracted (or contracted less than a specified parameter).

Problem Statement.
Let us consider a vehicle navigating in a 2D environment.At time step , the vehicle pose is represented by a three-dimensional vector X  = (  ,   ,   )  with   ,   being the vehicle's position and   the orientation.The  stationary landmarks are represented by M = (L 1 , L 2 , ⋅ ⋅ ⋅ , L  )  .L  is a state vector defining the landmark position (see Section 3.3).For the sake of simplicity, we assume that the pose evolution follows V ℎ, where the current pose X  depends only on the very last pose X −1 and the current control input vector U  .The input vector U  consists of linear and angular elementary movements, which were collected by proprioceptive sensors (e.g., odometer and gyro).If these odometric measurements are available, the state vector X  can be evaluated through the dead reckoning function (also called motion model) expressed as follows: where   is the measurement noise affecting the odometric measurements.
The vehicle is equipped with exteroceptive sensors, recording information about the environment.At each time step , the vehicle observes a number of landmarks which can be regarded as a subset of the entire map; the measurement Z  depends only on the current vehicle pose X  and the map state M: where H(⋅) is generally a nonlinear function; it differs for different types of exteroceptive sensors.The output of H(⋅) is a geometric parameterization of the landmark (e.g., distance, angle, or pixel coordinates), and   is the measurement noise of the exteroceptive sensors which introduce uncertainty to the landmark state.
Interval methods are based on the assumption that the support of sensor noises is unknown but bounded by real intervals.Mathematically, ∀ > 0,   ∈ [w] and   ∈ [r], where [w] and [r] are real intervals defined by lower and upper bounds.Then the localization problem can be regarded as an ICSP: the set of variables is defined by the vehicle pose and the landmark position; the motion and observation functions (( 16) and ( 17)) define the constraints.ICP techniques allow us to propagate the interval uncertainties in a consistent way without any probability hypothesis or linearization process.As a result, all possible solutions are found and guaranteed.

Motion Model.
The motion estimation process uses the measurements of proprioceptive sensors to predetermine the localization vector [X  ].EKF use a probabilistic motion model to fuse sensor data [30].We make an extension of this model to obtain an interval based model defined by Seignez et al. [18]: where )  is the input vector.  and   represent, respectively, the elementary displacement and rotation.
The elementary movements can be deduced from raw odometric data (odometer and gyro) using an interval based static fusion method: where   and   are the odometer measurements of the left and right wheels.  and   represent the radius of the wheels. is the odometer resolution and  is the length of the rear axle.  is the gyro measurement.For further information about interval based odometric sensor integration, the reader might refer to [31].Once we have constituted intervals from sensor data, the vehicle pose [X  ] can be estimated by applying FBP algorithm (Section 2.6) over constraints deduced from the motion model.

Landmark Parameterization.
A landmark (or feature point) is a 3D point in the global world.Probabilistic methods generally represent landmark with an inverse depth parameterization [32].They model the uncertainty of each parameter by Gaussian distributions.However, it turns out to be not so efficient to represent the linear depth uncertainty of monocular vision.Interval analysis provides an easy and efficient way to parameterize landmarks.Each landmark L  is defined as a six-dimensional state vector: )  , which models the estimated landmark position at where Since all the parameters are represented by intervals, and [  ] can be initialized as [0, +∞], the landmark's uncertainty is modeled as an infinite cone which combines the vehicle pose uncertainty and the observation uncertainty.It is a realistic representation for the monocular vision uncertainty.The major advantage is that the initialization of [  ] is undelayed, guaranteed, and efficient for landmarks over a wide range as it always includes all possible value without any prior information.

Observation Model.
The observation of a landmark is the coordinates of the pixel where the landmark is projected on the image.In order to detect and extract landmark information from an image, we used Speed Up Robust Features (SURF) [33].The work [34] recommended it after a comparison between several algorithms, showing that SURF provide results with a very low error rate.The projection of the  ℎ landmark to the image frame can be formulated as follows: From ( 21), the landmark's position in the world coordinate O  can be expressed: In the camera coordinate O  , the landmark [L   ] will be seen in the position where [X  ] is the current vehicle pose.  and   are, respectively, the transformational matrix between the world and the camera coordinate and the robot and world coordinate, respectively.The camera does not observe [L   ] directly but its projection in the image frame (, V)  .According to the pin-hole camera model [35], the observation Z   of [L   ] on the image frame can be predicted by where ,   ,  V ,   ,  V are the camera intrinsic parameters.Equation ( 26) is also used as the projection function.
3.5.Data Association.Data association focuses on finding out the correspondence between the map and observations.Different from probabilistic method, which deals with uncertainty ellipsoid in projection procedure, interval method proceeds with bounding box.The matching process is carried out as follows: (i) Project the landmarks to the current image plane; discard those which lay out of the image bounds.Each landmark defines a rectangle searching area.(ii) In the searching area, search for the candidate feature that is matched to the landmark.We adopt Dan's method [36] which performs a two-stage matching process (considering both the Euclidean distance and dominant orientation information of feature descriptors) to provide a robust and accuracy matching result.For each pair of matched features, a Zero Normalized Cross Correlation (ZNCC) [37] score is computed in the mapping stage to help to evaluate the robustness of the landmarks.(iii) Filtering step is necessary to keep a desirable amount of matched points.The landmark uncertainty and ZNCC score can be used for the selection process.We make the matched results distributed uniformly over the image, providing enough parallax for the localization.

ICSP Formulation.
The data association process generates a set of 3D to 2D correspondences, which we can use to build the ICSP constraints.Let us consider the  ℎ landmark [L  ] that has been matched.The observation of The predicted position on the image plane, denoted by )  , is computed via ( 23), (24), and (26).The observation can be used to correct the system state through the predicted observation: Equation ( 27) creates a link between the prediction and observation data of the  ℎ landmark [L  ].This link (thanks to (23), (24), (26), and ( 27)) imposes constraints on [X  ] and [L  ], which can be used to update both the vehicle pose and landmark position.Based on the above derivation, an ICSP composed of 2 top level constraints and a set of 9 variables can be set up.For each matched landmark , the ICSP  (  ,   ,   ) is generated and defined as follows: (i) A set of 9 variables: (ii) A set of 9 domains: (iii) A set of 2 top level constraints: If the data association process found  matched landmarks, then we get a multiple ICSP to solve: Each ICSP contains constraints between the vehicle pose and a unique landmark position.The good point is that they are all linked together via the same variables   ,   , and   (the vehicle pose at current time ).This correlation makes ICP algorithms very powerful to jointly contract the domains of all variables.
3.7.System Overview.Figure 4 depicts the framework of the mapping stage.It is an interval SLAM process which is cast into two parts: motion and observation.In the motion part, the preliminary pose of the vehicle is predicted by fusing the measurements of the odometers and gyro with bounded error model; in the observation part, features are firstly extracted from the image, data association task aims at finding out the correspondences between the features and the map, new features will be initialized as new landmarks and added to the map, and old ones will be processed to generate new ICSPs; then these ICSPs are jointly solved by an ICP algorithm, which correct the vehicle pose and update the map.
(1) while Localization is required do for  ← 1 to  do (8) end for (10) ] The localization stage is quite similar to the mapping stage.There are two main differences: high cost sensors are no longer used and no new feature is added to the map.Algorithm 1 gives the pseudocode of our localization method:

Simulation Results
A simulation experiment is set up to evaluate the performance of our proposed ICSP based mapping and localization method.The vehicle starts from the origin (see Figure 5) and moves straight forward in the positive direction of x-axis with constant linear and angular velocity ( = 0.1 ⋅  −1 ,  = 0).5 landmarks are positioned in the surrounding environment without any prior knowledge of their positions.At each time step, the vehicle detects the 5 landmarks with a simulated camera model [,   ,  V ,   ,  V ] = [40,8,8,320,240]) and focuses on estimating their position.The experiment is first carried out without odometric sensor noise (the vehicle pose is perfectly known) to verify the feasibility of our method.This condition (no odometric noise) is then released in the experiment presented in Section 4.3.

Landmark Initialization.
At time step  = 1, the 5 landmarks are first detected and the initialization procedure is executed.Figure 5(a) shows the top view of the initialization result of landmarks (see Section 3.3).Each landmark's uncertainty is initialized as an infinite cone because of the lacking information about the landmark depth.As there is no uncertainty on vehicle pose, the landmarks are well initialized and the opening angles which indicate the observation uncertainty are small (due to the bounded observation noise which is 1 pixel).If the vehicle has an uncertainty on its orientation (the heading angle is not perfectly defined: , then the cones of uncertainty are much larger than the previous ones (see Figure 5(b)).This is reasonable since the cone uncertainty is the fusion of observation uncertainty and vehicle pose uncertainty.Because our bounded error hypothesis and the use of interval analysis, the initialization result is guaranteed to contain the true landmark position.

Landmark Uncertainty Evolution.
As the vehicle moves, the landmarks are repeatedly observed with different parallaxes.By building and solving the associated ICSP (see Section 3.6), each of the landmark's parameters can be estimated.Figure 7  20.The parallax  (defined as the angle difference between the initial observation and current observation, see Figure 6) is displayed as the title of each subfigure.With the estimation proceeds, the landmark uncertainty estimate improves.As we can see, the bigger the parallax is, the better the contraction of the landmark uncertainty will be.When enough parallax is eventually available, the infinite interval uncertainty is significantly reduced.The uncertainty of landmark 3 keeps infinite because it locates on axis  and no parallax has been detected during the horizontal translation.
To visualize the evolution of a landmark state, it is necessary to convert the 6-dimensional representation to a global 3D point using ( 21): (  ,   ,   ,   ,   ,   )  → (  ,   ,   )  .The interval uncertainty (IU) can be evaluated by computing the box volume [31]: As the depth of each landmark is initialized as [0, +∞], the , , and  coordinates have infinite value, and the uncertainty turns out to be infinite at the first hand.After multiple observations (at different times) from different parallaxes, the landmark's state is updated.Figure 8 depicts the evolution of the uncertainty associated with the landmarks 1, 2, 4, and 5.The 4 landmarks encounter different observation parallaxes, so the contraction of their uncertainties is not simultaneous.However, they exhibit the same performance: Step:1 Step:5 Step:10 Step:20 the interval uncertainty of each landmark is well propagated and decreases exponentially with the vehicle navigating in the environment.

Estimation Accuracy.
In real world, the vehicle pose is always accompanied with uncertainties due to the cumulative odometric noise.The vehicle pose uncertainty will affect the estimation accuracy of the landmarks (a smaller vehicle pose uncertainty means higher landmarks estimation accuracy).In our simulation, the error bound of the odometric measurement is supposed to be proportional to elementary displacement, such that ([U  ]) ≤  ⋅ U  + Γ, Γ = [0.0001,0.01 ∘ ]  , with U  being the real odometric measurement,  denoting the ratio value (bigger  value indicates a relative bigger uncertainty of odometric measurements), and Γ accounting for the steady system error.The average position uncertainty of the 4 landmarks has been computed at each time step.Figure 9 displays the results with different  value ( = 0.01, 0.1 and 0.2).As it is shown, the estimation accuracy decreases when  value becomes bigger.This result supports the fact that the landmark estimation accuracy is strongly correlated with the vehicle pose uncertainty.Bigger  odometric uncertainty will result in a worse estimation of vehicle pose; as a result, the landmark estimation accuracy decreases.

Experiment Set-Up.
To evaluate the performance of our method in realistic circumstance, we used the experimental data set from Institute Pascal [38].It contains multisensory timestamped data which can be used in a large variety of applications.It includes data from low-cost sensors, ground truth, and multiple sequences as well as data for calibration.Data were collected on the VIPALAB platform (see Figure 10(a)), driving on the PAVIN track (see Figure 11(a)), an experimental site located on the French campus of Blaise Pascal University.The environment is composed of scaled street, road markings, functional traffic lights, painted walls, buildings vegetation, and so on (see Figure 11(b)).The platform was equipped with multiple sensors (see Figure 10(b)).In our experiment, we need to use 4 sets of timestamped sensor data: (i) Odometers: each rear wheel is equipped with an odometer in the form of a ring gear providing 64 "top" per wheel revolution.(ii) Gyro (Melexis MLX90609-N2): it measures the rotational speed in an inertial reference system.Vibrating silicon structures, that use the Coriolis force, output the yaw rate from which we can deduce the elementary rotation.(iii) Camera (FOculus FO432B camera + Pentax C418DX lens): it provides information about the perceived environment.We use the camera which located in the left-front of the vehicle and looking ahead.(iv) RTK-GPS: the data acquired with an embedded ProFlex500 RTK-GPS receiver from NavtechGPS coupled with a Sagitta Magellan GPS base station are considered as the ground truth.This system provides an accurate absolute localization measurement to within 2 cm (±1 cm).

Mapping Stage.
A mapping stage is first conducted by our method in order to build a map of the environment.The vehicle follows 6 simple consecutive loops around the PAVIN environment.Figure 12(a) shows the reference trajectory that the vehicle passed by and Figure 12(b) details the 6 loops in order.As we have discussed in Section 4.3, the estimation of landmark is strongly correlated with the vehicle pose uncertainty.So it is necessary to maintain the pose uncertainty in a reasonable size during the mapping stage in order to pursue a precise map.
Figure 13 shows the vehicle position uncertainty (calculated by (([])/2) ⋅ (([])/2)) with and without RTK-GPS correction during the mapping process.As we can see, without correction, the position uncertainty is cumulative and increases very quickly until loop closure is realized.This is because when navigating, the odometric noise is cumulative since there is no absolute measurement.Interval analysis is a pessimistic method maintaining all possible solutions, the cumulative errors will result in interval expansion, and the pose uncertainty thus becomes larger and larger.This is a hinder to pursue a good map.To overcome this problem, our method makes use of the RTK-GPS measurement, and the GPS error bounds are characterized as the maximum imprecision (≤ 2) of the GPS receiver.The cumulative errors can be eliminated and a precise estimation of the vehicle position is obtained (see Figure 13(b)).The result is assumed to be consistent since only ICP technique was used.
At the end of the loop, 3450 images are processed and a map of 1140 landmarks is created.Figure 14 shows part of the map projected on  −  plane.The black bounding boxes denote the estimated landmark's position.The average size of these bounding boxes is shown in Table 1, which can be regarded as a factor to evaluate the quality of the map.The  coordinate gets much higher precision than  and ; this is because the estimation of  is only correlated with the observation elevation angle which is invariant w.r.t the vehicle's heading angle, while  and  are affected by the heading uncertainty.A good way to improve the map is to get a better estimation of the vehicle's orientation.Note that the map building process could be done offline such that computation intensive ICP algorithms and manually guidance could be used to obtain a better map.

Localization Result.
The localization stage benefits from the map data which offer position constraints.With the built map, the vehicle seeks consistent localization using the   Our proposed localization method is firstly tested on Sequence 1, and the vehicle follows a nearly 200m trajectory; see Figure 15.The blue path is the reference trajectory from RTK-GPS, the red path is the dead reckoning result, and the black squares are the localization boxes output by our proposed interval method.The dead reckoning result is obtained via a geometric evolution model using the odometer and steering angle data.It can be seen from the figure that the localization boxes followed the reference trajectory in a consistent way (intersect with the blue line).Table 2 (DR: dead reckoning; IM: interval method) presents the localization result of both methods at  = 200 and  = 400 and at the end of the track.By fusing the map, our interval method gives a consistent estimation of the vehicle pose: the localization boxes well enclose the ground truth.
To compare the vehicle position estimated by our proposed method and the dead reckoning, the root of sum square error (RSSE) is computed at each time step.It is expressed by the following formula: where   () and   () are the estimation error of mid([]) position and mid([]) position at time step .RTK-GPS data is regarded as the ground truth.
Figure 16 shows the RSSE results of both methods.As it could be expected, the cumulative odometric error of dead reckoning is significant and caused an increasing discrepancy (at the end, the discrepancy reaches about 3 meters).Our method takes advantage of the map data and corrects the odometric error, providing a very small discrepancy.In order to verify the consistency of the localization results, interval error is defined by the upper and lower bounds of the estimated state minus the corresponding reference (real value), mathematically, where [  ,   ] is the estimated state of variable  at time step  and  *  is the corresponding reference.The estimated state is said to be consistent if and only if 0 ∈ [  ()] is a true statement.Moreover, an estimation method is consistent and precise if ∀ ∈ {1, 2, ⋅ ⋅ ⋅ , ∞}, 0 ∈ [  ()] is always satisfied and the interval error width ([  ()]) is thin.
We compute the real-time interval error of our localization result.At each time instant  when the new captured image had been processed,   () and   () are calculated, respectively.Figure 17 depicts the interval corridors consisting of the upper and lower bounds of   .The zero line is well included by those corridors, proving that the localization results are consistent all along the track.This is a significant result in vehicle localization field where safety is a crucial issue.Similar results are obtained when performing the localization process on Sequences 2 and 3 with the same map; the output localization boxes by our method and the corresponding reference trajectory are displayed in Figure 18.It shows the robustness of our method providing consistent  14.4 cm for .The average RSSE value is about 9.66 cm (less than 10 cm).Note that these localization results are consistent, and the localization boxes are guaranteed to enclose the real positions of the vehicle.

Discussion.
In this paper, we present a complete methodology for localizing a vehicle within a prebuilt map.We bring forward a two-stage framework (mapping and localization) to solve the problem.Closer approaches have already been proposed by different researchers [39][40][41].They all involve a

Figure 2 :
Figure 2: Images of an interval box by function  and its inclusion function  [] .

Figure 3 :
Figure 3: Contraction of a box by a contractor C.
(i) Line (4): motion estimation.The current vehicle pose is calculated based on the motion function  [] and input vector [U  ]. (ii) Line (5): image processing.A set of SURF   are extracted from the new image   .(iii) Line (6): data association.The correspondence between the features and the map is found.(iv) Line (8): ICSP generating.New ICSP are generated based on the matched landmarks.(v) Line (10)-(11): Solving.]ICSP  is composed by the ICSPs and solved by an ICP algorithm.

Figure 9 :
Figure 9: Average landmark uncertainty under different  values.

Figure 16 :Figure 17 :Figure 18 :
Figure 16: The error of localization results by DR and IA.

Figure 19 :
Figure 19: Interval error width of localization result.
of variables {V 1 , V 2 , .., V  }, (ii) a set  of domains { 1 ,  2 , ..,   }, such that for each variable V  , a domain   with the possible values for that variable is given;   is an interval or union of intervals, (iii) a set  of  constraints { 1 ,  2 , ..,   }; each constraint   defines the relationships of a number of variables from ; e.g.,  1 coordinates [  ], [  ], and [  ] represent the optical center of the camera when the landmark was seen for the first time, [  ] and [  ] represent azimuth and elevation angle for the ray which traces the landmark, and [  ] is the depth of the landmark.[m]([  ], [  ]) is a unitary vector pointing from the camera to the landmark [L  ]:

Table 1 :
Average bounding boxes size.

Table 3 :
Mean localization interval error and RSSE.
visual learning step to reconstruct a map of the environment, and they use this map for localization and navigation task.