SaDVIO: Sparsify and Densify VIO for UGV Traversability Estimation

Visual navigation has achieved significant maturity in recent decades. Indirect methods relying on sparse feature extraction, have become a standard for state estimation. However, these techniques are not frequently utilized as inputs for path planning due to the sparse nature of their maps. To address this limitation, this paper introduces a method to densify the sparse map generated by local indirect Visual or Visual Inertial Odometry (VO-VIO) based on factor graph sparsification. The intended application is the autonomous navigation of Unmaned Ground Vehicles (UGV) in planetary environments without the need to perform computationally expensive dense stereo vision. An incremental 3D mesh is built upon a 2D triangulation of the features identified by the VO. A photometric consistency check is conducted on each triangle, resulting in a triangle soup rather than a manifold mesh. A dense point cloud is then extracted by casting rays on the triangle soup and using the most reliable triangle to compute depth. Geometric traversability information can subsequently be derived from this point cloud to aid UGV navigation. This system is evaluated on a dataset recorded on an artificial Martian terrain and demonstrates superior performance compared to a similar method from the state-of-the-art.