Pose graph optimization algorithm based on nonlinear factor recovery

A pose graph optimization method employing nonlinear factor recovery was created in order to accomplish high-precision vehicle pose estimation in urban road situations. This method successfully incorporates vision, inertial information, and global navigation satellite system (GNSS) data inside the factor graph framework. To address the issue of unclear covariance estimation in previous pose graph optimization algorithms, the nonlinear factor recovery algorithm extracts the required information from the dense prior factors generated by marginalization, replacing the prior factors with relative pose factors and conducting optimal estimates for the covariance matrices of these factors. A procedure utilizing visual-inertial odometry for the active detection of GNSS signal anomalies has been designed, capable of adaptively adjusting the covariance matrix of GNSS signals. Throughout the pose graph optimization process, these strategies guaranteed consistency of factor information. Tests conducted on datasets and in real road environments indicate that this approach significantly improves the fusion efficiency of multisource asynchronous signals, providing robust and globally consistent high-precision pose estimation results.