Patent Document ID: 8698875
Application ID: 12708302
Patent Status: 1

Claim One:
1. A method for estimating orientation of a camera mounted on a vehicle, comprising: determining, by at least one processor, an initial pose estimate of a vehicle having a camera mounted thereon, the initial pose estimate based on global positioning system data, inertial measurement unit data, and wheel odometry data of the vehicle, wherein a pose of the vehicle is defined as a position and orientation of the vehicle relative to the world; obtaining images from one or more runs of image data captured by the camera, the images each having an orientation; processing image data from the images to obtain one or more tracks, wherein each track includes a sequence of matched feature points stemming from a single three-dimensional location; and determining from the initial pose estimate and tracks, a correction parameter to correct the orientations of the images captured by the camera, wherein the processing image data comprises: extracting a set of feature points from the images of a given run of the one or more runs; matching one or more of the feature points in successive images of the given run; and grouping matched feature points into tracks, wherein the determining the correction parameter includes using a nonlinear function defined by: 
 F ( P,X,R )=Σ t Σ i ρ(( T Pi,R ( X t )− I Xt )) 2 +λΣ i ( P i −P ESTi ) 2 wherein P=P 1 , P 2 ,. .. , P N and represents a set of vehicle poses; P i represents a pose of the vehicle at time i; X=X 1 , X 2 ,. .. , X M and represents three-dimensional locations of track points in a scene; X t represents a three-dimensional location of a track t in the scene; R represents the rotation of the camera; ρ denotes a robustifier function (e.g., a Cauchy robustifier); T represents projection; I Xt represents a fixed location in a given image, where a feature corresponding to track point X t was detected; λ represents a weight used to trade off strength of a first and a second term in F; and P ESTi represents an initial or a previous pose estimate of the vehicle, wherein the nonlinear function is used to optimize parameters P, X, and R, and wherein T Pi,R (X t )−I Xt represents reprojection error, and P j −P ESTi represents pose error.