SYSTEMS AND METHODS FOR DETERMINING A POSE AND MOTION OF A CARRIER VEHICLE

1 . A method of determining a pose and motion of a carrier vehicle, comprising:

capturing a video of an external environment surrounding a carrier vehicle using a camera of an edge device coupled to the carrier vehicle, wherein the video comprises a plurality of video frames comprising a keyframe and a subsequent video frame captured after the keyframe;

determining, using one or more processors of the edge device, a full vehicle pose of the carrier vehicle with respect to the keyframe based on monocular visual odometry measurements made using the keyframe and the subsequent video frame;

determining, using the one or more processors, a vehicle position and motion of the carrier vehicle based on positioning data obtained from a positioning unit of the edge device; and

providing the full vehicle pose with respect to the keyframe obtained from the monocular visual odometry measurements and the vehicle position and motion determined from the positioning data to a filter running on the edge device to obtain a fused vehicle pose and motion of the carrier vehicle, wherein the motion of the carrier vehicle comprises a velocity and acceleration of the carrier vehicle.

2 . The method of claim 1 , wherein the monocular visual odometry measurements are made by:

matching image points between the keyframe and the subsequent video frame to obtain a plurality of tracked points;

removing outliers from the tracked points to obtain an up-to-scale camera pose;

determining a camera translation magnitude based on the up-to-scale camera pose;

combining the up-to-scale camera pose and the camera translation magnitude to obtain a full camera pose; and

converting the full camera pose to the full vehicle pose based on a known relationship between the full camera pose and the full vehicle pose.

3 . The method of claim 2 , further comprising triangulating at least some of the tracked points using a triangulation algorithm and taking into account the full camera pose.

4 . The method of claim 3 , further comprising:

projecting a triangulated instance of an outlier image point back into the subsequent video frame using its calculated depth and the full camera pose; and

adding the outlier image point back into an inlier point set if a Sampson error of the outlier image point is within a preset threshold.

5 . The method of claim 2 , wherein matching the image points between the keyframe and the subsequent video frame further comprises:

masking the keyframe using image masks based on the tracked points;

detecting new image points in image areas of the keyframe outside of the image masks using a feature detector algorithm;

matching the new image points between the keyframe and the subsequent video frame to obtain a second set of tracked points; and

repeating the detecting, matching, and masking steps until a total number of tracked points exceeds a threshold amount.

6 . The method of claim 5 , wherein masking the keyframe using the image masks further comprises centering a filled circle representing the image mask around a tracked point, wherein a radius of the filled circle is a minimum point separation parameter.

7 . The method of claim 5 , wherein the feature detector algorithm is the FAST feature detector algorithm.

8 . The method of claim 2 , wherein the image points between the keyframe and the subsequent video frame is matched using a pyramidal optical flow algorithm run in a forward direction.

9 . The method of claim 1 , wherein the fused vehicle pose and motion further comprises a latitude of the carrier vehicle, a longitude of the carrier vehicle, an altitude of the carrier vehicle, a velocity of the carrier vehicle, an acceleration of the carrier vehicle, a vehicle rotation with respect to the keyframe, a vehicle translation with respect to the keyframe, and a vehicle heading with respect to keyframe.

10 . The method of claim 1 , further comprising selecting a new instance of the keyframe in response to an amount of tracked points falling below a tracked points threshold or a camera rotation and translation exceeding a mean pixel threshold.

11 . A device for determining a pose and motion of a carrier vehicle, comprising:

a camera configured to capture a video of an external environment surrounding the carrier vehicle, wherein the device is coupled to the carrier vehicle, and wherein the video comprises a plurality of video frames comprising a keyframe and a subsequent video frame captured after the keyframe; and

one or more processors programmed to:

determine a full vehicle pose of the carrier vehicle with respect to the keyframe based on monocular visual odometry measurements made using the keyframe and the subsequent video frame;

determine a vehicle position and motion of the carrier vehicle based on positioning data obtained from a positioning unit of the device; and

provide the full vehicle pose with respect to the keyframe obtained from the monocular visual odometry measurements and the vehicle position and motion determined from the positioning data to a filter running on the device to obtain a fused vehicle pose and motion of the carrier vehicle, wherein the motion of the carrier vehicle comprises a velocity and acceleration of the carrier vehicle.

12 . The device of claim 11 , wherein the one or more processors are further programmed to undertake the following steps to make monocular visual odometry measurements:

match image points between the keyframe and the subsequent video frame to obtain a plurality of tracked points;

remove outliers from the tracked points to obtain an up-to-scale camera pose;

determine a camera translation magnitude based on the up-to-scale camera pose;

combine the up-to-scale camera pose and the camera translation magnitude to obtain a full camera pose; and

convert the full camera pose to the full vehicle pose based on a known relationship between the full camera pose and the full vehicle pose.

13 . The device of claim 12 , wherein the one or more processors are further programmed to triangulate at least some of the tracked points using a triangulation algorithm and taking into account the full camera pose.

14 . The device of claim 13 , wherein the one or more processors are further programmed to:

project a triangulated instance of an outlier image point back into the subsequent video frame using its calculated depth and the full camera pose; and

add the outlier image point back into an inlier point set if a Sampson error of the outlier image point is within a preset threshold.

15 . The device of claim 12 , wherein the one or more processors are further programmed to undertake the following steps to match the image points between the keyframe and the subsequent video frame:

mask the keyframe using image masks based on the tracked points;

detect new image points in image areas of the keyframe outside of the image masks;

match the new image points between the keyframe and the subsequent video frame to obtain a second set of tracked points; and

repeat the detecting, matching, and masking steps until a total number of tracked points exceeds a threshold amount.

16 . A method of determining a full vehicle pose of a carrier vehicle, comprising:

capturing a video of an external environment surrounding a carrier vehicle using a camera of an edge device coupled to the carrier vehicle, wherein the video comprises a plurality of video frames comprising a keyframe and a subsequent video frame captured after the keyframe;

matching, using one or more processors of the edge device, image points between the keyframe and the subsequent video frame to obtain a plurality of tracked points;

removing outliers from the tracked points to obtain an up-to-scale camera pose;

determining a camera translation magnitude based on the up-to-scale camera pose;

combining the up-to-scale camera pose and the camera translation magnitude to obtain a full camera pose of the carrier vehicle; and

converting the full camera pose to the full vehicle pose based on a known relationship between the full camera pose and the full vehicle pose, wherein the full vehicle pose is determined with respect to the keyframe.

17 . The method of claim 16 , wherein matching the image points between the keyframe and the subsequent video frame further comprises:

masking the keyframe using image masks based on the tracked points;

detecting new image points in image areas of the keyframe outside of the image masks using a feature detector algorithm;

matching the new image points between the keyframe and the subsequent video frame to obtain a second set of tracked points; and

repeating the detecting, matching, and masking steps until a total number of tracked points exceeds a threshold amount.

18 . The method of claim 17 , wherein the feature detector algorithm is the FAST feature detector algorithm.

19 . The method of claim 16 , wherein the image points between the keyframe and the subsequent video frame is matched using a pyramidal optical flow algorithm run in a forward direction.

20 . The method of claim 16 , further comprising selecting a new instance of the keyframe in response to an amount of tracked points falling below a tracked points threshold or a camera rotation and translation exceeding a mean pixel threshold.