Incremental real-time multibody VSLAM with trajectory optimization using stereo camera
Covariance recovery from a square root information matrix for data association