Efficient trajectory bending with applications to loop closure

In robotic applications the absolute pose is often obtained as the integral of successive relative rigid-body motions. As each relative rigid-body motion is typically the product of statistical inference, the integrated absolute pose will exhibit error build-up and the estimated trajectory will differ from the true trajectory undertaken by the system. Some application areas allow the system to receive additional information about its current absolute pose, for example from loop detection, which is more accurate than the integral of the relative rigid-body motions. The availability of this absolute information is usually less frequent than the information underlying the relative rigid-body motions. This contribution addresses an efficient closed form algorithm which minimally bends a trajectory such that the integrated pose is exactly equal to any particular desired pose. The manner in which the bending is distributed over the trajectory is controllable using weights. The proposed method will be compared against a maximum likelihood solution on simulated trajectories as well as on trajectories estimated from binocular and monocular data. The results indicate that the performance differences between the closed form approach and the maximum likelihood solution are negligible while the closed form approach is significantly more efficient.