Toward Certifying Maps for Safe Registration-based Localization Under Adverse Conditions

TL;DR

This work tackles the safety certification of ICP-based localization under non-Gaussian, adversarial measurement faults arising from adverse conditions. It develops a closed-form worst-case pose-error bound for corrupted measurements $\mathbf{y} = \mathbf{A}\mathbf{x} + \mathbf{w} + \mathbf{Q}\mathbf{f}$ and a sector-based map certification framework to identify vulnerable regions via a resilience metric. By linearizing ICP and constraining inlier faults, the authors derive hazard probabilities using $p(|e_j| > r_j) = \min\{2\big(1 - \Phi_{\mu,\sigma}(r_j)\big), 1\}$ with $\mu$ and $\sigma$ defined from the system, enabling scalable analysis across maps. Experiments on urban, forest, and subterranean datasets show varying resilience linked to environmental structure, offering a practical tool to locate dangerous regions and guide map design for safer localization.

Abstract

In this paper, we propose a way to model the resilience of the Iterative Closest Point (ICP) algorithm in the presence of corrupted measurements. In the context of autonomous vehicles, certifying the safety of the localization process poses a significant challenge. As robots evolve in a complex world, various types of noise can impact the measurements. Conventionally, this noise has been assumed to be distributed according to a zero-mean Gaussian distribution. However, this assumption does not hold in numerous scenarios, including adverse weather conditions, occlusions caused by dynamic obstacles, or long-term changes in the map. In these cases, the measurements are instead affected by large and deterministic faults. This paper introduces a closed-form formula approximating the pose error resulting from an ICP algorithm when subjected to the most detrimental adverse measurements. Using this formula, we develop a metric to certify and pinpoint specific regions within the environment where the robot is more vulnerable to localization failures in the presence of faults in the measurements.

Figures (5)

• Figure 1: In the context of pose estimation, specific environments entail higher risks compared to others. For instance, a large intersection a) offers limited landmarks for localization, whereas a suburban area b) presents numerous houses and landmarks. Consequently, occlusions or map alterations at the intersection may lead to a significantly larger pose estimation error compared to the suburban area.
• Figure 2: Examples of faulted pointclouds from underground passages of the DARPA Subterranean challenge finals ebadi2022present. The faults try to corrupt the $y$ component of the pose, with the corrupted pose estimated by ICP in red, and the ground truth pose in black. Light grey points correspond to the reference map. The measured lidar point cloud is illustrated in green, with a sector (shaded blue) being corrupted. The blue and red points correspond to the sector before and after corruption, with gray arrows showing the shift induced by the corruption. The corruption offsets points in different directions and at different angles to achieve a maximum error in the $y$ component. Measured points corresponding to the ceiling and floor are omitted for better visualization.
• Figure 3: Signed difference of ICP errors between our theoretical estimate and real ICP for different inlier threshold distances. The violins depict the overall distribution of the difference. The medians are represented as black dots and the inner 50% of the data is depicted in shaded white. Bold numbers under the violins correspond to the associated ratio of false negative (i.e., negative signed error). Our estimate typically yields larger errors compared to real ICP, making it a reasonable approximation of an upper bound.
• Figure 4: Glen Shields trajectory of the Boreas dataset, colored by its resilience to corruption $R$. The vehicle starts on the top right of the map, before driving on a large main road. It then leaves the road for smaller streets with better features. Highlighted places a) and b) correspond to the left and right pictures of \ref{['fig:intro']}.
• Figure 5: Path A of the Montmorency Forest Wintertime dataset, colored by its resilience to corruption $R$. The robot starts in a garage at the bottom of the map, proceeds to drive by several buildings, and finally enters a narrow ski trail surrounded by tall pine trees.