Keyframe based visual inertial odometry using nonlinear optimization