Robust Initialization of Underwater Monocular Visual–Inertial–Pressure Odometry

In the underwater environment, image degradation and insufficient IMU excitation make the initialization of visual–inertial odometry (VIO) a serious challenge. The traditional method improves the performance of initialization by fusing depth information, but does not consider the camera-depth misalignment effect, and the estimation accuracy of the gyroscope bias is affected by inaccurate visual structure-from-motion (SfM). To address these issues, this article proposes a robust initialization method by introducing the robust cost function and depth information to provide a high-quality initial state for underwater monocular visual–inertial–pressure odometry (UM-VIPO). Our method begins with a vision-only structure. The truncated least-squares (TLSs) algorithm is used to suppress outliers in feature matching, thereby significantly improving the accuracy and robustness of visual SfM. In addition, the graduated nonconvexity (GNC) algorithm is used to solve the nonconvex optimization problem induced by the TLS, effectively avoiding convergence to local minima. During visual-depth alignment, depth measurements are introduced to recover the metric scale of the visual poses. By aligning the visual poses and depth measurements with the world coordinate system, the camera-depth misalignment effect is effectively mitigated. In the visual–inertial alignment phase, the gyroscope bias is estimated from the vision-only structure, while the IMU preintegration is leveraged to calculate the initial velocity in the world coordinate system. Then, all initialized parameters are used to execute a local bundle adjustment (BA). Finally, the proposed method is validated on the AQUALOC dataset, demonstrating its effectiveness and superiority.