Long-Term Map Maintenance in Complex Environments

Abstract

As changes in external environments are inevitable, a lifelong mapping system is desirable for autonomous robots that aim at long-term operation. Capturing external environment changes into internal representations (for example, maps) is crucial for proper behavior and safety, especially in the case of autonomous vehicles. In this work, we propose a new large-scale mapping system for our autonomous vehicle or any other. The new mapping system is based on the Graph SLAM algorithm, with extensions to deal with the calibration of odometry directly in the optimization of the graph and to address map merging for long-term map maintenance. The mapping system can use sensor data from one or more robots to build and merge different types of occupancy grid maps. The system’s performance is evaluated in a series of experiments carried out with data captured in complex real-world scenarios. The experimental results indicate that the new large-scale mapping system can provide high-quality occupancy grid maps for later navigation and localization of autonomous vehicles that use occupancy grid maps.

This study was financed in part by Coordenação de Aperfeiçoamento de Pessoal de Nível Superior – Brasil (CAPES) – Finance Code 001; Conselho Nacional de Desenvolvimento Científico e Tecnológico - Brasil (CNPq); and Fundação de Amparo à Pesquisa do Espírito Santo - Brasil (FAPES) – grants 75537958 and 84412844.

1 Introduction

Autonomous Vehicles (AVs) typically use maps of the environment to localize themselves and safely navigate in it. In this context, AVs can localize themselves in relation to the map using their sensor measurements (localization problem). When there is no map, they can create a new one also using their sensors. Vehicle poses can be known when creating the map (mapping problem) or can be estimated as the map is built (simultaneous localization and mapping - SLAM - problem [4]).

One of the requirements imposed on AVs is the ability to operate for a long period of time without human intervention. Therefore, AVs must display a life-long mapping capability and promptly update their internal maps whenever the environment changes. In addition, when operating outdoors, AVs face highly dynamic objects (for example, pedestrians and other vehicles) and slightly dynamic objects (for example, parked vehicles and traffic cones). Environments may undergo slower modifications to the infrastructure and, therefore, the AVs’ internal maps must be updated accordingly. These issues, altogether, pose the life-long mapping problem, which usually requires merging the current map already built with the new ones from another surveying mission with a single robot or multiple robots at the same time. The map merging problem consists of combining two or more individual maps without a common frame of reference into a larger global map.

Fig. 1.

Example of map merging. The lack of loop closure treatment between the sessions results in misalignment between the maps.

One of the main challenges in the map merging problem is illustrated in Fig. 1, where red and green pixels represent obstacles reached by a LiDAR sensor and projected in the ground. Red pixels represent obstacles in a map while green pixels represent obstacles in another. Considering the final map after improper merging, there is a prominent misalignment between the two individual maps.

We have developed a self-driving car (Fig. 2), whose autonomy system follows the typical architecture of self-driving cars [1]. Our self-driving car is based on a Ford Escape Hybrid tailored with a variety of sensors and processing units. Its autonomy system is composed of many modules, which include a localizer [6], a mapper [13], a moving obstacle tracker [15], a traffic signalization detector [14, 20], a route planner, a path planner for structured urban areas, a behavior selector, a motion planner [5], an obstacle avoider [10], and a controller [11], among other modules.

Fig. 2.

Our self-driving car and its autonomy software system. In light blue, perception modules. In orange, decision making modules. TSD denotes Traffic Signalization Detection and MOT, Moving Objects Tracking. Red arrows show the State of the car, produced by its Localizer module and shared with most modules; blue arrows show the car’s internal representation of the environment, jointly produced by several perception modules and shared with most decision making modules. See details in Badue et al. (2020).

In this work, we propose a new mapping system based on graph SLAM for solving the map merging problem in large outdoor environments. The proposed mapping system aims to provide high quality grid maps for autonomous driving. It is also able to calibrate the vehicle odometry using a new method applied in the graph optimization. The system receives as input one or more log files containing the surveying missions (or sessions) from a single or multiple vehicles, and it outputs the AVs’ poses in a global coordinate frame, the parameters of the odometry bias calibration for each vehicle and different types of grid maps.

We evaluated the performance of the proposed mapping system on logs containing high and low dynamics in real-world scenarios. The logs were collected using the available sensors in the AV over a period of two years. The new odometry bias calibration method was compared against an odometry calibration based on Particle Swarm Optimization (PSO) proposed by Mutz et al. [13]. The results indicate that the new method provides better calibration values in all logs. The new mapping features were also analyzed through mapping and map merging experiments. The results show that the proposed system is able to correctly estimate the AV poses in a global reference frame, and to correctly merge local maps without drifts and misalignment for all logs.

The remaining sections are organized as follows. In Sect. 2, we present related work. In Sect. 3, we detail the proposed mapping system. In Sect. 4, we describe the experimental methodology and, in Sect. 5, we discuss experimental results. Finally, in Sect. 6, we close with our conclusions and directions for future work.

2 Related Work

Progress on the Simultaneous Localization and Mapping (SLAM) problem can be measured in terms of a wide variety of algorithms, techniques and theoretical frameworks that have been developed in last decades. However, unresolved questions remain such as robustness and outlier rejection for long-term operations [4].

The map merging problem addresses theses open issues in different contexts of application, either with multiple robots running synchronously or with only one robot gathering data during multiple sessions. While the first is focused in collaborative strategies to efficient mapping, the later is oriented to long-term map maintenance in order to accommodate infrastructural changes along time. Both share the same main goal which is to produce a final aggregated map alongside the robot trajectories for all sessions.

The common approach in multi-robot scenarios is to fetch all maps using map-matching to find the relative map transformations and merge all maps into one final map alongside the corresponding trajectory of each robot in that map.

A system for real-time multi-robot collaborative SLAM is presented in Deutsch et al. [7]. The distributed robots use their own SLAM system to build local maps, but they also share information (features from images) with a central system, which builds a global pose graph including information from all robots. After the graph optimization, the central system sends the results to all robots, so they keep a consistent local position and map estimates. In contrast, in our solution, maps are built before autonomous operation on outdoor environments. Ours can handle any map size as it depends only in the available storage.

Sun et al. [19] convert the map merging problem into an image registration problem. When two grid maps contains overlapping areas, their approach tries to find the best transformation which aligns the maps. The algorithm extract features from both maps using the Harris corner detector and it uses the selected features to calculate a initial transformation matrix which is later refined by an iterative algorithm. In contrast, our method finds the common reference frame through the graph optimization by making usage of GPS and LiDARs sensors, hence the proper alignments are achieved before the map construction.

The approach in single-robot multi-session scenarios is to use the all maps using map-matching to find the relative map transformations and merge all maps into one final map alongside the corresponding trajectory of each robot in that map

A SLAM system using Normal Distribution Transform (NDT) maps is presented in Einhorn and Gross [9]. The authors propose a modification in the NDT mapping to handle free-space measurements which makes them better suited for SLAM. The map merging is achieved by the alignment of the NDT maps through registration techniques.

Bonanni et al. [3] present a SLAM method that considers the map as a graph of point clouds. In order to merge two maps, it visits each node in the first graph and tries to find the best match in the second graph. Normal Iterative Closest Point (NICP) [17] is used to find the registration between the point clouds. The method is similar to our solution as point clouds are used to provide the correct relationship between sessions. We use GPS and a nearest neighbor algorithm to find the best candidates in the second graph.

The work of Ding et al. [8] is meant for large scale outdoor environments, as ours, and use 3D LiDAR for loop closure detection whilst we use GPS. Their system is capable of building maps using multi-session data as ours. Their approach splits maps into submaps and use an efficient method for loop closure detection using feature vectors and a loop closure database using the kd-tree algorithm. Their method also takes care of loop closure outliers and actively detects changes in the environment (high and low dynamics).

3 Large-Scale Mapping System

In this section, we describe the proposed full SLAM-based mapping system. Despite being able to build many types of grid maps, the next sections will focus only on OGMs.

Figure 3 shows the architecture of the proposed mapping system. The Pre-processing and the Hypergraph Builder modules are responsible for handling low-level information coming from sensors, removing undesired measurements, and building the hypergraph. The Hypergraph Optimizer is responsible for estimating the AV poses along the sessions while also calibrating the odometry bias. The Mapper module builds and merges the maps.

Fig. 3.

System overview.

3.1 Pre-processing

The Pre-Processing module sorts all measurements by their timestamps and also discards measurements in which the AV is below a minimum speed MS. This module also removes a specific error on GPS measurements as illustrated in Fig. 4. Figure 4(b) shows the poor results after increasing the GPS error variance in a try to solve this issue. Our solution is illustrated on Fig. 4(c). The GPS measurements are clustered together if the distance between immediate neighbors is less than NeighborDistance. After clustering, all groups containing fewer elements than NeighborQuantity are removed.

Fig. 4.

(a) GPS error. (b) Increasing variance from left to right. (c) Clustering GPS and removing small groups.

3.2 Hypergraph Building

Nodes in the hypergraph represent AV poses and odometry biases. Edges represent constraints between the nodes.

A pose node $\boldsymbol{N}_{i}$ is added to the hypergraph for each sensor measurement $\boldsymbol{z}_{i}$. $\boldsymbol{N}_{i}$ refers to an unknown pose $\boldsymbol{x}_{i}$:

$$\begin{aligned} \boldsymbol{N}_{i} = \boldsymbol{x}_{i} = \left( x_{i}, y_{i}, \theta _{i} \right) \end{aligned}$$

(1)

where $(x_{i}, y_i)$ is the position of the vehicle in the ground plane and $\theta _{i}$ is its heading.

For each log, the Hypergraph Builder creates odometry edges connecting each pair $\boldsymbol{N}_{i}$ and $\boldsymbol{N}_{i+1}$. This procedure guarantees that all nodes in the hypergraph are connected by odometry measurements, therefore, there is no need to synchronize the sensors since the odometry is used o join all nodes from the same log. While traversing the nodes in the graph, the system maintains the current command $\boldsymbol{u}_{i} = (v_{i}, \varphi _{i}, \varDelta t)$. $\varDelta t$ is the time difference between $\boldsymbol{N}_{i}$ and $\boldsymbol{N}_{i+1}$. The velocity $v_{i}$ and steering angle $\varphi _{i}$ are obtained from the last odometry measurement and they are updated at each new odometry measurement. The odometry edge indicates a constraint between the nodes $\boldsymbol{N}_{i}$ and $\boldsymbol{N}_{i+1}$ through the error function $\boldsymbol{e}_{}^{o}(\boldsymbol{x}_i, \boldsymbol{u}_i)$.

We also simplify the notation by encoding the involved quantities in the indices of the error function as in [12]:

$$\begin{aligned} \boldsymbol{e}_{}^{o}(\boldsymbol{x}_i, \boldsymbol{u}_i) {\mathop {=}\limits ^{def.}} \boldsymbol{e}_{i}^{o}(\boldsymbol{x}, \boldsymbol{u}) \end{aligned}$$

(2)

The odometry constraint between each node pair is defined as:

$$\begin{aligned} \boldsymbol{e}_{i}^{o}(\boldsymbol{x}, \boldsymbol{u}) = (\boldsymbol{x}_{i + 1} \ominus \boldsymbol{x}_{i}) \ominus K(\boldsymbol{u}_{i}) \end{aligned}$$

(3)

where $\ominus$ is the inverse of the motion composition operator $\oplus$ as defined in [18], and K(.) uses the Ackerman steering geometry with understeer corrections [2] to compute a relative movement given the command $\boldsymbol{u}_{i}\in \boldsymbol{u}$, and $\boldsymbol{u}$ is a vector containing all commands. All odometry edges are combined in:

$$\begin{aligned} \boldsymbol{J}^{o} \left( \boldsymbol{x}, \boldsymbol{u}\right) = \sum _{i}^{n} \boldsymbol{e}_{i}^{o}(\boldsymbol{x}, \boldsymbol{u}^{\intercal })^{\intercal } \boldsymbol{\hat{\varOmega }}_{i}^{o} \boldsymbol{e}_{i}^{o}(\boldsymbol{x}, \boldsymbol{u}^{\intercal }) \end{aligned}$$

(4)

where $\hat{\varOmega }_{i}^{o}$ is the odometry information matrix at node $\boldsymbol{N}_{i}$ projected through the non-linear function K(.) via the unscented transformation [22].

Each node $\boldsymbol{N}_{i}$ created by a GPS measurement $\boldsymbol{z}_{i}^{GPS}$ receives a unary edge containing the error:

$$\begin{aligned} \boldsymbol{e}_{i}^{GPS} \left( \boldsymbol{x}\right) = \boldsymbol{x}_{i} \ominus \boldsymbol{z}_{i}^{GPS} \end{aligned}$$

(5)

All GPS edges constraints are combined in the total GPS error:

$$\begin{aligned} \boldsymbol{J}^{GPS} \left( \boldsymbol{x}\right) = \sum _{i} \boldsymbol{e}_{i}^{GPS}(\boldsymbol{x})^{\intercal } \boldsymbol{\varOmega }_{i}^{GPS} \boldsymbol{e}_{i}^{GPS}(\boldsymbol{x}) \end{aligned}$$

(6)

where $\varOmega _{i}^{GPS}$ is the inverse of the GPS covariance matrix at node $\boldsymbol{N}_{i}$.

We use LiDAR and GPS to compute intra-session loop closures as in [13]. however, we use an ad-hoc method to reduce the number of edges in the graph by only considering a new loop closure if it is at a certain distance from the previous one. We also extend this approach to handle inter-session loop closures. Inter-session loop closures provide the required relationships between the AV poses in multiple logs. A loop closure edge computes the following error:

$$\begin{aligned} \boldsymbol{e}_{i,j}^{L}(\boldsymbol{x}) = (\boldsymbol{x}_{j} \oplus \boldsymbol{s}^{L}) \ominus (\boldsymbol{x}_{i} \oplus \boldsymbol{s}^{L}) \ominus ICP_{i, j}^{L} \end{aligned}$$

(7)

where $\boldsymbol{x}_{i}$ and $\boldsymbol{x}_{j}$ are the AV poses that are considered as a loop closure, $\boldsymbol{s}^{L}$ is the pose of the LiDAR sensor in the AV reference frame and $ICP_{i,j}^{L}$ is a linear transform that aligns the point cloud $\boldsymbol{z}_{i}^{L}$ with the point cloud $\boldsymbol{z}_{j}^{L}$. We use a custom method based on the Generalized Iterative Closest Point (GICP) [16] algorithm to find the transform. The error below combines the constraints of all loop closure edges:

$$\begin{aligned} \boldsymbol{J}^{L} \left( \boldsymbol{x}\right) = \sum _{i} \boldsymbol{e}_{i,j}^{L}(\boldsymbol{x})^{\intercal } \boldsymbol{\varOmega }_{i,j}^{L} \boldsymbol{e}_{i,j}^{L}(\boldsymbol{x}) \end{aligned}$$

(8)

where $\varOmega _{i}^{L}$ is the inverse GICP covariance matrix.

Mutz et al. [13] showed that odometry measurements are subject to biases. This error can be propagated to the edges in the graph if not addressed. The authors proposed a PSO-based optimization method to calibrate the odometry bias before building the graph. The calibration is approximated by the following linear functions:

$$\begin{aligned} v_{i}^{'}= v_{i} b_{mult}^{v} \end{aligned}$$

(9)

$$\begin{aligned} \varphi _{i}^{'} = \varphi _{i}b_{mult}^{\varphi }+ b_{add}^{\varphi } \end{aligned}$$

(10)

where $b_{mult}^{v}$ is the velocity multiplicative bias, $b_{mult}^{\varphi }$ and $b_{add}^{\varphi }$ are the steering wheel angle multiplicative and additive bias. The authors employed a PSO-based optimization method using a fitness function, which computes the difference between GPS and dead reckoning poses. Each particle contains the odometry bias parameters, $\boldsymbol{B} = \left( b_{mult}^{v}, b_{mult}^{\varphi }, b_{add}^{\varphi }\right)$, and the best parameters are used to update all odometry measurements before the AV poses optimization.

We propose a new method, which is performed directly in the hypergraph optimization. A bias calibration node $\boldsymbol{N}^{B}_{j:k}$ contains the parameters of the Eqs. 9 and 10:

$$\begin{aligned} \boldsymbol{N}^{B}_{j:k} = (b_{mult}^{v}, b_{mult}^{\varphi }, b_{add}^{\varphi })_{j:k} \end{aligned}$$

(11)

The range subscript $j:k$ indicates that a single bias calibration node $\boldsymbol{N}^{B}_{j:k}$ can be related to any subgroup of AV poses $\boldsymbol{x}$ containing all consecutive poses from j to k.

As each session generates its hypergraph and each hypergraph have at least one bias calibration node, the system can calibrate the odometry bias on each session.

Each pair $\left( \boldsymbol{N}_{i}, \boldsymbol{N}_{i+1} \right)$ is linked to a calibration node, forming a hyperedge. We use the following error to calibrate the odometry bias:

$$\begin{aligned} \boldsymbol{e}_{i, j:k} \left( \boldsymbol{x}, \boldsymbol{B}, \boldsymbol{u} \right) = \left( \boldsymbol{x}_{i+1} \ominus \boldsymbol{x}_{i} \right) \ominus \overline{K}(\boldsymbol{B}_{j:k}, \boldsymbol{u}_{i}, \varDelta t) \end{aligned}$$

(12)

where $\overline{K}(.)$ is a function that takes the bias parameters $\boldsymbol{B}_{j:k}$ at $\boldsymbol{N}^{B}_{j:k}$ to update the command $\boldsymbol{u}_{i}$ through Eqs. 9 and 10, and then it calls K(.) to estimate a relative displacement. The global odometry bias calibration error is defined as:

$$\begin{aligned} \boldsymbol{J}^{B}&(\boldsymbol{x}, \boldsymbol{B}, \boldsymbol{u}) \nonumber \\&=\,\sum _{j:k} \sum _{i = j}^{k-1} \boldsymbol{e}_{i, j:k} \left( \boldsymbol{x}, \boldsymbol{B}, \boldsymbol{u} \right) ^{\intercal } \boldsymbol{\hat{\varOmega }}_{i}^{B} \boldsymbol{e}_{i, j:k} \left( \boldsymbol{x}, \boldsymbol{B}, \boldsymbol{u} \right) \end{aligned}$$

(13)

where $\hat{\varOmega }_{i}^{B}$ is the odometry information matrix projected through $\overline{K}(.)$ via the unscented transformation. In this case, the projection must be updated when the bias changes substantially.

Fig. 5.

Graph containing edges from odometry, GPS and loop closure.

Fig. 6.

Odometry bias calibration node and hyperedges.

The resulting hypergraph contains all AV pose nodes, the odometry bias calibration nodes and all required edges to impose the constraints defined in Eqs. 4, 6, 8 and 13. Figure 5 illustrates the relationships between the elements of the hypergraph, including pose nodes, odometry edges, a GPS edge, and a loop closure edge. Figure 6 displays an odometry bias calibration node and some calibration hyperedges represented by regions with different colors for better visualization.

3.3 Hypergraph Optimization

The hypergraph optimization is an iterative process limited by a pre-defined number of iterations and it is separated in two main steps. The first step maximizes only the likelihood of the AV poses by combining Eqs. 4, 6 and 8 while excluding Eq. 13:

$$\begin{aligned} \boldsymbol{J}_{1} \left( \boldsymbol{x}, \boldsymbol{u}\right) = \boldsymbol{J}^{o} \left( \boldsymbol{x}, \boldsymbol{u}\right) + \boldsymbol{J}^{GPS} \left( \boldsymbol{x}\right) + \boldsymbol{J}^{L} \left( \boldsymbol{x}\right) \end{aligned}$$

(14)

The second step swaps the node update strategy by handling only the odometry bias calibration:

$$\begin{aligned} \boldsymbol{J}_{2} = \boldsymbol{J}^{B} (\boldsymbol{x}, \boldsymbol{B}, \boldsymbol{u}) \end{aligned}$$

(15)

When the second step reaches a defined number of iterations, the new odometry biases are used to update all edges related to Eq. 7 by first updating the corresponding commands $\boldsymbol{u}$ using Eqs. 9 and 10. After these updates, the optimization process restarts from the first step. After repeating the two steps for a desired number of iterations, the optimization returns the AVs poses and the odometry calibration biases.

3.4 Large-Scale Environment Mapping

The map is represented in log-odds to avoid numerical instabilities [21]. The log-odds of a given cell $\boldsymbol{m}_{c}$ in the map is defined as:

$$\begin{aligned} l_{c} = \log {\frac{p \left( \boldsymbol{m}_{c}| \boldsymbol{x}, \boldsymbol{z}\right) }{1 - p \left( \boldsymbol{m}_{c}| \boldsymbol{x}, \boldsymbol{z}\right) }} \end{aligned}$$

(16)

where $p \left( \boldsymbol{m}_{c}| \boldsymbol{x}, \boldsymbol{z}\right)$ is the probability of $\boldsymbol{m}_{c}$ being occupied given $\boldsymbol{x}$ and $\boldsymbol{z}$.

Initially, all grid cells are set to an invalid state (e.g. -1.0) and, at each time step, if a cell $\boldsymbol{m}_{c}$ is in the LiDAR perceptual field then it’s current log-odds is updated:

$$\begin{aligned} l_{i,c} = l_{i-1,c} + ISM \left( \boldsymbol{m}_{c}, \boldsymbol{x}_{i}, \boldsymbol{z}_{i}^{L}\right) - l_{0} \end{aligned}$$

(17)

where $l_{i-1,c}$ is the previous log-odds of the cell $\boldsymbol{m}_{c}$, $l_{0}$ is the map prior probability, $\boldsymbol{z}_{i}^{L}$ is a LiDAR measurement and ISM(.) is the inverse sensor model function [21].

3.5 Large-Scale Map Merging

The Mapper module merges the grids maps in an incremental approach. A first map is created using any of the resulting output files from the hypergraph optimization. While the mapping process evolves, the system saves how many times each cell $\boldsymbol{m}_{c}$ is reached by the LiDAR rays in the current session. After the first map is complete, the system changes the rule defined in Eq. 17 and then only the cells with the hit counter below a threshold value K are updated:

$$\begin{aligned} l_{i, c} = {\left\{ \begin{array}{ll} l_{i-1,i} + ISM \left( \boldsymbol{m}_{c}, \boldsymbol{x}_{i}, \boldsymbol{z}_{i}^{L}\right) - l_{0}, &{} hits \left( \boldsymbol{m}_{c}\right) < K \\ l_{i-1,i}, &{} otherwise \end{array}\right. } \end{aligned}$$

(18)

We assume that the LiDAR sensor is reliable and after enough hits the system can freeze the resulting log-odds. This procedure can unintentionally keep the low dynamics fixed in a previous map, but we can easily return to the original update rule defined in Eq. 17 if the difference in time between the sessions are greater than that of parameterized in Sect. 4. Assuming that the main changes in the environment infrastructure takes a time greater than that, then the system can update the current map while discarding most of the undesired changes in the next sessions.

4 Experimental Methodology

In this section, we describe each one of the recorded sessions, and the metrics used to compare the odometry calibration methods and to evaluate the estimated AV poses. We also indicate all the experiments that evaluates the main features of the new proposed large-scale mapping system.

Fig. 7.

Satellite view of the environments. (a) University campus beltway (red), parking lot 1 (yellow), parking lot 2 (dark blue) and parking lot 3 (cyan). (b) Route from the university campus to another city (light blue). (Color figure online)

We collected data using the IARA platform. The AV was conducted by a human along the beltway of the university campus and within parking lots in it’s vicinity. Figure 7(a) shows the beltway (red) and the parking lots 1 (yellow), 2 (dark blue) and 3 (cyan). Log UB1 was acquired along the beltway at night, in the clockwise direction and without moving obstacles. Log UB2 was recorded in the morning, in counterclockwise direction, with moving objects and many parked vehicles. The Logs PL1, PL2 and PL3 are related to the parking lots. Finally, Fig. 7(b) shows the route from the campus to another city in which the AV crosses highways, rural and urban streets and a long bridge. The TRVL log was recorded along this route which has about 72 km of extension.

4.1 Experiments

We carried out a series of experiments to evaluate the odometry calibration and the map building and merging proposed methods.

The main parameters of the proposed mapping system used in our experiments are the Minimum Speed (MS) set to 0.01 ms in the speed filtering, the Neighbor Distance (ND) and the Neighbor Quantity (NQ) thresholds used in the GPS clustering are set to 1.2 m and 40, respectively. The cell hits threshold (K) represented in Eq. 18 is set to 64. The time distance between sessions (ISTD) is set to 1month for low dynamics consideration.

Odometry Bias Calibration. In this experiment, we compare the PSO-based optimization [13] and the proposed hypergraph optimization methods for calibrating odometry biases. We use the mean absolute error (MAE) metric to compare the paths obtained by both calibration methods (PSO-based and hypergraph) against the GPS. A better odometry calibration should produce lower MAE values as the resulting path should better approximate GPS measurements.

Map Building and Merging. In this experiment, we compare the estimated AV poses with the GPS measurements for each log using the MAE metric. We constructed a single map using all logs in order to verify the merging capabilities of the proposed system. We illustrate same obstacles viewed in different visits to the same region with different colors in order to qualitatively evaluate the results of the map merging.

5 Results and Discussions

5.1 Odometry Bias Calibration

Table 1 compare the MAE results of the calibration using both methods. In all cases, the novel approach has found better parameters since it produces lower MAE values and the recovered paths are closer to GPS than both raw odometry and PSO. Figure 8 compares GPS and raw odometry with the paths computed by the two odometry bias calibration methods in the TRVL log.

Table 1. Odometry bias calibration using PSO and hypergraph optimization.

Fig. 8.

Odometry calibration - TRVL.

Fig. 9.

Map merging. (a) Errors in the OGM (UB1 and UB2). (b) Enabling inter-session loop closures. (c) Complete map.

5.2 Map Building and Merging

Given that our new system exploits all relevant sensor data, additional filtering strategies and LiDAR odometry, it was capable of handling the TRVL dataset and providing the maps in that large-scale context. Figure 9(a) illustrates the absence of a common reference frame while merging UB1 and UB2 without the inter-session loop closures while also using 18. Red pixels are the obstacles found in UB1 and green pixels are obstacles in UB2. Similar errors occur when we merge the remaining logs without using the inter-session loop closure to estimate a good reference frame. Figure 9(b) illustrates the removal of the errors after activating the inter-session loop closures between UB1 and UB2. Figure 9(c) presents the final merged map including all parking lots.

6 Conclusions and Future Work

In this work, we presented a new system for long-term map maintenance. The system uses multi-session data with the purpose of building and updating grid maps for posterior autonomous navigation and localization. The system deals with different types of sensors and can handle typical errors found in noisy sensor data. The Hypergraph Builder module includes intra-session and inter-session loop closures in order to estimate the AV poses in a global coordinate frame. The system also uses a new odometry bias calibration method to turn the odometry more reliable in the entire process. The Mapper module uses a custom method to freeze highly observed cells and so handle high and low dynamics in the map merging. We evaluated the system using datasets collected in an university campus and in a travel to another city. Experiments indicate that the new odometry bias calibration method surpass a previous PSO-based and also shows that the system can build and merge submaps into a global map using inter-session loop closures.

Equation 7 is used in Kümmerle et al. [12] to estimate the 2D pose of some laser scanner sensors directly in the hypergraph optimization. We did not obtain same quality results for the Velodyne HDL-32E in our AV. In a future work, we can investigate proper methods to calibrate the extrinsic of our LiDAR and cameras.