Abstract:
Pose graph optimization (PGO) is a back-end optimization method for 3D simultaneous localization and mapping, and its accuracy depends on good initial values. We present a chordal with noise (CN) model, which is a new PGO target formula for initializing PGO noise datasets. This proposed model considers the rotation deviation caused by noise, and the deviation matrix is set as a parameter. The optimize rotation with the deviation matrix (ORDM) algorithm is proposed to solve the CN model. The algorithm builds the relative rotation measurement equation of the deviation matrix in the subgraph of the pose graph and finally models the CN into the matrix form. A closed solution of the deviation matrix is obtained by using linear least squares to correct the direction of rotation. Our experimental results show that the ORDM algorithm is robust and scalable in PGO noise data sets. In addition, the ORDM algorithm can correspond to poor initialization scenarios when compared with the iterative initialization algorithm.