Curve-graph odometry: Orientation-free error parameterisations for loop closure problems

Abstract

During incremental odometry estimation in robotics and vision applications, the accumulation of estimation error produces a drift in the trajectory. This drift becomes observable when returning to previously visited areas, where it is possible to correct it by applying loop closing techniques. Ultimately a loop closing process leads to an optimisation problem where new constraints between poses obtained from loop detection are applied to the initial incremental estimate of the trajectory. Typically this optimisation is jointly applied on the position and orientation of each pose of the robot using the state-of-the-art pose graph optimisation scheme on the manifold of the rigid body motions. In this paper we propose to address the loop closure problem using only the positions and thus removing the orientations from the optimisation vector. The novelty in our approach is that, instead of treating trajectory as a set of poses, we look at it as a curve in its pure mathematical meaning. We define an observation function which computes the estimate of one constraint in a local reference frame using only the robot positions. Our proposed method is compared against state-of-the-art pose graph optimisation algorithms in 2 and 3 dimensions. The benefit of eliminating orientations is twofold. First, the objective function in the optimisation does not mix translation and rotation terms, which may have different scales. Second, computational performance can be improved due to the reduction in the state dimension of the nodes of the graph.