Patent Document ID: 9946264
Application ID: 15076745
Patent Status: 1

Claim One:
1. A method for autonomously navigating a vehicle, the method comprising: capturing a sequence of image pairs using a stereo camera; providing a navigation application embedded in a non-transitory memory including a sequence of processor executable instructions for: storing a vehicle pose, where a vehicle pose is defined as a position and orientation with respect to a coordination frame; detecting a plurality of matching feature points in a first matching image pair; determining a plurality of corresponding object points in three-dimensional (3D) space from the first image pair; using a plane model, finding a dominant plane in the 3D space that fits the maximum number of points in the first image pair; defining the dominant plane as an estimated ground plane in a world coordinate system; in response to defining the estimated ground plane, determining vehicle camera height with respect to the estimated ground plane and camera pitch with respect to the estimated ground plane; continuously redefining the estimated ground plane over a plurality of sequential image pairs as the vehicle changes position; wherein determining the camera height and camera pitch includes continuously redetermining the camera height and the camera pitch, such redetermining including avoiding the definition of a changing ground plane slope as an obstacle and finding the pitch angle sign as follows: 
 sign=(( N M )· V ); where N is a vector (0, 1, 0) normal to a plane in the camera coordinate system with y =0; where M is a vector normal to the estimated ground plane in the camera coordinate system; where V is a vector along the x axis (1, 0, 0) of the camera coordinate system; where is a symbol representing a cross product operation; tracking the plurality of feature points from the first image pair to a second image pair; determining a plurality of corresponding object points in 3D space from the second image pair; calculating a vehicle pose transformation using the object points from the first and second image pairs; determining rotation angle and translation from the vehicle pose transformation; and, when the rotation angle or translation exceed a minimum threshold, updating the stored vehicle pose.