Efficient implementation of incremental smoothing and mapping based on fluid variable reordering for AUVs

Simultaneous localization and mapping (SLAM) has become a hot topic for autonomous underwater vehicles (AUVs) used in underwater applications. The most recent approach iSAM2 has been successfully applied in large-scale environment. In order to choose a good variable ordering for the efficiency of the sparse matrix solution, iSAM2 uses a strategy called incremental variable reordering. However, applying this strategy at every incremental update increases the computational cost. So in this paper, we innovatively improve iSAM2 algorithm by introducing fluid variable reordering. The variables reordering is need to be done only at necessary steps decided through a multi-restraint loop-closure detection method. The proposed method is applied on our own research platform, C-Ranger AUV, through sea trials in Tuandao Bay. The results from simulation and sea trial demonstrate the feasibility of the proposed method and reveal that the modified iSAM2 algorithm has lower computational cost and keeps a highly accurate estimation.