In this paper, we propose a sparse point-plane odometry used in structured environments. Compared to a point-based odometry, we add additional planar constraints into the process of optimization, making the results more reliable. A novel grid-based plane detection algorithm is proposed to cluster sparse points in the same planes. Then, the planes are parameterized by inverse normal and take part in the windowed optimization. By reducing the size of Hessian Matrix, the process of optimization converges faster. Compared to the original point-based odometry, the proposed method performs better on both robustness and efficiency in structured environments.

We propose a novel stereo visual IMU-assisted (Inertial Measurement Unit) technique that extends to large inter-frame motion the use of KLT tracker (Kanade–Lucas–Tomasi). The constrained and coherent inter-frame motion acquired from the IMU is applied to detected features through homogenous transform using 3D geometry and stereoscopy properties. This predicts efficiently the projection of the optical flow in subsequent images. Accurate adaptive tracking windows limit tracking areas resulting in a minimum of lost features and also prevent tracking of dynamic objects. This new feature tracking approach is adopted as part of a fast and robust visual odometry algorithm based on double dogleg trust region method. Comparisons with gyro-aided KLT and variants approaches show that our technique is able to maintain minimum loss of features and low computational cost even on image sequences presenting important scale change. Visual odometry solution based on this IMU-assisted KLT gives more accurate result than INS/GPS solution for trajectory generation in certain context.