Curve-graph Odometry: Removing the orientation in loop closure optimisation problems

Abstract

In robot odometry and SLAM applications the real trajectory is estimated incrementally. This produces an accumulation of errors which gives raise to a drift in the trajectory. When revisiting a previous position this drift becomes observable and thus it can be corrected 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 main advantages of our method are the elimination of the need of mixing the orientation and position in the optimisation and the savings in computational cost due to the reduction of the dimension of the optimisation vector.