Robust stereo visual odometry using weighted and sparse optimization

A stereo based visual odometry was proposed to accurately estimate the 6DOF robot motion in real time. Uncertainties of feature point reconstruction are defined by covariance matrix to reduce the error propagation during egomotion estimation by weighted spare optimization. Ego-motion estimation was a nonlinear equality constrained optimization problem. It was transformed into a nonlinear unconstrained optimization by Lagrange theorem. The initial estimation was achieved by Weighted Least Square methods to reduce the effect of reconstruction uncertainty of the feature points. Sparse Levenberg-Marquardt algorithm was utilized to obtain more accuracy. The robustness and efficiency were validated by experimental results.