A method for estimating egomotion of a camera mounted on a vehicle that uses infra-red images is disclosed, comprising the steps of (a) receiving a pair of frames from a plurality of frames from the camera, the first frame being assigned to a previous frame and an anchor frame and the second frame being assigned to a current frame; (b) extracting features from the previous frame and the current frame; (c) finding correspondances between extracted features from the previous frame and the current frame; and (d) estimating the relative pose of the camera by minimizing reprojection errors from the correspondences to the anchor frame. The method can further comprise the steps of (e) assigning the current frame as the anchor frame when a predetermined amount of image motion between the current frame and the anchor frame is observed; (f) assigning the current frame to the previous frame and assigning a new frame from the plurality of frames to the current frame; and (g) repeating steps (b)-(f) until there are no more frames from the plurality of frames to process. Step (c) is based on an estimation of the focus of expansion between the previous frame and the current frame.
展开▼
