Ground vehicle monocular visual-inertial odometry via locally flat constraints

1 . A method of visual-inertial odometry for a ground vehicle comprising:

obtaining, by a controller comprising a processor, an initial set of images with a camera on-board a vehicle;

identifying, by the controller, visual features within the initial set of images;

determining, by the controller, a three-dimensional pose using the visual features in the initial set of images;

obtaining information indicative of vehicle movement with an inertial measurement unit (IMU) of the vehicle;

obtaining information indicative of vehicle movement with wheel speed sensors and a steering wheel angle sensor;

aligning a local reference planes for each of the IMU, the wheel speed sensors and the steering wheel angle sensor with a world reference plane;

determining a locally flat two-dimensional plane between the three-dimensional pose and subsequent sets of images and information indicative of vehicle movement;

fusing in real-time and continuously, by the controller, the identified features within the images, the vehicle movement information from the IMU, and vehicle movement information from the wheel speed sensors and the steering wheel angle sensors within a two-dimensional plane; and

determining, by the controller, a vehicle position relative to an initial start location of the vehicle based on the real-time and continuous fusing of the visual features in the images and the vehicle movement information from the IMU, the wheel speed sensors, and the steering wheel angle sensor;

wherein the fusing comprises determining the two-dimensional plane according to a sliding window based optimization and continually updating the determined locally flat two-dimensional plane based on updated visual features and information from the IMU, wherein the sliding window based optimization is constrained between any two images as a 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.

2 . The method of vehicle-visual-inertial odometry as recited in claim 1 , wherein an alignment of images poses is constrained to the two-dimensional plane.

3 . The method of vehicle-visual-inertial odometry as recited in claim 2 , wherein the fusing comprising fusing, by the controller, vehicle speed information from the wheel speed sensors with the visual features from the camera's images.

4 . The method of vehicle-visual-inertial odometry as recited in claim 3 , wherein the fusing comprising fusing, by the controller, a steering wheel angle from the steering wheel angle sensor with the visual features from the camera's images.

5 . The method of vehicle-visual-inertial odometry as recited in claim 4 , wherein the fusing of the poses coming from the identified features is within a common plane between two or more consecutive images.

6 . The method of vehicle-visual-inertial odometry as recited in claim 5 , wherein vehicle acceleration and orientation data obtained from the IMU is gathered at a rate higher than that of the rate that the camera captures images.

7 . The method of vehicle-visual-inertial odometry as recited in claim 6 , wherein the poses are transformed to match an IMU reference frame.

8 . The method of vehicle-visual-inertial odometry as recited in claim 6 , wherein motion between images is constrained to provide a best fit of a plurality of sampled points from the IMU, the wheel speed sensors, and the steering wheel angle sensor.

9 . 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.

10 . The vehicle-visual-inertial odometry system as recited in claim 9 , wherein the controller is further configured to align the poses coming from the visual features in the two-dimensional plane.

11 . The vehicle-visual-inertial odometry system as recited in claim 10 , further including a wheel speed sensor obtaining information indicative of a vehicle speed and the controller is further configured to fuse the vehicle speed information from the wheel speed sensor with the information coming from the camera's images.

12 . The vehicle-visual-inertial odometry system as recited in claim 11 , further including a steering angle sensor providing an angle of the steering and the controller is configured to fuse the steering angle with the information coming from the camera's images.

13 . The vehicle-visual-inertial odometry system as recited in claim 12 , wherein the controller is configured to constrain the solution of the odometry system by the identification of a common plane for the visual features, the IMU, and the vehicle information between two consecutive images.

14 . The vehicle-visual-inertial odometry system as recited in claim 13 , wherein the controller is configured by a linear constraint to constraint the solution of the odometry system.

15 . The method as recited in claim 1 , wherein the controller determines the two dimensional plane based upon a linear constraint.

16 . The method of vehicle-visual-inertial odometry as recited in claim 1 , wherein vehicle measurements are pre-integrated on two consecutive image frames and transformed to an IMU frame, the vehicle measurements comprises vehicle position, vehicle speed and vehicle yaw angle, and the vehicle position, the vehicle speed information from the wheel speed sensors and the vehicle yaw angle are pre-integrated within the sliding window according to the vehicle model.

17 . The vehicle-visual-inertial odometry system as recited in claim 9 , wherein vehicle measurements are pre-integrated on two consecutive image frames and transformed to an IMU frame, the vehicle measurements comprising vehicle position, vehicle speed and vehicle yaw angle, and the vehicle position, the vehicle speed information from the wheel speed sensors and the vehicle yaw angle are pre-integrated within the sliding window according to the vehicle model.

18 . The method of vehicle-visual-inertial odometry as recited in claim 1 , further comprising operating the vehicle based at least in part upon the determined vehicle position.

19 . The vehicle-visual-inertial odometry system as recited in claim 9 , wherein the controller is further configured to operate the vehicle based at least in part upon the determined vehicle position.