DegeneracyAware Factors with Applications to Underwater SLAM 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 posetopose measurements, such as odometry measurements or loop closures. For range sensors, these posetopose 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 degeneracyaware approach which has two stages: (1) a degeneracyaware 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 methodxe2x80x99s 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.

SVIn2: An Underwater SLAM System using Sonar, Visual, Inertial, and Depth Sensor This paper presents a novel tightlycoupled keyframebased Simultaneous Localization and Mapping (SLAM) system with loopclosing and relocalization capabilities targeted for the underwater domain.Our previous work, SVIn, augmented the stateoftheart visualinertial state estimation package OKVIS to accommodate acoustic data from sonar in a nonlinear optimizationbased framework. This paper addresses drift and loss of localization xe2x80x93 one of the main problems affecting other packages in underwater domain xe2x80x93 by providing the following main contributions: a robust initialization method to refine scale using depth measurements, a fast preprocessing step to enhance the image quality, and a realtime loopclosing and relocalization method using bag of words (BoW). An additional contribution is the addition of depth measurements from a pressure sensor to the tightlycoupled optimization formulation. Experimental results on datasets collected with a custommade underwater sensor suite and an autonomous underwater vehicle from challenging underwater environments with poor visibility demonstrate performance never achieved before in terms of accuracy and robustness.