6 DOF Stereo Visual EKF Slam