Chordal Based Error Function for 3D Pose-Graph Optimization

Pose-Graph Optimization (PGO) is a well-known problem in the Robotics community. Optimizing a graph means finding the configuration of the nodes that best satisfies the edges. This is generally achieved using iterative approaches that refine a current solution until convergence. Nowadays, Iterative Least-Squares (ILS) algorithms such as Gauss-Newton (GN) or Levenberg-Marquardt (LM) are dominant. Common to all these implementations is the influence of the error function used to measure the difference between prediction and observation. The smoother the error function is, the better the convergence properties of the system become, resulting in an increased convergence basin and more stable behavior. In this work, we propose an alternative error function based on a variant of the Frobenious norm between transformation matrices. The proposed approach leads to a larger convergence basin and to numerical properties in the Jacobian computation that can potentially speed-up the system. In contrast with some existing approximations, our formulation allows to model isotropic and anistropic noise covariances. To validate our conjectures, we present an extensive comparative analysis between our approach and one of the most used error function that computes the distance in the unit-quaternion space.