Pose graph optimization (PGO), or equivalently pose synchronization, that is synchronizing rotations and positions, is the state-of-the-art formulation for simultaneous localization and mapping in robotics. In this article, we first present a new manifold Gauss-Newton method for solving the rotation synchronization problem. In this article, we derive an efficient implementation of the method and develop a convergence theory thereof. A structural parameter appears in the proof, which has a significant influence on the convergence basin. In this article, we show that this structural parameter is the norm of the inverse of the reduced graph Laplacian and obtains the explicit relation of this parameter for special graph structures. We also present another method that directly solves the pose synchronization using both relative rotation and translation observations. Experimental results show that our rotation synchronization method can be successfully used to initialize iterative PGO solvers. Furthermore, we show that our pose synchronization algorithm outperforms state-of-the-art solvers in high-noise cases.

Pose Graph Optimization (PGO) is an important optimization problem arising in robotics and machine vision applications like 3D reconstruction and 3D SLAM. Each node of pose graph corresponds to an orientation and a location. The PGO problem finds orientations and locations of the nodes from relative noisy observation between nodes. Recent investigations show that well-known iterative PGO solvers need good initialization to converge to good solutions. However, we observed that state-of-the-art initialization methods obtain good initialization only in low noise problems, and they fail in challenging problems having more measurement noise. Consequently, iterative methods may converge to bad solutions in high noise problems. In this paper, a new method for obtaining orientations in the PGO optimization problem is presented. Like other well-known methods the initial locations are obtained from the result of a least-squares problem. The proposed method iteratively approximates the problem around current estimation and converts it to a least-squares problem. Therefore, the method can be seen as an iterative least-squares method which is computationally efficient. Simulation results show that the proposed initialization method helps the most well-known iterative solver to obtain better optima and significantly outperform other solvers in some cases.