Real-Time Vehicle Positioning and Mapping Using Graph Optimization

Abstract

In this work, we propose and evaluate a pose-graph optimization-based real-time multi-sensor fusion framework for vehicle positioning using low-cost automotive-grade sensors. Pose-graphs can model multiple absolute and relative vehicle positioning sensor measurements and can be optimized using nonlinear techniques. We model pose-graphs using measurements from a precise stereo camera-based visual odometry system, a robust odometry system using the in-vehicle velocity and yaw-rate sensor, and an automotive-grade GNSS receiver. Our evaluation is based on a dataset with 180 km of vehicle trajectories recorded in highway, urban, and rural areas, accompanied by postprocessed Real-Time Kinematic GNSS as ground truth. We compare the architecture's performance with (i) vehicle odometry and GNSS fusion and (ii) stereo visual odometry, vehicle odometry, and GNSS fusion; for offline and real-time optimization strategies. The results exhibit a 20.86% reduction in the localization error's standard deviation and a significant reduction in outliers when compared with automotive-grade GNSS receivers.

Keywords: multi-sensor fusion; pose-graph optimization; vehicle localization.

Figure 1

Multi-sensor fusion for vehicle positioning and mapping using automotive-grade sensors. The sensor data are used to model a pose-graph, which is then optimized in real-time to estimate the vehicle’s accurate pose and generate a map.

Figure 2

A simple graph before (a) and after (b) optimization [29]. The initial node X_i is kept fixed. The node X_j is at its initial position before optimization. The nodes X_i and X_j are connected by an edge (measurement) Z_ij. The black dashed circle visualizes the measured position of node X_j contained in the edge Z_ij. The error vector e_ij before optimization is depicted as a red dashed line. After the optimization, this error is minimized by moving node X_j to the position according to the measurement contained in Z_ij.

Figure 3

PGraph modeling strategies G1, G2, and G3, in (a–c), respectively [29]. The black circles are the absolute vehicle poses initialized from the odometry, and the black arrows are the corresponding edges. The blue arrows are the GNSS edges connecting the UTM origin node (black circle with a cross) with the corresponding nodes. In (a), the black dashed circles represent the GNSS readings, and the error is depicted by a red dashed line. In (b), the blue circles are the GNSS readings nodes. The green arrows represent the (virtual) identity edges. In (c), The blue circles with crosses are the GNSS nodes that are kept fixed during optimization.

Figure 4

Pose-graph based sensor fusion framework for vehicle positioning and mapping.

Figure 5

Circle feature matching process. The green boxes represent the matched feature point and the blue boxes represent the search space. The red arrows represent the sequence of the matching process.

Figure 6

The red arrows represent a window; a new window is generated at every multiple of w meters traveled by the vehicle, with all the nodes contained in the last l meters of the last optimized window, which creates a hopping effect [30].

Figure 7

The red arrow represents a pose-graph window; the window size increases with every new measurement and is optimized after the vehicle travels every b meters [30].

Figure 8

Overview of our eight datasets recorded around Eindhoven, The Netherlands. Each dataset is depicted in a different color [29].

Figure 9

(a–h) shows the GNNS position errors with respect to the postprocessed RTK-GNSS ground truth for the eight datasets. The plots with a tile size of 2 × 2 m, clearly show that GNSS errors are biased and correlated, causing nonzero mean behavior for time spans up to 65 min [29].

Figure 10

(a) Batch size vs. average precision plot for different window sizes (500, 1000, 1500, 2000, 2500 in meters) of all datasets [30]. (b) Batch size vs. total time plot for different window sizes. The configuration with a window of 1500 m and a batch size of 40 m is marked with a black dot [30].

Figure 11

(a,b) shows the average computation time and the update frequency for one optimization iteration for different batch sizes, respectively [30].

Figure 12

(a–c) shows the fusion results projected onto the Google map. (d–f) shows the fusion results projected on to the Google street view. Where, RTK GNSS (red), GNSS (yellow), Offline Global optimization of vehicle odometry and GNSS (GO1) in blue, incremental hopping window optimization of vehicle odometry and GNSS (LO1) in green, Offline Global optimization of SVO, vehicle odometry and GNSS (GO2) in orange, incremental hopping window optimization of SVO, vehicle odometry and GNSS (LO2) in pink.