Sergio A Rodríguez
email: sergio.rodriguez@hds.utc.fr

Vincent Frémont
email: vincent.fremont@hds.utc.fr

Philippe Bonnifait
email: philippe.bonnifait@utc.fr

Véronique Cherfaoui
email: veronique.cherfaoui@hds.utc.fr

Multi-Modal Object Detection and Localization for High Integrity Driving Assistance

Keywords: Multi-modal perception, Visual odometry, Object tracking, Dynamic map, Intelligent vehicles Sensor Calibration

Much work is currently devoted to increasing the reliability, completeness and precision of the data used by driving assistance systems, particularly in urban environments. Urban environments represent a particular challenge for the task of perception, since they are complex, dynamic and completely variable. This article examines a multi-modal perception approach for enhancing vehicle localization and the tracking of dynamic objects in a world-centric map. 3D ego-localization is achieved by merging stereo vision perception data and proprioceptive information from vehicle sensors. Mobile objects are detected using a multi-layer lidar that is simultaneously used to identify a zone of interest in order to reduce the complexity of the perception process. Object localization and tracking is then performed in a fixed frame which simplifies analysis and understanding of the scene. Finally, tracked objects are confirmed by vision using 3D dense reconstruction in focused regions of interest. Only confirmed objects can generate an alarm or an action on the vehicle. This is crucial to reduce false alarms that affect the trust that the driver places in the driving assistance system. Synchronization issues between the sensing modalities are

Introduction

Advanced Driver Assistance Systems (ADAS, an acronym list is given at the end of the paper) can improve road safety through their obstacle detection and avoidance functions. For these functions, the location and the speed of nearby mobile objects are key pieces of information. In the literature, a number of different approaches have been applied to problems of object localization and tracking. Robotics approaches have been used to distinguish the static part of the environment [START_REF] Durrant-Whyte | Simultaneous localisation and mapping (slam)[END_REF] and to simultaneously detect moving objects [START_REF] Wang | Simultaneous localization, mapping and moving object tracking[END_REF]. Leibe et al. have presented in [START_REF] Leibe | Dynamic 3d scene analysis from a moving vehicle[END_REF], a stereo vision strategy for obtaining a 3D dynamic map using a Structure-from-Motion technique and image object detectors. Using lidar information only, it is possible to estimate the egomotion and to detect mobile objects through a dense 3D grid-map approach [START_REF] Miyasaka | Ego-motion estimation and moving object tracking using multi-layer lidar[END_REF]. In contrast, for [START_REF] Broggi | A new approach to urban pedestrian detection for automatic braking[END_REF] and [START_REF] Labayrade | Cooperative fusion for multi-obstacles detection with use of stereovision and laser scanner[END_REF], real-time sensor-referenced approaches (i.e. ego-localization is not considered) are presented using multisensor systems showing the complementarity of lidar and vision systems in automotive applications.

A world-centric approach presents interesting properties once the ego-localization is estimated accurately (up to 1 cm per Km/h). The tracking performance can be improved, since the dynamics of the mobile objects are better modeled. This sort of approach simplifies the understanding of the scene and the ADAS implementation, and is also well adapted to cooperative applications [START_REF] Safespot | Cooperative vehicles and road infraestructure for road safety[END_REF] (e.g. vehicle-to-vehicle communication). The present study addresses the problem of ego-localization and object tracking in urban environments.

Ego-localization can be achieved using proprioceptive and exteroceptive sensors [START_REF] Cappelle | Multi-sensors data fusion using dynamic bayesian network for robotised vehicle geolocalisation[END_REF]. GPS is an affordable system that provides 3D positioning. Unfortunately, GPS performance can decrease significantly in urban environments because of multi-paths and satellites outages. Dead-reckoning is a complementary solution which can be used when GPS information is unreliable. Stereo Vision Systems (SVS), often used for 3D reconstruction, detection and recognition tasks, are also useful for deadreckoning (also called 3D ego-motion estimation) [START_REF] Comport | Accurate quadrifocal tracking for robust 3d visual odometry[END_REF].

Object tracking for ADAS is still an active research domain. Urban environments are characterized by com-plex conditions: moving and static objects, mobile perception, varied infrastructures. Object representation [START_REF] Petrovskaya | Model based vehicle tracking in urban environments[END_REF][START_REF] Nashashibi | Vehicle recognition and tracking using a generic multisensor and multialgorithm fusion approach[END_REF], association methods [START_REF] Bar | Multitarget/Multisensor Tracking: Applications and Advances[END_REF], motion model and tracking strategies [START_REF] Liggins | Handbook of Multi-Sensor Data Fusion[END_REF] are key points requiring particular attention.

In this work, we study a tightly coupled multi-modal system able to provide a 3D local perception of the vehicle's surrounding environment in a world-centric frame, as depicted in Fig. 1. The modeled environment is composed of static and moving objects and a zone of interest localized in front of the vehicle. Our contribution lies in the estimation of the dynamics (location and speed) of the surrounding objects in order to build a dynamic map, and in ensuring the map's integrity by using different, independent sensing principles. A dynamic map is composed of a list of the states of tracked objects together with the changing vehicle dynamics in the 3D scene. A particular difficulty faced in this study is the use of asynchronous modalities which are sampled independently. To address this issue, we propose a multi-sampled strategy.

The multi-sensor system we have designed makes use of essential information provided by an SVS coupled with a Multi-Layer lidar (denoted ML lidar) and proprioceptive sensors (i.e. wheel speed sensors and a yaw rate gyro).

The overall system design and architecture of the proposed system are illustrated in Fig. 2. First, nearby objects are detected based on ML lidar data. The SVS and the proprioceptive vehicle sensors are used simultaneously to estimate the 3D ego-localization of the vehicle. Subsequently, the detected objects are localized and tracked w.r.t. a world reference frame. Finally, tracked objects are transferred to a vehicle-centered frame in order to be confirmed by the SVS, by taking into account the different sampling times of the two sensing modalities. Confirmed tracks that are declared as verified then become the input into an ADAS for the detection of possible collisions.

In this article four main topics are addressed: 3D ego-localization, exteroceptive sensor calibration (i.e. data alignment), object localization and tracking, and visual track confirmation. First, a detailed description of the embedded multi-sensor system setup is given in section 2. Next, the perception function is presented and experimental results are discussed. Section 3 is devoted to 3D ego-localization using vision and proprioceptive sensors. The extrinsic calibration procedure is described in detail in section 4. Object localization and tracking are examined in section 5. Visual track confirmation and synchronization issues are discussed in section 6. Finally, the conclusion and perspectives of this work are presented.

Multi-Modal Perception System

Experimental Set-up

An experimental vehicle belonging to the Heudiasyc Laboratory was used for the implementation and validation of the global perception system in real-life conditions. As illustrated in Fig. 3, the vehicle is equipped with a 47cm-baseline Videre SVS. This SVS is composed of two CMOS cameras, with 4.5mm lenses configured to acquire 320x240 gray-scale images at 30 fps.

The vision system provides essential information for ego-motion estimation and visual track confirmation. This system covers 45-degrees with an effective range up to 50 m in front of the vehicle.

Multi-layer Lidar

Stereo Vision System WSS + Gyro Fig. 3 The experimental vehicle with the stereo vision system and the IBEO Alasca XT. (Velodyne is not used in this study)

A large surrounding region is covered by an IBEO Alasca XT lidar which transmits a sparse perception of the 3D environment at 15 Hz. Installed at the front of the vehicle, this sensor emits 4 crossed-scan-planes covering a 3.2°field of view in the vertical direction and 140°in the horizontal direction with an advertised 200m range. The ML lidar technology is particularly suitable for automotive applications since detected objects are not easily lost under pitch vehicle movements, in contrast to single-raw range-finders. Additionally, the 4-layer configuration allows the extraction of 3D scene structure attributes such as camber and curbs, since two layers are angled downwards.

The exteroceptive sensors (i.e. SVS and ML lidar here) were set up to ensure a complete Field-Of-View (FOV) overlap, as depicted in Fig. 4. The redundant sensing coverage is intended to achieve reliable object detection and tracking.

A CAN-bus gateway provides the speed of the rearwheels (WSS) and the yaw rate of the vehicle (from the ESP). These sensors deliver complementary information for the vehicle localization task.

Frames

The geometrical set-up of the multi-sensor system is formalized through four local sensor frames as illustrated in Fig. 5 and6. The stereo vision camera system is represented by two classical pinhole camera models (i.e. with focal length f and the principal point coordinates [u 0 v 0 ] T in pixels, assuming no distortion and zero skew [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF]). The two cameras are rigidly linked and horizontally aligned at a baseline distance, b. As illustrated in Fig. 5, the reference frame of the SVS, denoted S, is located midway between the two cameras. Information referenced w.r. In order to sense information in a common perception space, the relative pose of the sensors frames (i.e. SVS and ML-lidar frames) has to be estimated through an extrinsic calibration procedure (Sensor Calibration module in Fig. 2). Extrinsic parameters can be obtained using the left camera images and the ML-lidar measurements, given that the frame transformations between the cameras composing the SVS are known (see Fig. 5). This process is presented in detail in section 4. The complete frame transformation from the lidar frame L into the vision frame S is denoted L [q, t] S and composed of a unit quaternion and a translation vector.

Additionally, the gyro and wheel speed sensor measurements are referenced with respect to the midpoint of the vehicle's rear-axis.

The ego-frame E (X-Right, Y-Down and Z-Front) is rigidly linked to the vehicle (i.e. body-frame). As this frame is chosen tangential to the road plane, it simplifies the geometrical information analysis for the visual lidar track confirmation (see Fig. 6).

3D Ego-Localization

3D ego-localization consists in estimating the 3D pose of the vehicle as a function of time with respect to a static frame lying, for instance, in a plane tangential to the earth's surface. Odometry methods using stereo vision systems can provide very precise 3D pose estimations based on multiple-view geometrical relations (i.e. 4-views or quadrifocal constraints) [START_REF] Comport | Accurate quadrifocal tracking for robust 3d visual odometry[END_REF]. However, visual odometry may require considerable computation time. Here, 3D vehicle ego-localization is estimated using sparse visual odometry for rapid processing, aided by the embedded proprioceptive sensors of the vehicle [START_REF] Rodriguez | An experiment of a 3d real-time robust visual odometry for intelligent vehicles[END_REF].

Visual odometry aided by CAN-bus sensors

The ego-motion of the vehicle is defined by an elementary transformation (rotation-translation composition, 6 degrees-of-freedom) performed in an interval of time ∆t. This 3D motion is represented by an axis-angle rotation and a translation vector, S(t-1) [∆ω, ∆v] T S(t) . First, an initial planar motion guess is computed using the proprioceptive sensors in ∆t. Secondly, a 3D visual motion estimation algorithm is initialized with this motion guess and is then iteratively refined (see Fig. 7). Let C(t) be the center of the body frame defined at time t -∆t. If the sampling frequency of the gyro and the WSS is high enough (about 40 Hz), the wheel speed is almost constant and the planar ego-motion can be approximated by the arc of a circle. As illustrated in Fig. 8, the planar ego-motion of the vehicle is modeled as follows [START_REF] Dudek | Springer Handbook of Robotics, chapter Inertial Sensors, GPS, and Odometry[END_REF]:

∆ω0 = 0 ∆θ 0 ∆v0 = ∆s • sin(∆θ/2) 0 ∆s • cos(∆θ/2)
where ∆θ is the angle obtained by integrating the yaw rate, and ∆s is the integrated rear-wheel odometry in meters. ∆ω 0 is a vector representing the axis-angle rotation of the vehicle motion and ∆v 0 is a vector representing the estimated displacement of the rear wheel axis center.

Using successive stereo image pairs, we obtain a set of n tracked stereo feature points denoted, p= {p 1 , ..., p n } and p = {p 1 , ..., p n }, (i.e. the corresponding left-hand and right-hand features respectively) and their corresponding optical flow constituting the image motion. For this purpose, a set of stereo feature points denoted, p * = {p * 1 , . . . , p * n } and p * = {p * 1 , . . . , p * n }, is extracted using Harris features [START_REF] Harris | A combined corner and edge detector[END_REF] with a ZNCC (Zero-mean Normal Cross Correlation) criterion and image constraints (disparity and epipolar constraints) [START_REF] Scharstein | A taxonomy and evaluation of dense two-frame stereo correspondence algorithms[END_REF]. The set of stereo feature points, p * and p * , is then tracked over time using Lucas-Kanade method [START_REF] Bouguet | Pyramidal implementation of the lucas kanade feature tracker description of the algorithm[END_REF].

Alternatively, a stereo feature can be "predicted" after a 3D motion of the vision system by using a warping function [START_REF] Comport | Accurate quadrifocal tracking for robust 3d visual odometry[END_REF] based on geometrical constraints. These constraints are induced by the SVS configuration and the static scene assumption. The idea is to predict the sets p and p as a function of the sets p * and p * of stereo features at time t -1 (i.e. t -∆t) and the vehicle motion encapsulated in the trifocal tensors l T jk i and r T jk i [START_REF] Shashua | On the trilinear tensor of three perspective views and its underlying geometry[END_REF]. Consider a stereo feature pair, {p * , p * } ∀ p * ∈ p * , p * ∈ p * . Thus, their predicted image position is given by:

p p = p * l j l T jk i p * l j r T jk i ( 1
)
where l j and l j are respectively the left and right image lines passing through the image points p * and p * , and perpendicular to the epipolar line. l T jk i , is the trifocal tensor composed by the stereo image pair at time t -1 and the left image at time t. The second tensor, r T jk i , is composed by the stereo image pair at time t -1 and the right image at time t. It is worth recalling that the tensors r T jk i and l T jk i are nonlinear functions of the SVS parameters (i.e. intrinsic and extrinsic) and of the vehicle's motion S(t-1) [∆ω, ∆v] T S(t) . However, since urban scenes are not composed exclusively of static objects, the static scene assumption is not respected. To address this issue, a robust iterative nonlinear minimization is performed according to the following criterion:

min S(t-1) [∆ω, ∆v] T S(t) ( ) = k i=1 W ||p i -pi || + ||p i -p i || (2)
where k, is the number of tracked stereo feature pairs and p i and p i are the left-and right-tracked stereo features at time t. pi and p i are the left and right stereo features at time t warped by the estimated motion (i.e. S(t-1) [∆ω, ∆v] T S(t) ) and the warping function stated in Eq. 1. W is the weighting matrix, estimated by an M-estimator function [START_REF] Stewart | Robust parameter estimation in computer vision[END_REF] updated using the Iterative Re-weighted Least Squares algorithm (IRLS).

This robust minimization converges into a solution by rejecting the feature points that are mainly generated by mobile objects. The convergence is guaranteed if at least 50% of the stereo feature points correspond to static objects (i.e. the environment). The criterion of Eq. 2 is minimized by using the Levenberg-Marquardt Algorithm (LM) in an IRLS loop [START_REF] Stewart | Robust parameter estimation in computer vision[END_REF]. The convergence speed of the LM algorithm is increased using the planar ego-motion S(t-1) [∆ω 0 , ∆v 0 ] T S(t) , from the CAN-bus sensors. This information provides a close initialization guess, and thus helps to reduce the number of iteration cycles.

Once the ego-motion parameters are estimated, the vehicle position fix can be obtained by integrating all the successive estimates. With the aim of reducing noise before integrating the ego motion S(t-1) [∆ω, ∆v] T S(t) , a filter is implemented for the estimated parameters. To this end a constant accelerated model is considered, since the ego-vehicle can experience significant speed changes in braking situations. Regarding the axis-angle formalism for the attitude changes, ∆ω, it will be remarked that for a non-holonomic moving vehicle, the variations of the attitude parameters are smooth, except for the yaw angle. The latter assumption does not hold for extreme driving situations (e.g rollover). Under these assumptions, the linear speed and the attitude changes are filtered using a single first-order model that reconstructs the derivatives of the ego-motion parameters. This approach is useful for predictive filtering.

Let x(t|t), be the state vector and A(t) the state transition matrix defined as follows:

x(t|t) =    ω(t) v(t) Ω(t) a(t)    A(t) = I6×6 ∆t • I6×6 06×6 I6×6 (3)
where v(t) and a(t) are the linear speed and acceleration. ω(t) represents the derivatives of the axisangle parameters which have been considered linear to a drift Ω(t), assumed to be constant over time. In this model, the sampling period, ∆t, is not constant. The covariance of the model, denoted Q, is chosen taking into account the errors due to the model approximation and, the covariance noise R is estimated considering a zero-mean Gaussian white noise.

Through the discrete Kalman filter equations [START_REF] Durrant | Springer Handbook of Robotics, chapter Multisensor Data Fusion[END_REF], the predicted state (a priori), x(t|t -1), and its covariance, P(t|t -1), are estimated.

Once a new ego-motion estimation (i.e. state observation), denoted x(t), is obtained, the predicted state and its covariance are corrected providing the a posteriori state, x(t|t), and covariance, P(t|t).

The filtered ego-motion which is reconstructed from the state and the observation model, H, is given by:

S(t-1) [∆ω, ∆v] T S(t) = H • x(t|t -1) ( 4
)
with H = ∆t.I6×6 06×6 Fig. 9 World (W) and Ego (E(t)) and SVS (S(t)) frames at 3 sampling times

Making use of the smooth estimates provided by the predictive filter using Eq. 4, the vehicle localization is estimated in the world frame. For this, let W be the world reference frame and E(t), the ego frame which is linked to the vehicle as illustrated in Fig. 6 and9. The vehicle pose at time t is denoted W E(t) = W [q(t), p(t)] T and is represented in the world frame by its attitude -W q(t) a unit quaternion -and its position -W p(t) -a vector in meters. It is obtained as follows:

W q(t) = E q S S q S(t) and W p(t) = E q S S p S(t) E qS (5)
where t denotes the temporal frame index and has been conveniently omitted for t = 0, denotes the quaternion multiplication operator, q represents the corresponding quaternion conjugate and the underlined vector (e.g. p) denotes an expanded form (i.e. p = 0, p T T ) for the use of the quaternion multiplication [START_REF] Hanson | Visualizing Quaternions[END_REF]. As stated in Eq. 5, the rotation E q S is used to compute the relative orientation of the world frame, W, w.r.t. the SVS frame, S, since W has been chosen as the initial position of the ego frame, E(t = 0) (W is chosen to be coplanar to the road plane). The rigid transformation S q, p S(t) corresponds to the visual odometry given by the following equations:

S q S(t) = S q S(t-1) S(t-1) q(∆ω) S(t)

S p S(t) = S q S(t-1) S(t-1) ∆v S(t) S qS(t-1) + S p

S(t-1) (7)
where S(t-1) q(∆ω) S(t) is the unit quaternion corresponding to the axis-angle rotation, ∆ω, composing the ego-motion parameters S(t-1) [∆ω, ∆v] T S(t) .

Experimental Real Time 3D Ego-Localization Results

A data set was acquired in an urban environment composed of low-rise buildings, trees and moving objects (i.e. pedestrians and vehicles). During the experiment, the vehicle's speed was around 30 Km/h. The vehicle trajectory is a closed loop featuring pedestrians and other vehicles. Low-textured scenes (e.g. rural environments and parking lots) were not considered in this study. The 3D ego-localization function was implemented in C/C++. The 3D trajectory is reconstructed in real time and is obtained by integrating the ego-motion estimations. Fig. 10 illustrates one of the tests performed. This test consists of a 227m-clockwise loop (i.e. 90 seconds video sequence duration).

These results show that the cooperative strategy helps to cope with errors from the CAN-sensor-based odometry, errors mainly attributable to wheel slippage. This technique also improved the visual odometry performance in critical situations (e.g. high rotational speed in 90°turns and roundabouts). These improvements were obtained as a result of the planar motion initialization which avoids any local minima ego-motion solution. It also enhances outlier rejection and reduces the minimization iteration cycles. The 3D ego-localization system performs quite well in situations where GPS cannot provide a precise position (see the GPS jumps in Fig. 10).

Exteroceptive Sensor Calibration (ML Lidar -Vision)

The extrinsic calibration of the exteroceptive sensors (i.e. the ML lidar and the stereo vision system) is necessary for sensing information in a common perception space. These extrinsic parameters link the local sensor frames by a rigid transformation estimated from the sensor measurements. Thus, the calibration process aims at estimating the parameters which minimize the transformation error between a common set of measurements.

Problem Statement

Different calibration methods have been proposed to estimate with accuracy the rigid transformation between a camera and a range sensor. Zhang et al. proposed a target-based calibration method between a camera and a single-row laser range finder [START_REF] Zhang | Extrinsic calibration of a camera and laser range finder (improves camera calibration)[END_REF]. This method was considerably improved by Dupont et al. in [START_REF] Dupont | An improved calibration technique for coupled single-row telemeter and ccd camera[END_REF]. A targetless calibration approach for a 3D laser range finder was later presented by Scaramuzza et al. in [START_REF] Scaramuzza | Extrinsic self calibration of a camera and a 3d laser range finder from natural scenes[END_REF]. Calibration can also be done by making use of the 4-layer provided by the ML lidar [START_REF] Rodriguez | Influence of intrinsic parameters over extrinsic calibration between a multi-layer lidar and a camera[END_REF]. Let L p, be the coordinates of a 3D laser point expressed in the lidar frame L. Therefore, S p, the corresponding coordinates of L p in the stereo vision frame, S, is given by the following expression:

S p = G q L L p G qL + G t L + S t G (8)
where G [q, t] L is the rigid-body transformation from the lidar frame, L, to the left camera frame, G. S t G is the rigid-body transformation from the left camera frame, G, to the stereo vision system frame, S, as stated in Eq. 8.

As illustrated in Fig. 11, knowledge of the rigid transformation G [q, t] L means that SVS and ML lidar data can be merged.

The calibration method relies on a set of simultaneous detections of a circle-based calibration target by the camera and the multi-layer lidar. For every detection, the attitude and the position of the calibration target are estimated with respect to the camera and the lidar frames. The complete frame transformation from the lidar frame L into the vision frame S, S [q, t] L , is obtained as follows:

S q L = q(I) G q L and S t L = S t G + G t L ( 9
)
where q(I) is the unit quaternion representation of the identity matrix since the frames S and G are aligned.

Once the lidar-vision frame transformation S [q, t] L is known, all the lidar measurements at time t are carried over to the vision frame S.

Exteroceptive Sensor Calibration Results

The ML lidar -Vision calibration was obtained using 8 poses. Each pose comprises 20 lidar scans and the corresponding target image of the left camera. The transformation parameters and their corresponding intervals of confidence were computed as an Euler vector in radians and a translation in meters respectively.

Rotation parameters, [φ, θ, ψ] T , are then converted into a quaternion, formalizing the extrinsic parameters to the form G [q, t] L . Fig. 12 illustrates the resulting lidar data re-projection onto the stereo images.

Object Localization and Tracking

This stage corresponds to a multi-modal strategy which is able to estimate the planar trajectory of the neighboring objects as they move in the 3D scene. The proposed system operates in a fixed reference frame, since precise ego-localization is available (see section 3). The tracking strategy is presented below as follows: identifying the zone of interest, object detection, track prediction, and object-track association and updating. Finally, the experimental results of this perception function are reported. A first lidar data filtering step is performed with the aim not only of improving the ZOI detection, but also of significantly attenuating scene clustering issues. This filtering involves detecting the 3D lidar data corresponding to the road surface, using the characteristic pattern observed when the two lower layers intersect the road plane at different angles, as illustrated in Fig. 13. The second lidar layer can be predicted from the first lidar layer and the geometrical constraints. The measurements belonging to the road plane are detected and excluded from further processing when the Euclidean error between the predicted and the measured layer falls below a predefined threshold. The pitch angle of the ML-lidar, denoted ρ, is estimated by a temporal filtering and is updated using the detected road impacts. The parameters h (ML-lidar height) and γ (inter-layer angle) are assumed to be known.

The zone of interest is characterized by two local limits in the x-axis direction of the lidar frame. As illustrated in Fig. 14, a 4-layer data scan is projected onto the L xy plane (see upper subplot) and provides a convenient histogram L y axis (see lower subplot). Objects such as security barriers, walls and parked vehicles limit the zone of interest effectively. The detected limits are then filtered using a fixed-gain Luenberger observer in order to reduce oscillations produced by significant changes in pitch. Intersections and roundabouts may lead to the loss of histogram peaks. In such cases no further updates of the ZOI are performed, meaning that it is the last ZOI estimation which is retained.

Object Detection

Based on each ML-lidar scan, an object detection function delivers a set of neighboring objects obtained by a 3D Euclidean inter-distance clustering. Predefined geometric features [START_REF] Nashashibi | Vehicle recognition and tracking using a generic multisensor and multialgorithm fusion approach[END_REF] would be an alternative, but these require prior knowledge of the object. Detected objects are characterized by their planar location (i.e. L Z = 0) in the lidar frame L, their dimension (i.e. bounding circle) and a detection confidence indicator [START_REF] Fayad | Object-level fusion and confidence management in a multi-sensor pedestrian tracking system[END_REF], estimated using the following criteria:

-The ability of the ML sensor to detect vertical objects with respect to its range position. -The beam divergence which worsens the measurement precision, particularly in situations of a nonperpendicular incidence angle. -The theoretical maximum number of laser impacts (per layer) lying on a detected object. This factor can be computed as a function of the object dimension, the detection range and the laser scanner resolution.

Knowing the 3D localization of the vehicle, the detected objects can be referenced with respect to the world frame, W . For instance, let L y(t) = [x y 0] T be the coordinates of a detected object at time t. Its corresponding localization in W can be computed as follows:

S(t) y(t) = S q L L y(t) S qL + S t L (10)
W y(t) = W q(t) S(t) y(t) W q(t) + W p(t) [START_REF] Fayad | Tracking objects using a laser scanner in driving situation based on modeling target shape[END_REF] where S(t) y(t) and W y(t) are the corresponding coordinates of the detected object in the SVS and the world frame respectively.

Only the detected objects lying in the ZOI are localized w.r.t. the world frame using Eq. 10 and 11. They are then tracked independently using Kalman filters [START_REF] Mohinder | Kalman Filtering: Theory and Practice Using Matlab[END_REF]. This tracking step improves the robustness of the perception scheme.

Track Prediction

The track state is described by W y(t|t) consisting of the W XZ plane coordinates (x(t), z(t)) in meters and the planar velocity (v x (t), v z (t)) in m/s as follows:

W y(t|t) = x(t) z(t) v x (t) v z (t) T (12)
The object size is considered as an attribute of the track but is not included in the state. Assuming that the motion of the objects is linear and uniform, the prediction of the track state is given by the following evolution equation:

W y(t|t -1) = B(t) • W y(t -1|t -1) (13)
with

B(t) = I 2×2 ∆t • I 2×2 0 2×2 I 2×2
where W y(t|t-1) is the predicted state of the tracked object, B(t) is the state transition matrix and ∆t is the sampling time period (which is not constant).

Track-Object Association and Updating

At each ML-lidar sampling, the tracks are updated. This involves matching the new detected objects and tracks using a mono-hypothesis assumption. The implemented association test relies on a nearest neighbor criterion [START_REF] Samuel | Design and Analysis of Modern Tracking Systems[END_REF] (i.e. min(η)) using the following normalized metric:

η 2 = µ(t) T (M(t|t -1) + N) -1 µ(t)+ln (det (M(t|t -1) + N)) (14) with µ(t) = C • W y(t|t -1) -W y(t) (x,z) and C = I2×2 02×2
where W y(t) (x,z) represents, with a slight abuse of the notation, the XZ coordinates of the detected object in the W frame, C is the observation matrix and M(t|t-1) the covariance matrix of the predicted state, W y(t|t-1). The first term of Eq. 14, corresponds to the classical Mahalanobis metric, and the second corresponds to a weighting factor, ln (det (M(t|t -1) + N)), computed from the track imprecision. The uncertainties in the lidar localization of the objects and the object motion model are taken into account through the covariance of the measurement noise, N and the covariance of the state transition model, O.

In order to cope with temporal object occlusions, the unmatched tracks are retained for a fixed time duration (for example, 2 seconds). However, setting a long prediction time may result in retaining track artifacts. The unmatched objects in the zone of interest generate new tracks until the algorithm reaches a previously defined maximum number of tracked objects. Here, the fixed number of tracks is set sufficiently high to track all the detected objects in the ZOI.

Finally, the states of the tracks and their corresponding covariances are improved by combining the information provided by the associated lidar objects positions and the predicted tracks [START_REF] Samuel | Design and Analysis of Modern Tracking Systems[END_REF]. For this we use the Kalman filter update equations.

It is worth recalling that the object tracking stage increases the robustness of the system by allowing the occlusion of objects, since tracks contain information that has been confirmed several times by the same source (the ML lidar here).

Experimental Results

In the experimental study of the multi-modal object localization and tracking system, the 3D ego-localization, the identification of the zone of interest and the detection of objects functions were processed in real time and their outputs were logged. The object tracking function was implemented in Matlab. The reported results were obtained in offline conditions, taking as the input the logged data. Fig. 15 illustrates the XZ view of the reconstructed zone of interest in the fixed-reference frame. The reconstructed ZOI was obtained by exchanging geometrical data between the lidar and vision-based functions through the calibration information. It is important to remark that at the beginning of the test sequence (i.e. initial position (0,0) on XZ view), the vehicle remains stationary, which shows how the boundaries of the zone of interest converge. These results constitute a very interesting feature which can be linked to GIS (Geographic Information System) for map-matching applications.

Fig. 16 illustrates a zoomed area of the dynamic map. In this figure we focus on a tracked vehicle. The size of the track is represented by its bounding circle in red and its center as a red triangle. The detected track size changes as the surface is impacted by the ML lidar. The corresponding image track projections (3D red boxes) and their speed vector (green line) are also illustrated in the upper part of the figure. Observing the image projection of the track speed vector, one can see that the multi-modal system performs quite well.

Fig. 17 shows another section of the dynamic map. No ground truth for the track localization was available during the experiment. However, the reconstructed trajectory corresponds to the pedestrian's observed trajectory in the snapshot sequence. The multi-modal system presented thus far provides a precise localization, with respect to a fixed-reference frame, of the vehicle and the surrounding objects that represent potential obstacles. However, the object detection relies on a single source: the ML lidar. This section presents visual track confirmation, which represents a way of increasing the integrity of the information provided by the system. Integrity is a key attribute of a perception system when dealing with ADAS [START_REF] Liggins | Handbook of Multi-Sensor Data Fusion[END_REF]. For the system proposed in this paper, integrity means that the intended perception functions provide trustworthy information without false alarms or ambiguity. Visual track confirmation is performed using the following strategy. First, each lidar-tracked object is transformed into the ego frame E(t). Its corresponding bounding cylinder (lidar bounding circle with an arbitrary height) is reprojected onto the stereo images. In each image, the track projection provides a Region Of Interest (ROI). Secondly, the pixels composing the ROI are reconstructed by stereopsis in the 3D space in order to provide a 3D point cloud. Thirdly, this set of 3D points is segmented into 2 clusters : the object and Fig. [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF] Trajectory of a tracked vehicle the background. Finally, the track is confirmed if one of the 3D point cluster centroids is validated using a Mahalanobis distance given a confidence level.

Region Of Interest in the images

The projection of an observed track onto the image plane can be done once the track position is referenced w.r.t. the camera frame. But, the 3D track fix must be firstly reconstructed, since the state vector provides its planar coordinates (i.e. W XZ coordinates) only. To this end, the last object altitude (i.e. W Y coordinate) associated to the track is used. In the following, the reconstructed track fix is denoted W y + (t|t -1) . The track is then localized in the ego frame, E(t) as follows:

S(t) y + (t|t -1) = W q(t) W y + (t|t -1)-W p(t) W q(t) (15)
E(t) y + (t|t -1) = E q S S(t) y + (t|t -1) E qS ( 16
)
where S(t) y + (t|t -1) is the reconstructed track fix in the S(t) frame and E(t) y + (t|t -1) is the resulting position of the track in E(t).

The ROI is characterized by re-projecting the bounding box vertices of the track on the images. These vertices are estimated from the track size (the track height is known a priori) and its 3D centroid position (see Fig. 19).

The track position is projected onto the image plane by the following equation:

u v 1 ∼ K• S q E E(t) y + (t|t -1) S qE + G t S (17)
where [u v 1] T are the image coordinates and K is the intrinsic camera matrix. S q E corresponds to the conjugate quaternion of E q S and G t S = -S t G . The operator ∼ means up to a scale factor.

3D dense reconstruction of the ROI

Every pair of images contains 3D dense information of the scene since the pixel images correspondence and the camera parameters are known. This information can be represented by a disparity map which in this study is assumed to be referenced w.r.t. the left camera of the SVS.

The 3D dense reconstruction of the ROI consists in overlapping the ROI and the disparity map as illustrated in Fig. 18. The set of corresponding disparity values is then extracted. Finally, the 3D coordinates, [x y z] T , of each pixel are estimated by performing a classical triangulation process [START_REF] Hartley | Multiple View Geometry in Computer Vision[END_REF].

Track confirmation

Conceptually, the existence of a tracked object is confirmed if its position matches the visual 3D points. One possible strategy would be to test all the points in order to determine the percentage that match with the track. However, this approach may be time consuming and is not compatible with an embedded system. An alternative is to cluster the visual points to apply a featurelike test. Because of the imprecision of the lidar object model, the ROI usually contains the imaged object and some scene background, as illustrated in Fig. 18. For this reason, clustering the 3D points can be seen as an ideal solution for simplifying and speeding up the process. Assuming that the objects and the scene background in the ROI are distinguishable in the 3D space (see the example shown in Fig. 19), the reconstructed 3D points can be segmented into two clusters: track and background. This assumption is usually justifiable, because the tested objects are located on the ZOI which corresponds to the navigable space, as shown in Fig. 20. For this kind of clustering, a K-means method [START_REF] Macqueen | Some methods for classification and analisys multivariate observations[END_REF] is particularly suitable, since the number of classes is known. The clustering is based on the Euclidean distance between 2 clusters characterized by theirs centroids, E(t) c j for all j = {1, 2}, and their associated points. Two particular cases can occur, but they have no impact. If the ROI contains only the object (i.e. no background is included), the two clusters obtained will remain close to the real track. If no real object is included in the ROI (i.e. a lidar false detection), the clusters will form part of the background. Once the clusters have been determined, the uncertainty associated with each cluster needs to be estimated. As the centroid of every cluster is localized from a set of reconstructed points, its uncertainty is strongly correlated to the triangulation process quality. As shown in [START_REF] Blostein | Error analysis in stereo determination of 3d point positions[END_REF], the confidence in the range of each centroid, E(t) c j can be estimated, up to a tolerance factor α, by the following:

τ j =      1 -1 -α • b•f E(t) c (z),j 2 , α < E(t) c (z),j b•f 1 , α ≥ E(t) c (z),j b•f ( 18
)
where E(t) c (z),j refers to the z metric coordinate of the centroid E(t) c j and τ j is a score which expresses the confidence in the cluster centroid fix in depth w.r.t the SVS. This score is directly derived from the first-order approximation of the joint probability distribution of the centroid image projection on the left and right image planes. The error tolerance factor, α, is set in meters, taking into account the image resolution, the focal distance, the baseline of the SVS and the accuracy of the SVS calibration.

In order to perform a normalized test between the clusters and the track to be verified, the covariance of the 2D position of the cluster centroid U can be estimated using this confidence score τ j : [START_REF] Liggins | Handbook of Multi-Sensor Data Fusion[END_REF] where the weighting parameters k 1 and k 2 can be chosen on the basis that the reconstruction error regarding depth has more impact in the longitudinal direction ( E(t) Z axis) than transversely (i.e. k 2 > k 1 : typically one can choose k 2 = 2k 1 ).

U = 1 k1•τj 0 0 1 k2•τj
Each cluster is tested w.r.t the ML-lidar tracked object using a Mahalanobis distance, ξ, with respect to the track position:

ξ 2 j = κ • (U + M(t|t -1)) -1 • κ T (20) with κ = E(t) c (x,z),j -E(t) y + (t|t -1) (x,z)
where E(t) c (x,z),j and E(t) y(t|t -1) (x,z) are respectively the E(t) XZ coordinates of the centroid cluster to be tested and the tracked object. The matrix M(t|t -1) is the covariance of the tracked object location. If min(ξ) is below a certain threshold (typically corresponding to 3 standard deviations), this means that one visual cluster matches with the track in position. A match confirms the existence of the tracked object. Integrity is automatically satisfied, since two independent sources have been used.

Sampling synchronization issues

The visual track confirmation scheme relies on three main functions interacting together: the disparity map computation, the 3D-ego localization and the object detection. These functions are asynchronous and run in different threads at different frequencies (26, 16 and 15 Hz respectively) which are quite constant as illustrated in Fig. 21. Fig. 22 shows an example of possible measurements arrivals. At t 0 , the disparity map and the localization information are available but there is no object. Thus, only the vehicle pose is updated. At t 1 , a set of objects have been detected. They are localized using the predicted vehicle pose. At t 2 , tracks can be confirmed using the predictions of the objects and the vehicle pose.

We remarked during our experiments that this mechanism dramatically increases the performance of the system, particularly when the scene includes a significant dynamic content.

Experimental results

The data output log of the disparity map, 3D-ego localization and object detection functions became the input into the object tracking and track confirmation functions that were post-processed using Matlab.

Use Cases and Evaluation Methodology

In order for the performance of the system to be evaluated, we report experimental results for the visual confirmation function using five sequences. These use cases are relevant to common scenarios in urban environments. Fig. 23 gives a graphical description of the evaluated situations involving three kinds of mobile objects: pedestrians, wheelchair pedestrians and cars.

In the reported experiments, the ML-lidar did not give rise to any misdetections. The evaluation methodology aims at quantifying the percentage of time during which the object tracking function becomes unavailable because of visual non-confirmations.

The ground truth was referenced manually in the left image plane of the SVS: the center point coordinates of the observed objects of interest were selected, frame by frame. All the objects considered in the ground truth were localized in a common perception region for the SVS and the ML-lidar. The confirmation track rate was obtained by counting the number of times where the bounding box of the confirmed track contains the ground truth.

Real Data Results

The results obtained using the ground truth are reported in Table 1. A total of 650 frames in 5 different situations showed that at least 81% of the time, the detected objects of interest were confirmed by the two modalities. Although one may conclude that the visual track confirmation may sometimes reduce the number of true positives, it should nevertheless be remarked that the confirmed tracks enhance the integrity of the perception process. The false alarm rejection rate was also evaluated by providing manually a phantom track located four meters in 2. As the proposed method can eliminate up to 94% of FA as a result. In use cases C and E, it was observed that large changes in vehicle pitch angle can influence the precision of object tracking, since object motion is considered to be planar and the vehicle pitch angle is unknown. In Fig. 24, the left-hand side illustrates the world map where the ego-vehicle and the detected objects are localized and tracked. The right-hand side of the figure shows the reconstructed points of the ROI image. Looking at the ego-map, it will be remarked that one of the centroids of the clustered point cloud has been matched with the track. This match confirms the existence of the detected object, as illustrated in the upper image in Fig. 25.

Conclusion and Future Work

An asynchronous embedded multi-modal system for object localization and tracking has been proposed and experimentally validated. The approach presented here provides a 3D dynamic map of a vehicle's surroundings. The method merges sensor data to achieve a robust, accurate 3D ego-localization. This function is combined with a lidar-based object tracking focused on a zone of interest providing the trajectories and speeds of objects as they move in the space. A visual confirmation function integrated into the tracking system checks the integrity of the provided information. To this end, ROI are processed in a dense way. 3D points are reconstructed and compared to the lidar-tracked object. This scheme makes full use of the broad functional spectrum of stereo vision systems. Synchronization issues are taken into account to ensure the temporal consistency of the system. 23). The middle figure shows a vehicle having just turned. Here the ROI is delocalized because of pitch changes, given that the egovehicle is accelerating (Senario C in Fig. 23). The bottom figure shows the confirmed pedestrian when crossing the road in Scenario E (Fig. 23)

The results obtained show the effectiveness and the integrity of the proposed strategy. The visual confirmation function was tested in five different scenarios, demonstrating a good confirmation rate. Although visual confirmation inevitably reduces the availability of the detection function, the obtained rate would appear to be compatible with the development of ADAS functions (e.g. collision detection and avoidance). The inclusion of a visual object recognition function for selecting the most suitable object motion model might improve the tracking process. This is one perspective of our research.

Acronyms

Fig. 1 A

 1 Fig. 1 A Dynamic Map is composed of a list of the states of tracked objects located within a zone of interest together with the changing vehicle dynamics in the 3D scene.

Fig. 2

 2 Fig. 2 Multi-Modal System Design

Fig. 4

 4 Fig. 4 Layout representation of the perception coverage provided by the exteroceptive sensors SVS and the ML lidar

Fig. 5

 5 Fig.5The multi-sensor frames are located at the optical center of the left camera, G, the midpoint of the SVS, S, and the ML-lidar sensor, L.

Fig. 6

 6 Fig. 6 On-board frames and exteroceptive vertical sensor coverage. The depicted lidar layer divergence and the camera FOV have been exaggerated for clarity.

Fig. 7

 7 Fig. 7 Multi-modal 3D ego-localization scheme

Fig. 8

 8 Fig. 8 Yaw rate-WSS dead-reckoning for planar odometry estimation

Fig. 10

 10 Fig. 10 Bird's eye view of the 3D Reconstructed Trajectory compared to WSS-Gyro odometry and GPS positioning

Fig. 11

 11 Fig. 11 Extrinsic Camera-Multi-layer Lidar Calibration

Fig. 12 MLFig. 13

 1213 Fig. 12 ML Lidar data re-projection on stereo images

Fig. 14

 14 Fig.[START_REF] Hanson | Visualizing Quaternions[END_REF] Identifying the zone of interest using a y-axis histogram

Fig. 15

 15 Fig. 15 Reconstruction of the Zone of Interest ( W XZ plane view)

  Fig. 17 Trajectory of a pedestrian

Fig. 18

 18 Fig. 18 3D dense reconstruction of the ROI

Fig. 19

 19 Fig. 19 Visual 3D Track Confirmation

Fig. 20

 20 Fig.203D visualization of a confirmed track using 3D point clusters

Fig. 21 Fig. 22

 2122 Fig. 21 Sampling function frequencies

Fig. 23

 23 Fig. 23 Use cases considered in the evaluation test

Fig. 24

 24 Fig.[START_REF] Rodriguez | Influence of intrinsic parameters over extrinsic calibration between a multi-layer lidar and a camera[END_REF] Example of a confirmed tracked object using the SVS

Fig. 25

 25 Fig.[START_REF] Rodriguez | An experiment of a 3d real-time robust visual odometry for intelligent vehicles[END_REF] Confirmed objects. The top figure illustrates the visual confirmation of a highly dynamic object (Scenario B in Fig.23). The middle figure shows a vehicle having just turned. Here the ROI is delocalized because of pitch changes, given that the egovehicle is accelerating (Senario C in Fig.23). The bottom figure shows the confirmed pedestrian when crossing the road in Scenario E (Fig.23)

Table 1

 1 Rate of detected objects confirmed by vision

	Video Sequence	A	B	C	D	E
	Duration (s)	5	4	5	6	10
	Number of	110	90	90	125	235
	Analyzed Frames
	Number of scans	78	51	62	94	137
	Positioning updates	72	37	65	121	160
	Visual Confirmation	100 100 81.8 98.5 83.5
	Rate (%)

Table 2

 2 False Alarm Rejection for 2856 frames

	Track size (m)	0.8	1	1.2	1.5	2
	Erroneous	170	207	208	219	184
	Confirmations
	FA Rejection	94.04 92.75 92.72 92.33 93.55
	Rate (%)

Acknowledgements

The authors wish to thank Fadi Fayad for the real time implementation of the ML lidar detection functions and Gerald Dherbomez and Thierry Monglon for their experimental support.


Sergio A Rodríguez

Vincent Frémont

Philippe Bonnifait

Véronique Cherfaoui

An Embedded Multi-Modal System for Object Localization and Tracking

Keywords: Multi-modal perception, visual odometry, object tracking, dynamic map, intelligent vehicles

Reliable obstacle detection and localization is a key issue for driver assistance systems, particularly in urban environments. In this study a multi-modal perception approach is investigated, the objective being to enhance vehicle localization and dynamic object tracking in a world-centric map. A 3D ego-localization is achieved by merging information from a stereo vision system and data obtained from vehicle sensors. Mobile objects are detected using a multi-layer lidar that is also used to identify a constrained search space within the multiple target tracking process. Object localization and tracking is then performed in the fixed frame, which facilitates analysis and understanding of the scene. Experimental results using real world data are performed to evaluate the performance of the multi-modal system, and these are presented to show the effectiveness of the approach.

I. INTRODUCTION

Advanced Driver Assistance Systems (ADAS) can improve road safety through their obstacle-detection and collision-avoidance features. In this context, knowing the position and the speed of surrounding mobile objects is crucial.

The literature includes a number of different approaches to tackling the object localization and tracking problem. SLAM-like approaches can be used to characterize the static part of the environment and to simultaneously detect moving objects [START_REF] Wang | Simultaneous localization, mapping and moving object tracking[END_REF]. In [START_REF] Barth | Estimating the driving state of oncoming vehicles from a moving platform using stereo vision[END_REF], [START_REF] Leibe | Dynamic 3d scene analysis from a moving vehicle[END_REF] and [START_REF] Nedevschi | Stereo-based pedestrian detection for collision-avoidance applications[END_REF], two different stereo vision strategies are proposed to obtain a 3D dynamic map by combining 2D intensity and 3D depth information. A lidar alone can be used to estimate the ego-motion and to detect mobile objects via a dense 3D grid-map approach [START_REF] Miyasaka | Ego-motion estimation and moving object tracking using multi-layer lidar[END_REF]. Alternatively, [START_REF] Gidel | Pedestrian detection and tracking in an urban environment using a multilayer laser scanner[END_REF] presents a lidar based approach which performs mobile object tracking within a stochastic recursive Bayasien framework. In [START_REF] Broggi | A new approach to urban pedestrian detection for automatic braking[END_REF] and [START_REF] Labayrade | Cooperative fusion for multi-obstacles detection with use of stereovision and laser scanner[END_REF], real-time sensor-referenced approaches (i.e. ego-localization is not considered) are presented using multi-sensor systems, thus showing the complementarity of lidar and vision systems in automotive applications. Recently, Weigel et al. [START_REF] Weigel | Vehicle tracking with lane assignment by camera and lidar sensor fusion[END_REF] have investigated a complete multi-modal object localization and tracking system using a grid based space representation. Their proposed system performs object detection based on lidar data, CAN bus sensors are used for motion compensation and a camera is employed to determine the height of objects.

For the purposes of object localization and tracking, a world-centric approach presents interesting properties for cooperative ADAS applications once the egolocalization has been accurately estimated. One wellknown drawback of this approach is an unbound error propagation over long distances if it is based on odometry [START_REF] Altendorfer | Observable dynamics and coordinate systems for automotive target tracking[END_REF]. However, this strategy facilitates the motion model equations of the mobile objects and increases the accuracy of the tracking system over short distances, as discussed in [START_REF] Buhren | A global motion model for target tracking in automotive applications[END_REF]. Ego-localization can be achieved using environment perception and vehicle sensor data. GPS is an affordable system that provides 3D positioning. Unfortunately, GPS performance can be significantly reduced in urban environments because of multi-paths and satellite outages. Odometry is a complementary solution which can provide good positioning when the relative motion is integrated over short distances. In this way, Stereo Vision Systems (SVS), often used for detection and recognition tasks, can also be useful for localization and navigation applications [START_REF] Meilland | A spherical robotcentered representation for urban navigation[END_REF]. Several studies have already looked at how odometry using vision sensors (called in the following visual odometry) can provide precise positioning of a mobile platform in complex dynamic environments. Scaramuzza et al. [START_REF] Scaramuzza | Real-time monocular visual odometry for on-road vehicles with 1-point ransac[END_REF] have proposed a stable feature tracking performing in the large field-of-view provided by an omnidirectional camera. Such a method dramatically reduces association errors, but it still remains a monocular approach subject to provide estimates up to a scale factor. In [START_REF] Kitt | Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme[END_REF], full 6-

Multi-modal object tracking

Tracks lidar objects in a world-reference frame and localizes the ego-vehicle

Object Detection

Detects and characterizes static and mobile objects based on the ML lidar perception

CAN-Bus Sensors Wheel speed and Yaw rate measurements

Stereo Vision

Synchronized and Rectified images

ML lidar

Scan points

Dynamic Map

Provides the environment with the dynamic states of vehicle and the surrounding objects

Multi-modal 3D Odometry Computes the ego-motion and filters the vehicle state

Maneuvering Window Identification

Detects and provides the limits of the research space DOF (degrees of freedom) motion is estimated using a robust filtering estimator, subsequently enhanced by a feature-classification scheme in [START_REF] Kitt | Moving on to dynamic environments: Visual odometry using feature classification[END_REF]. In contrast, other methods make use of voting strategies [START_REF] Obdrzalek | A voting strategy for visual egomotion from stereo[END_REF] and decoupled rotation/translation estimates [START_REF] Pojar | Improving localization accuracy based on lightweight visual odometry[END_REF] with constrained motion models. In a previous work [START_REF] Rodriguez | An experiment of a 3d real-time robust visual odometry for intelligent vehicles[END_REF] we proposed and implemented a 6-DOF real-time visual odometry strategy based on a sparse optical flow, and on multiple view rigidity constraints introduced in [START_REF] Comport | Accurate quadrifocal tracking for robust 3d visual odometry[END_REF].

Object tracking from a mobile platform remains an active research domain for ADAS. Urban environments are characterized by complex and dynamic conditions: moving (whether holonomic or non-holonomic) and static objects, and very different infrastructures. Object representation [START_REF] Petrovskaya | Model based vehicle tracking in urban environments[END_REF], [START_REF] Nashashibi | Vehicle recognition and tracking using a generic multisensor and multialgorithm fusion approach[END_REF], association methods [START_REF] Shalom | Multitarget/Multisensor Tracking: Applications and Advances[END_REF], motion model and tracking strategies [START_REF] Liggins | Handbook of Multi-Sensor Data Fusion[END_REF] are key points requiring particular attention.

We present here a multi-modal system able to provide a 3D local perception of the vehicle environment in a world-centric frame (see Fig. 1). The environment is composed of static and moving objects, and a maneuvering window is defined in front of the vehicle. The contribution of this work is estimating the dynamics of the surrounding objects (location and speed) based on different sensing modalities in order to build a dynamic map comprising a list of states of tracked objects and the changing vehicle dynamics in the 3D scene.

The embedded multi-sensor system employed uses proprioceptive sensors (i.e. wheel speed sensors and a yaw rate gyro) and two exteroceptive sensors: a Multi-Layer lidar (denoted ML lidar) and a Stereo Vision System (denoted SVS).

The overall strategy is described in Fig. 2. First, the vehicle ego-localization is estimated by merging CANbus (Controller-Area Network) information with visual odometry. The ML lidar then provides a 3D perception of the scene structure used to identify a maneuvering window. Following this, objects situated within the zone of interest are localized in the fixed-reference frame by compensating for the motion of the ego-vehicle. Finally, objects are tracked in this world frame and the resulting information can be used by an ADAS to estimate possible collisions.

This paper addresses 3D ego-localization, as well as object localization and tracking. Section II gives a detailed description of the embedded multi-sensor system setup. The different multi-modal functions are next presented and discussed. Section III is devoted to multimodal 3D odometry using vision and proprioceptive sensors. Object localization and tracking are studied in Section IV. Finally, Section V presents conclusions and discusses future work.

II. MULTI-MODAL PERCEPTION SYSTEM

Let us consider a vehicle equipped with an ML lidar, a yaw rate gyro, wheel speed sensors (WSS) accessible through a CAN-bus gateway, and a stereo vision system (SVS). These instruments supply the asynchronous inputs into our perception system. Figure 3: The experimental vehicle and its different perception capabilities. This study makes use of a stereo vision system, the IBEO Alasca XT and wheel speed and gyroscopic sensors.

Our vehicle is equipped with an IBEO Alasca XT lidar which provides a sparse perception of the 3D environment by the means of 4 crossed-scan-planes covering 150°horizontally in a 200 m range. Perception of the surroundings is complemented by a 47 cm-baseline Videre SVS installed on the roof of the vehicle and covering a 45°field of view. Vehicle data is acquired through a CAN-bus gateway giving access to the speed of the rear-wheels and the yaw rate measurements.

A. Coordinate systems

Each of the perception devices references the acquired data with respect to its own frame. Lidar measurements are reported in a Cartesian frame, denoted L (X-Front, Y-Left and Z-Up) and refreshed at a rate of 15 Hz. SVS data is referenced to the vision frame, S (X-Right, Y-Down and Z-Front) and updated at 30 FPS. Vehicle measurements (WSS and Gyro) are reported in a frame located at the center of the rear-axis, denoted R (X-Right, Y-Down and Z-Front).

As stated above, our work aims at generating a dynamic map using different sensing modalities. To this end, the transformations between the different sensor frames have to be determined, as illustrated in Fig. 5. The geometrical transformation between the ML lidar and the SVS is denoted as S [q, t] L and is determined via an extrinsic calibration [START_REF] Rodriguez | Extrinsic calibration between a multi-layer lidar and a camera[END_REF]. The transformation between the SVS and the World frame is denoted W [q, t] S(t) , and changes over time since we are dealing with a mobile platform. This transformation is computed using a real-time odometry method presented later in this paper. Multi-modal 3D odometry corresponds here to the 3D pose estimation of the ego-vehicle as a function of time with respect to a fixed initial frame. Odometry methods using stereo vision systems can provide very precise 3D pose estimations based on quadrifocal constraints, as presented by Comport et al. [START_REF] Comport | Accurate quadrifocal tracking for robust 3d visual odometry[END_REF]. However, visual odometry may require significant computation time, since it makes use of a dense image warping technique. Real-time execution consequently calls for optimization and adequate computational resources.

In order to achieve a good trade-off between precision and execution time, we estimate the 3D vehicle egolocalization using visual odometry in addition to the odometry information obtained from the CAN-bus sensor measurements. A global overview of this alternative localization technique is illustrated in Fig. 4.

A. Visual odometry aided with CAN-bus sensors

The ego-motion of the vehicle is defined by an elementary transformation (rotation-translation composition, 6 DOF) over a certain time interval. This estimate is represented by an axis-angle rotation and a translation vector, S(t-1) [∆ω, ∆v] S(t) . We first make an estimate of planar motion using the CAN-bus sensors over a time interval ∆t (see Motion initialization in Fig. 4). This initial guess is then fed into a 3D visual motion estimation algorithm that iteratively refines the solution (see Robust function in Fig. 4).

Let R(t) be the center of the rear-wheel axis defined at time t. If the sampling frequency of the gyro and the WSS is sufficiently high (around 40 Hz), the wheel speed is almost constant and the planar ego-motion can be approximated by a circular arc. As illustrated in Fig. 6, the planar ego-motion of the vehicle is modeled as follows:

∆ω0 =   0 ∆θ 0   ∆v0 =   ∆s • sin(∆θ/2) 0 ∆s • cos(∆θ/2)   (1)
where ∆θ is the angle obtained by integrating the yaw rate, ∆s is the integrated rear-wheel odometry in meters, ∆ω 0 is a vector representing the axis-angle rotation of the vehicle motion and ∆v 0 is a vector representing the estimated displacement of the rear-wheel axis center.

The estimated motion R(t-1) [∆ω 0 , ∆v 0 ] R(t) is then considered as a near estimate of S(t-1) [∆ω 0 , ∆v 0 ] S(t) . 7), we obtain a set of tracked stereo feature points, p , p , and their corresponding optical flow constituting the image motion. For this purpose a set of stereo feature points, p * , p * , is extracted using Harris features associated with a ZNCC (Zero-mean Normal Cross Correlation) correlation criterion and image constraints (disparity and epipolar constraints). Here, p * i and p * i are defined as the projection of an observed 3D point P. The stereo features, p * , p * , are tracked over time using the Lucas-Kanade method [START_REF] Bouguet | Pyramidal implementation of the lucas kanade feature tracker description of the algorithm[END_REF], thus defining the tracked stereo feature points set p , p . As stated in [START_REF] Comport | Real-time quadrifocal visual odometry[END_REF], the simplified constraint parameters comprise the tensors linking the camera triplets (C, C , C ) and (C , C, C ), denoted respectively T jk i , T mn l , and the fundamental matrix F 34 linking the cameras (C , C ), which is equivalent to F 12 for the cameras (C, C ) (see Fig. 7). This parametrization remains robust to camera modeling errors and depends on the unknown motion parameters only, S(t-1) [∆ω, ∆v] S(t) . The stereo warping operator is thus given by:

3D static point

p k p n = p * i l j T jk i p * l lm T mn l (2)
where l j and l m are respectively the covariant representations of the left and right image lines passing through the image points p * i and p * l , and perpendicular to the epipolar line. The image points p * i , p * l , p k , p n are the contravariant representations of p * , p * , p , p .

Thus far, the visual odometry scheme is valid under the rigid scene assumption. If this assumption is not satisfied, the estimated ego-motion will be biased or completely incorrect. However, such an assumption is not realistic for intelligent vehicle applications, since the SVS will be operating in environments characterized by mobile objects, complex backgrounds and occlusions. The relaxation of the rigid scene assumption can be made through the use of a robust estimator which will also prove useful in dealing with other sources of error like image noise, stereo matching and tracking drifts. It should be remarked that scene can no longer be assumed static in the presence of such errors.

To this end, a robust iterative non-linear minimization is performed on the following criterion:

arg min S(t-1) [∆ω, ∆v] S(t) s i=1 W || p i -p i ||2 + || p i -p i ||2 (3)
where || • || represents the L2-Norm operator, s is the number of tracked stereo feature pairs, and W is the weighting matrix estimated by an M-estimator function [START_REF] Stewart | Robust parameter estimation in computer vision[END_REF] updated using an Iterative Re-weighted Least Squares algorithm (IRLS). This robust minimization converges into a solution by rejecting the feature points that are generated principally by mobile objects. A consistent solution is obtained if at least 50% of stereo feature points correspond to static objects (i.e. the environment). The criterion of Eq. 3 is minimized by using the Levenberg-Marquard Algorithm (LM) in an IRLS loop [START_REF] Stewart | Robust parameter estimation in computer vision[END_REF]. The convergence speed of the LM algorithm is increased using the planar ego-motion R(t-1) [∆ω 0 , ∆v 0 ] R(t) from the CAN-bus sensors, this information providing a close initialization guess and then helping to reduce the number of iteration cycles.

After convergence, the 3D localization of the vehicle with respect to the initial position S(t = 0) is estimated using the following state evolution equations.

Let S(t) = S [q(t), p(t)] be the 3D vehicle position at time t with q(t) = [q 0 q 1 q 2 q 3 ] T and p(t) = [p 0 p 1 p 2 ]

T representing the attitude as a unit quaternion and the vehicle position in meters. S(t) can be computed as follows:

q(t) = S(0) q S(t-1) S(t-1) q(∆) S(t) (4)
p(t) = S(0) q S(t-1) S(t-1) ∆v S(t) S(0) qS(-1) + ...

... S(0) t S(t-1) (5)
where q(∆ω) is the unit quaternion corresponding to the estimated rotation of the vehicle egomotion S(t-1) [∆ω, ∆v] S(t) , denotes the multiplication quaternion operator and q = [-q 0 q 1 q 2 q 3 ] T is the conjugated unit quaternion of q. Underlined vectors (e.g. p) denote expanded forms (i.e. p = [0, p] T ) for the use of the quaternion multiplication.

B. Experimental Real-time 3D Ego-Localization Results

A data set was acquired in an urban environment featuring low-rise buildings, trees and moving objects (i.e. pedestrians and vehicles). During the experiment, a landmark on the road was employed as a start/stop ground truth of the vehicle's trajectory and the vehicle's speed was limited to 30 Km /h. The vehicle trajectory was a closed loop featuring pedestrians and vehicles. Lowtextured scenes (e.g. rural environments and parking lots) were not considered in this study.

The 3D ego-localization function is implemented in C/C++. The 3D trajectory is reconstructed in real time and is obtained by integrating the ego-motion estimations. Fig. 8 illustrates one of the tests performed. It consists of a 270 m-clockwise loop (i.e. 90 s video sequence duration).

These results show that the cooperative strategy helps to cope with errors in the CAN-sensor based odometry, mainly due to wheel slippage and gyrometer bias drift. This technique also improved the visual odometry performance in adverse conditions (e.g. high rotational speed in 90°turns and roundabouts). These improvements were obtained thanks to the planar motion initialization which reduces the risk of converging into a local minimum ego-motion solution. It also improves outlier rejection and reduces the minimization iteration cycles. The 3D ego-localization system performs quite well in situations where GPS cannot provide a precise position (see the GPS jumps shown in the upper part of Fig. 8). The bird's eye view of the estimated trajectories in Fig. 8 shows the gaps at the loop closing point. The 2D trajectory obtained using the WSS-Gyro based odometry (the blue curve) achieves an acceptable drift representing 1.84% of the total traveled distance (i.e. 4.17 m for a closed loop).

The total drift of the multi-modal strategy, computed as the Euclidean distance between the starting and the final trajectory position, was 1.9% of the total traveled distance. However, the drift of the planar trajectory projection of the multi-modal algorithm corresponds to 0.58% only, which represents an error 3 times lower than that obtained from the WSS-Gyro based odometry. The goal of this stage is to estimate the planar trajectory of a set of objects as they move in the 3D scene using a multi-modal approach (i.e. vision, lidar and WSS-Gyro sensing modalities). Object tracking also helps to maintain the temporal coherence of the dynamic map and to provide information about the objects' speeds and sizes.

The proposed multi-modal strategy starts by detecting objects using a lidar-based technique only. Objects are then tracked using a Kalman filter algorithm with motion constraints in a characterized zone of interest. These constraints correspond to assumptions made in order to simplify the object tracking problem.

The tracking strategy is presented in four parts: object detection; maneuvering window identification; track prediction; object-track association and updating. At the end of this section, experimental results are reported to show the effectiveness of the approach.

A. Maneuvering Window

Urban environments are complex, dynamic and completely variable. Large numbers of static and mobile objects may considerably reduce the computational efficiency of an object tracker. A key issue here is identifying objects which constitute potential maneuvering targets so that computational resources can be mainly devoted to tracks requiring particular attention. Moreover, many incorrect observation-to-track pairings can be avoided. In the literature, the concept of a maneuvering window has been widely studied in military MTT 1 applications [START_REF] Blackman | Design and Analysis of Modern Tracking Systems[END_REF]. This concept has been recently proposed for pedestrian detection applications [START_REF] Fayad | Tracking objects using a laser scanner in driving situation based on modeling target shape[END_REF], where this zone is estimated based on the egovehicle state (typically speed and steering angle).

For the present study, the research space is restricted to a part of the 3D scene which is identified based only on prior knowledge of the environment (e.g. ground plane / urban canyon). Restricting the research space in this fashion provides the tracking algorithm with the ability to efficiently focus the computational resources on a maneuvering space, where collisions might be predicted at appropriate reaction times. Fig. 10 provides an example of the maneuvering window concept in an urban environment. The method is based on the detection of maxima in lidar scan histograms. A first lidar data filtering is performed not only to improve the maneuvering window detection, but also to significantly decrease scene clustering issues. This filtering consists in detecting the 3D lidar data corresponding to the road surface. Detection makes use of the characteristic pattern observed when 1 Multiple Target Tracking the two lower layers intersect the road plane at different angles, as illustrated in Fig. 11. The second lidar layer can be predicted from the first lidar layer and from the geometrical constraints. The measurements belonging to the road plane are detected and excluded from further processes, when the Euclidean error between the predicted and the measured layer falls below a predefined threshold. The pitch angle of the ML-lidar, denoted ρ, is estimated by a temporal filtering and is updated using the detected road impacts. The parameters h (ML-lidar height) and γ (inter-layer angle) are considered known. The maneuvering window is characterized by two local limits in the x-axis direction of the lidar frame. As illustrated in Fig. 12, a 4-layer data scan is projected onto the L XY plane (see Fig. 12a) and provides an easy-to-exploit histogram L Y axis (see Fig. 12b). Objects like security barriers, walls and parked vehicles efficiently reduce the maneuvering window. The detected limits are finally filtered using a fixed-gain Luenberger observer in order to reduce the oscillations produced by important pitch changes situations [START_REF] Fayad | Tracking objects using a laser scanner in driving situation based on modeling target shape[END_REF].

B. Object Detection

The object detection function provides a list of 2D objects at each scan cycle. The detection involves a 3D point clustering stage which can be efficiently implemented using maximal Euclidean inter-distance [START_REF]Object-level fusion and confidence management in a multi-sensor pedestrian tracking system[END_REF]. Predefined geometrical features [START_REF] Nashashibi | Vehicle recognition and tracking using a generic multisensor and multialgorithm fusion approach[END_REF] are an alternative, but they require prior knowledge of objects. The output objects are characterized by their planar position in the lidar frame L, their dimension (i.e. bounding circle) and detection confidence indicators [START_REF]Object-level fusion and confidence management in a multi-sensor pedestrian tracking system[END_REF].

The list of 2D object positions provided by the ML lidar at time t are transformed into the camera frame S and finally reported in a world frame (i.e. local dynamic map), W, by compensating for the vehicle's motion (see section III-A).

For instance, let L y(t) = [x y 0] T be the coordinates of a detected object at time t in the lidar frame, as illustrated in Fig. 14. Its corresponding localization in W can be computed as follows, by composing two rigid transformations:

S(t) y(t) = S qL L y(t) S qL + S t L (6)
W y(t) = W q S(t) S(t) y(t) W qS(t) + W t S(t) [START_REF] Broggi | A new approach to urban pedestrian detection for automatic braking[END_REF] where S(t) y(t) and W y(t) are respectively the coordinates of the detected object in the SVS and in the world frame at time t. It will be recalled that the transformation denoted as S [q, t] L refers to the geometrical transformation between the SVS and the ML lidar frames, determined via an extrinsic calibration procedure.

Object Detection and Localization

Detects lidar objects and incorporates them in a fixed-reference frame (World)

Detected objects in world frame

Multi-modal 3D Odometry 3D Ego-Vehicle positioning Assuming that the objects included in the maneuvering window locally follow a planar motion, the object state can be efficiently reduced to the W XZ components of its Cartesian coordinates in the world frame. By abuse of notation the object state is represented as W y(t) (x,z) . The uncertainty of the lidar object localization is modeled through a zero-mean, white, Gaussian measurement noise with a covariance denoted N.

ML-lidar Scan points

Lidar to SVS frame SVS to World frame

The track state is denoted by W y(t) and is composed of the W XZ plane coordinates, [x(t), z(t] in meters, and the planar velocity [v x (t), v z (t)] in m•s -1 stacked in a 4D vector as follows:

W y(t) = x(t) z(t) v x (t) v z (t) T (8)
This notation is intended to represent the knowledge of W y(t) obtained by combining all the information acquired up to time t. In addition to the state parameters, other attributes are handled in parallel, including the object size in meters (i.e. bounding circle radius) and the creation and update time-stamps in µs.

C. Track Prediction

The studied tracking system detects and tracks objects usually present in urban environments, such as pedestrians, cyclists and vehicles. Since their movements are linked to a fixed reference with a sampling frequency assumed to be sufficiently high, these movements can be taken as locally linear with a constant speed during a sample interval, ∆t. This model is then given by:

A T = I 2×2 ∆t • I 2×2 0 2×2 I 2×2 (9)
with I 2×2 as the 2 × 2 identity matrix. Accordingly, the prediction of the track state is given by the following evolution equations:

W y(t) = A T • W y(t -1) + α T (t) W y(t) (x,z) = C T • W y(t) + β T (t) (10)
with

C T = I 2×2 0 2×2 . W y(t|t-1)
is the predicted state of the tracked object, W y(t) (x,z) is the observed track location, and C T is the observation matrix and ∆t is the sampling time period, which is not constant. α T (t) and β T (t) are additive errors considered as white zero-mean Gaussian noises.

D. Object-Track Association and Updating

After an initial object detection sampling, a new set of tracks is created and all track parameters are set up. When the next incoming object arrives (i.e. MLlidar sampling), iterative tracking actions are performed, starting with a state prediction of all the tracks at the current sampling time. The association process consists in selecting and assigning the closest new detected object to the predicted track position. It uses a nearest neighbor criterion based on the Generalized Statistical Distance metric d (.) which maximizes the object-track association probability [START_REF] Blackman | Design and Analysis of Modern Tracking Systems[END_REF]:

d W y(t|t -1), W y(t) (x,z) = µ(t) T Σ -1 µ(t) + ln |Σ| (11)
with µ(t) = C T • W y(t|t -1) -W y(t) (x,z) , the state innovation, Σ = M(t|t-1)+N, the residual covariance matrix and M is the covariance matrix of the track.

In order to cope with object occlusions, the nonassociated tracks are kept for a fixed time interval (for example, 2 s). The non-associated objects in the maneuvering window generate new tracks until the algorithm reaches a maximum number of tracked objects.

The tracks' states and their corresponding covariances are updated using the information provided by the associated lidar object positions and classical Kalman filter equations.

Nearest neighbor (NN) methods imply singlehypothesis association approaches. These methods address ambiguous assignments by retaining the best available observation (i.e. the closest one). More sophisticated methods can address this issue, one example being Joint Probabilistic Data Association (JPDA) which is an extension, for the MTT problem, of the Probabilistic Data Association (PDA) in Single Target Tracking (STT) [START_REF] Blackman | Design and Analysis of Modern Tracking Systems[END_REF].

We have limited the scope of this study to the test of a simple assignment method that provides acceptable results through the use of a maneuvering window to filter and considerably reduce the number of candidates (which may be large in urban conditions.)

E. Experimental Results

3D ego-localization, maneuvering window detection and object-detection are real-time functions whose results have been logged (see the function scheme in Fig. 2). Fig. 15 shows the mean output frequencies of the 3D localization function and the ML lidar-based functions (namely maneuvering window and object detection). It will be observed that the convergence time of the 3D ego-localization function is not constant because it depends on the vehicle motion and the varying complexity of the scene.

The object tracking function was implemented with MATLAB. The reported results were obtained in an Focusing now on the kinematic state estimation of the tracked objects, Fig. 17 illustrates a zoomed area of the dynamic map. This zoomed area shows some state samples of a tracked vehicle that is accelerating and its corresponding track re-projection on the left SVS camera image.

Since our implementation provides a precise time reference (in µs), a reliable indicator of the correctness of the obtained results can be obtained by projecting predictions about tracked objects onto acquired images. Images here are only used as a means of identifying ground truth.

At the bottom of the figure, the size of the track is represented by its bounding circle, in red, and its center as a red triangle. The corresponding image track projections (3D red boxes) and their speed vectors (green line) are also illustrated in the upper part of the figure. The projected bounding box encloses the 3D cylinder of the track.

Additionally, Fig. 17 shows how the size of a detected track changes as the object surface is impacted by the ML lidar. This fact sometimes entails perturbations in the speed estimation, since large changes in size give rise to a spurious motion in the track centroid. This problem has been addressed in [START_REF] Petrovskaya | Model based vehicle tracking in urban environments[END_REF], [START_REF] Fayad | Tracking objects using a laser scanner in driving situation based on modeling target shape[END_REF], where the spurious centroid displacement and the occlusions were dealt with using a model based vehicle tracking. Looking at the image projection of the track speed vector, however, one can see that the multi-modal system performs quite well. It is worth mentioning that objects are tracked even if they go out of the SVS field-of-view.

Fig. 18 shows another section of the dynamic map. Estimating the speed of a pedestrian is a challenge, since pedestrians' movements can be unpredictable. However, a linear motion at constant speed has shown to be extremely pertinent. The reconstructed trajectory illustrated in Fig. 18 corresponds to the pedestrian's real trajectory. Indeed, the pedestrian is following a semi-circular path while crossing the road and the estimated speed is in accordance with the real one of the pedestrian who was motionless at the beginning and reaches a usual human walking speed [START_REF] Simms | Pedestrian and Cyclist Impact[END_REF]. The filter converges in less than 0.4s (i.e. five laser scans in average), giving information on the kinematic characteristics of the tracks that are difficult to estimate using only a lidar.

In Fig. 19 a wheelchair pedestrian moving slow is successfully tracked even if the ego-vehicle has a high rotational speed because of a 90°-turn. Thanks to the accurate lidar measurements and the good estimation of the vehicle displacement, the tracking system is able to correctly recover the motion of the mobile. Indeed, the wheelchair goes firstly on the sidewalk, then follows it and finally turns right.

V. CONCLUSION AND FUTURE WORK

An embedded multi-modal system for object localization and tracking has been proposed and experimentally validated. The presented approach provides a 3D dynamic map of the vehicle's surroundings. The method merges sensor data to achieve a robust and precise 3D ego-localization that is crucial in compensating for the displacement of the ego-vehicle in the tracking process. This function is combined with a lidar-based object tracking focused in a maneuvering window, providing objects' trajectories and speeds as they move in space. The results obtained facilitate analysis and understanding of the scene, and can be used for ADAS (e.g. collision detection and avoidance).

The conducted investigations have led us to identify interesting clues which worth pursuing in a future research. A first perspective concerns the real-time implementation of the presented tracking approach which could be possible thanks to its low complexity. A further improvement of the complete perception architecture would allow the system to deal with dense traffic and crowded urban scenarios. Another perspective aims at improving trust in object detection and tracking through the use of a visual confirmation features which would allow to perform a statistical evaluation of the system performance. Finally, anyone interested in testing a new approach on the real dataset can address a request to the authors.

VI. ACKNOWLEDGMENTS

The authors would like to thank Fadi Fayad for the implementation of the ML lidar functions and Gerald Dherbomez and Thierry Monglon for their experimental support

Figure 1 :

 1 Figure 1: A dynamic map corresponds to a list containing the kinematic states of the tracked objects and the vehicle dynamics in the 3D scene

Figure 2 :

 2 Figure 2: Multi-Modal Strategy: perception functions and their interactions for the object localization and tracking

Figure 5 :

 5 Figure 5: Coordinate systems III. MULTI-MODAL 3D ODOMETRY

Figure 6 :

 6 Figure 6: Yaw rate-WSS for planar odometry estimation Using successive stereo image pairs, denoted I * , I * and I , I (see Fig.7), we obtain a set of tracked stereo feature points, p , p , and their corresponding optical flow constituting the image motion. For this purpose a set of stereo feature points, p * , p * , is extracted using Harris features associated with a ZNCC (Zero-mean Normal Cross Correlation) correlation criterion and image constraints (disparity and epipolar constraints). Here, p * i and p * i are defined as the projection of an observed 3D point P. The stereo features, p * , p * , are tracked over time using the Lucas-Kanade method[START_REF] Bouguet | Pyramidal implementation of the lucas kanade feature tracker description of the algorithm[END_REF], thus defining the tracked stereo feature points set p , p .

Figure 7 :

 7 Figure 7: Quadrifocal warping principle

Figure 4 :

 4 Figure 4: Localization system overview

  Bird's eye view of the closed-loop trajectory

Figure 8 :

 8 Figure 8: 3D Reconstructed Trajectory. The circled region evidences GPS jumps, while visual odometry provides smooth estimates.

Figure 9 :

 9 Figure 9: Aerial view of the GPS and multi-modal 3D odometry trajectory projections

Figure 10 :

 10 Figure 10: Maneuvering window

Figure 11 :

 11 Figure 11: Characteristic pattern observed when two layers of the ML lidar intersect the road plane. This geometrical constraint is used to identify which scan points lie in the road plane so as to exclude them from further processes.

  (a) Bird's eye view of laser scan (b) Lidar scan projection into the y-axis

Figure 12 :

 12 Figure 12: Maneuvering window identification using a y-axis histogram of the ML lidar scan points

Figure 13 :

 13 Figure 13: ML lidar y-axis histogram peaks in turns may fade out. To deal with this, the detected limits are filtered using a fixed-gain Luenberger observer.

Figure 14 :

 14 Figure 14: The detected objects are localized in the world frame by compensating for vehicle motion.

Figure 15 :

 15 Figure 15: Real-Time Output Frequency of 3D Ego-Localization and ML lidar based Functions

Figure 16 :

 16 Figure 16: Reconstruction of the maneuvering window W XZ plane view . Notice the convergence of the filter after initialization. Reduced sections are due to parked vehicles

Figure 18 :

 18 Figure 18: Trajectory of a pedestrian (XZ plane view)

Figure 19 :

 19 Figure 19: Wheelchair pedestrian trajectory observed from the moving vehicle in a 90°turn

Figure 17 :

 17 Figure 17: Trajectory of a tracked vehicle represented by red boxes. Track speed vector in green. The detected track size is illustrated by red circles. Black circles represents track covariance uncertainty.