A Linear Least Square Initialization Method for 3D Pose Graph Optimization Problem

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.