Abstract

This paper presents a simultaneous localization and mapping algorithm suitable for large-scale visual navigation. The estimation process is based on the viewpoint augmented navigation (VAN) framework using an extended information filter. Cholesky factorization modifications are used to maintain a factor of the VAN information matrix, enabling efficient recovery of state estimates and covariances. The algorithm is demonstrated using data acquired by an autonomous underwater vehicle performing a visual survey of sponge beds. Loop-closure observations produced by a stereo vision system are used to correct the estimated vehicle trajectory produced by dead reckoning sensors.