Ground vehicle monocular visual-inertial odometry via locally flat constraints

A vehicle-visual-inertial odometry system for a ground vehicle comprising:

• at least one camera on-board the vehicle obtaining images of object proximate the vehicle;
• an inertial measurement unit (IMU) generating information indicative of vehicle movement;
• a wheel speed sensor generating information indicative of wheel speed;
• and
• a controller, comprising a processor and memory, configured to
• obtain an initial set of images with a camera on-board a vehicle,
• identify visual features within the initial set of images, obtain information indicative of vehicle movement with an inertial measurement unit (IMU),
• obtain information indicative of vehicle movement with the vehicle's wheel speed sensors and a steering wheel angle sensor,
• determine a three-dimensional pose using the identified visual features from the initial set of images;
• align local reference planes for each of the IMU, the wheel speed sensors and the steering wheel angle sensor with a world reference plane;
• determine a locally flat two-dimensional plane between the three-dimensional pose and subsequent sets of images and information indicative of vehicle movement
• with the visual features in a sliding window and for a plurality of sampled points from the IMU, the vehicle's wheel speed sensors, and the steering wheel angle sensor,
• fuse in real-time and continuously the identified features within the images and the vehicle movement from the IMU and vehicle sensors within the two-dimensional plane, and
• determine a vehicle position relative to an initial start location based on the real-time and continuous fusion of the visual features in the images and the vehicle movement information from the IMU and vehicle sensors,
• wherein the two dimensional plane is determined according to a sliding window based optimization that is continually updated based on updated visual features and information from the IMU and vehicle sensors, wherein the sliding window based optimization is constrained between any two images as locally flat movement based on the continually updated locally flat two-dimensional plane, the sliding window based optimization being based on a loss function for a vehicle model, the vehicle model modeling movement of the vehicle within a two-dimensional space corresponding to the two-dimensional plane.