Degeneracy-aware factors with applications to underwater SLAM

Abstract

Simultaneous Localization and Mapping (SLAM) is commonly formulated as an optimization over a graph. A popular approach is the pose graph, which seeks to solve for robots poses that are constrained by pose-to-pose measurements, such as odometry measurements or loop closures. For range sensors, these pose-to-pose constraints can be achieved by performing scan matching techniques, such as Iterative Closest Point (ICP). However, in environments with insufficient or degenerate geometric features, the ICP solution can be unreliable and lead to significant drift in the trajectory of the graph optimization solution. In this paper, we propose a degeneracy-aware approach which has two stages: (1) a degeneracy-aware ICP algorithm and (2) a partially constrained loop closure factor to incorporate the results from (1) into the SLAM pose graph optimization. Our approach performs updates and optimizes both ICP and the pose graph in only the well constrained directions of the state space. These directions are selected on the basis of a dynamic threshold, which updates at each iteration. We apply the proposed algorithm to autonomous underwater mapping with sonar. To evaluate the performance of this algorithm, we conduct experiments in both simulation and real world scenarios, and show the method's robustness to navigational drift and ability to reject poor loop closures in degenerate environments, which would otherwise degrade the accuracy of the trajectory and the quality of the resulting map.