Computing MAP trajectories by representing, propagating and combining PDFs over groups

This paper addresses the problem of computing the trajectory of a camera from sparse positional measurements that have been obtained from visual localisation, and dense differential measurements from odometry or inertial sensors. A fast method is presented for fusing these two sources of information to obtain the maximum a posteriori estimate of the trajectory. A formalism is introduced for representing probability density functions over Euclidean transformations, and it is shown how these density functions can be propagated along the data sequence and how multiple estimates of a transformation can be combined. A three-pass algorithm is described which makes use of these results to yield the trajectory of the camera. Simulation results are presented which are validated against a physical analogue of the vision problem, and results are then shown from sequences of approximately 1,800 frames captured from a video camera mounted on a go-kart. Several of these frames are processed using computer vision to obtain estimates of the position of the go-kart. The algorithm fuses these estimates with odometry from the entire sequence in I50 mS to obtain the trajectory of the kart.