Chordal Based Error Function for 3-D 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 letter, 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 speedup the system. In contrast with some existing approximations, our formulation allows isotropic and anistropic noise covariances to be modeled. To validate our conjectures, we present an extensive comparative analysis between our approach and one of the most used error functions that computes the distance in the unit-quaternion space.