Two-stage pose optimization algorithm using color information for underwater SLAM with light-sectioning-based 3D scanning method

The demand for 3D shape measurement of underwater scene is increasing in various applications. Especially, simultaneous localization and mapping (SLAM) technique utilizing remotely operated vehicle (ROV) attached with 3D sensors has been intensively researched. This paper focuses on solving pose optimization problem for underwater robots with camera/multiple-line-lasers setup, especially for the scene with some textures (color information). To this end, a two-stage pose optimization technique is proposed. In the first stage, due to the sparse nature of the reconstructed shape in the light-sectioning method consisting of several 3D curves, we bundle 10 to 20 consecutive frames to form a block shape, refining significant errors in the initial sensor poses using a novel bundle adjustment algorithm. In the second stage, remaining pose errors are corrected by a block-based matching algorithm utilizing iterative closest point (ICP) algorithm with color information. Through experiments in underwater environment with a real system, it was validated that the proposed method demonstrates superior performance compared to past underwater SLAM techniques.