Robust world-centric stereo EKF localization with active loop closing for AUVs

Visual localization is a crucial task in Autonomous Underwater Vehicles (AUV) and it is usually complicated by the extreme irregularity of the natural aquatic environments, or by unfavorable water conditions. Visual Simultaneous Localization and Mapping (SLAM) approaches are widely used in land and represent the most precise techniques for localization, but applied underwater, they are still an open and ongoing challenge. This paper presents a general approach to visual 3D pose-based SLAM based on Extended Kalman Filters (EKF). This approach has a general design being applicable to any vehicle with up to 6 Degrees of freedom, so, it is particularly suitable for AUV. It uses only visual data coming from a stereo camera, all orientations involved in the system are represented in the quaternion space in order to avoid the gimbal lock singularities, and the sparsity of the covariance matrix is guaranteed during the whole trajectory since the state vector only includes the vehicle global pose. The vehicle pose is continuously predicted by means of a stereo visual odometer, and eventually corrected with the pose constraints given by a particularization of the Perspective N-Point problem (PNP) [1], applied to the registration of images that most likely close a loop. Experimental results show the important pose corrections given by the SLAM approach with respect to a ground truth, compared with the evident trajectory errors present in the visual odometer estimates.