Visual-inertial positional awareness for autonomous and non-autonomous mapping

• US 10,402,663 B1
• Filed: 08/29/2016
• Issued: 09/03/2019
• Est. Priority Date: 08/29/2016
• Status: Active Grant

• Current Assignee: Trifo Incorporated
• Original Assignee: Trifo Incorporated
• Inventors: Tsai, Grace, Liu, Shaoshan, Zhang, Zhe
• Primary Examiner(s): Owens, Tsion B
• Application Number:
  US15/250,581
• Time in Patent Office: 1,100 Days
• CPC Class Codes: B60R 2300/80 characterised by the intended use of the viewing arrangement
• CPC Class Codes: G01B 11/24 for measuring contours or curvatures
• CPC Class Codes: G02B 2027/0138 comprising image capture systems, e.g. camera
• CPC Class Codes: G02B 2027/014 comprising information/image processing systems
• CPC Class Codes: G02B 2027/0178 Eyeglass type eyeglass details G02C
• CPC Class Codes: G02B 27/0172 characterised by optical features
• CPC Class Codes: G02B 27/0179 Display position adjusting means not related to the information to be displayed
• CPC Class Codes: G06T 2207/10028 Range image; Depth image; 3D point clouds
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/60 Analysis of geometric attributes
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80
• CPC Class Codes: G06V 20/10 Terrestrial scenes scenes under surveillance with static cameras G06V20/52; scenes perceived from the exterior of a vehicle G06V20/56; scenes perceived from the interior of a vehicle G06V20/59
• CPC Class Codes: G06V 20/56 exterior to a vehicle by using sensors mounted on the vehicle
• CPC Class Codes: H04N 23/57 Mechanical or electrical details of cameras or camera modules specially adapted for being embedded in other devices

Abstract

The described positional awareness techniques employing visual-inertial sensory data gathering and analysis hardware with reference to specific example implementations implement improvements in the use of sensors, techniques and hardware design that can enable specific embodiments to provide positional awareness to machines with improved speed and accuracy.

Claims

1. A system including:

a mobile platform having disposed thereon;

at least one camera;

a multi-axis inertial measuring unit (IMU); and

an interface to a host including one or more processors coupled to memory, the memory loaded with computer instructions to guide the mobile platform that includes the camera with distance calculation and the multi-axis inertial measuring unit (IMU), the instructions, when executed on the processors, implement actions comprising;

receiving sets of image data including feature points and pose information, the pose information including a location of the mobile device and view of the camera that captured the image data, the sets referred to collectively as keyrigs;

reviewing the keyrigs to select keyrig content to include in a point cloud of features, based upon comparisons of keyrig content with content of other selected keyrigs subject to one or more intelligent thresholds;

for selected keyrigs, (a) triangulating new feature points in the keyrig using feature points of keyrigs previously added to the point cloud of features to obtain feature points in a coordinate system of the device, and (b) aligning coordinates of the feature points in the point cloud of features to a coordinate system having a z-axis aligned with gravity;

creating a multilayered hybrid point grid from the feature points selected for the point cloud of features, using at least one layer of a multilayered 2D occupancy grid, including;

initializing a 2D occupancy grid corresponding to one selected from a plurality of x-y layers covering the feature points in the point cloud of features;

populating at least one layer of the occupancy grid with points from the point cloud of features within a height range using ray tracing from an observed location of a point in the keyrig aligned to a corresponding point in the occupancy grid and a location of a corresponding point reprojected on the layer of the occupancy grid; and

finding cells along a ray between the aligned observed point and the corresponding point reprojected on the layer and marking the found cells as empty; and

responsive to receiving a command to travel to a location, using the occupancy grid to plan a path of travel to a location commanded and contemporaneously using the descriptive point cloud while traveling the planned path to avoid colliding with obstructions.

2. The system of claim 1, further implementing actions comprising refining the point cloud of features by:

selecting sets of at least some of image data and pose information of the point cloud of features;

reprojecting 3D points from the point cloud onto a 2D image plane using the pose information;

calculating image coordinates for the reprojected points on the 2D image plane and comparing the calculated image coordinates for the reprojected points on the 2D image plane to actual coordinates of corresponding observed points obtained directly from the image data selected to compute a plurality of re-projection errors; and

minimizing a weighted sum of the re-projection errors for the selected sets.

3. The system of claim 2, wherein the selecting further includes selecting image data and pose information in which IMU readings for static images is [0,0,1g] thereby aligning z-axis of the image data coordinates with gravity.

4. The system of claim 2, wherein the minimizing further includes constraining the pose to moving along a plane when the mobile device is mounted on a vehicle with planar motion.

5. The system of claim 4, wherein the constraining includes applying a pose moving along a plane constraint to a cost function.

6. The system of claim 2, further including refining the multilayered hybrid point grid by:

using ray tracing, determining cells in each layer of the occupancy grid to update based upon the points in the refined the point cloud of features by;

determining an array between a point in the refined point cloud of features and a corresponding point located by a cell of an x-y layer of the occupancy grid such that the array is substantially parallel to a direction of gravity in a coordinate system of the occupancy grid; and

marking the cell as containing an object;

finding cells along the ray between the point in the refined point cloud of features and the corresponding point in the x-y layer of the occupancy grid and marking the cells found as empty; and

finding cells between the point in the refined point cloud of features and a point from which the image data is captured and marking the cells as empty.

7. The system of claim 1, further including providing pose information with respect to the point cloud of features to track position of the mobile device that captured images used to build the point cloud of features.

8. The system of claim 1, wherein the triangulating further includes one selected from (i) triangulating new feature points across images from a current keyrig, (ii) triangulating new feature points across images from two different keyrigs, wherein the two different keyrigs are not necessarily in sequence, (iii) triangulating new feature points from images in keyrigs chosen based upon a criterion.

9. A method of guiding a mobile device that includes a camera with distance calculation and multi-axis inertial measuring unit (IMU) the method including:

receiving sets of image data including feature points and pose information, the pose information including a location of the mobile device and view of the camera that captured the image data, the sets referred to collectively as keyrigs;

reviewing the keyrigs to select keyrig content to include in a point cloud of features, based upon comparisons of keyrig content with content of other selected keyrigs subject to one or more intelligent thresholds;

for selected keyrigs, (a) triangulating new feature points in the keyrig using feature points of keyrigs previously added to the point cloud of features to obtain feature points in a coordinate system of the device, and (b) aligning coordinates of the feature points in the point cloud of features to a coordinate system having a z-axis aligned with gravity;

creating a multilayered hybrid point grid from the feature points selected for the point cloud of features, using at least one layer of a multilayered 2D occupancy grid, including;

initializing a 2D occupancy grid corresponding to one selected from a plurality of x-y layers covering the feature points in the point cloud of features;

populating at least one layer of the occupancy grid with points from the point cloud of features within a height range using ray tracing from an observed location of a point in the keyrig aligned to a corresponding point in the occupancy grid and a location of a corresponding point reprojected on the layer of the occupancy grid; and

finding cells along a ray between the aligned observed point and the corresponding point reprojected on the layer and marking the found cells as empty; and

responsive to receiving a command to travel to a location, using the occupancy grid to plan a path of travel to a location commanded and contemporaneously using the descriptive point cloud while traveling the planned path to avoid colliding with obstructions.

10. The method of claim 9, further including refining the point cloud of features by:

selecting sets of at least some of image data and pose information of the point cloud of features;

reprojecting 3D points from the point cloud onto a 2D image plane using the pose information;

calculating image coordinates for the reprojected points on the 2D image plane and comparing the calculated image coordinates for the reprojected points on the 2D image plane to actual coordinates of corresponding observed points obtained directly from the image data selected to compute a plurality of re-projection errors; and

minimizing a weighted sum of the re-projection errors for the selected sets.

11. The method of claim 10, wherein the selecting further includes selecting image data and pose information in which IMU readings for static images is [0,0,1g] thereby aligning z-axis of the image data coordinates with gravity.

12. The method of claim 10, wherein the minimizing further includes constraining the pose to moving along a plane when the mobile device is mounted on a vehicle with planar motion.

13. The method of claim 12, wherein the constraining includes applying a pose moving along a plane constraint to a cost function.

14. The method of claim 10, further including refining the multilayered hybrid point grid by:

using ray tracing, determining cells in each layer of the occupancy grid to update based upon the points in the refined the point cloud of features by;

determining an array between a point in the refined point cloud of features and a corresponding point located by a cell of an x-y layer of the occupancy grid such that the array is substantially parallel to a direction of gravity in a coordinate system of the occupancy grid; and

marking the cell as containing an object;

finding cells along the ray between the point in the refined point cloud of features and the corresponding point in the x-y layer of the occupancy grid and marking the cells found as empty; and

finding cells between the point in the refined point cloud of features and a point from which the image data is captured and marking the cells as empty.

15. The method of claim 9, further including providing pose information with respect to the point cloud of features to track position of the mobile device that captured images used to build the point cloud of features.

16. The method of claim 9, wherein the triangulating further includes one selected from (i) triangulating new feature points across images from a current keyrig, (ii) triangulating new feature points across images from two different keyrigs, wherein the two different keyrigs are not necessarily in sequence, (iii) triangulating new feature points from images in keyrigs chosen based upon a criterion.

17. A non-transitory computer readable storage medium impressed with computer program instructions to guide a mobile device that includes a camera with distance calculation and multi-axis inertial measuring unit (IMU), the instructions, when executed on a processor, implement a method comprising:

receiving sets of image data including feature points and pose information, the pose information including a location of the mobile device and view of the camera that captured the image data, the sets referred to collectively as keyrigs;

reviewing the keyrigs to select keyrig content to include in a point cloud of features, based upon comparisons of keyrig content with content of other selected keyrigs subject to one or more intelligent thresholds;

for selected keyrigs, (a) triangulating new feature points in the keyrig using feature points of keyrigs previously added to the point cloud of features to obtain feature points in a coordinate system of the device, and (b) aligning coordinates of the feature points in the point cloud of features to a coordinate system having a z-axis aligned with gravity;

creating a multilayered hybrid point grid from the feature points selected for the point cloud of features, using at least one layer of a multilayered 2D occupancy grid, including;

initializing a 2D occupancy grid corresponding to one selected from a plurality of x-y layers covering the feature points in the point cloud of features;

populating at least one layer of the occupancy grid with points from the point cloud of features within a height range using ray tracing from an observed location of a point in the keyrig aligned to a corresponding point in the occupancy grid and a location of a corresponding point reprojected on the layer of the occupancy grid; and

finding cells along a ray between the aligned observed point and the corresponding point reprojected on the layer and marking the found cells as empty; and

responsive to receiving a command to travel to a location, using the occupancy grid to plan a path of travel to a location commanded and contemporaneously using the descriptive point cloud while traveling the planned path to avoid colliding with obstructions.

18. The non-transitory computer readable storage medium of claim 17, implementing the method further comprising refining the point cloud of features by:

selecting sets of at least some of image data and pose information of the point cloud of features;

reprojecting 3D points from the point cloud onto a 2D image plane using the pose information;

calculating image coordinates for the reprojected points on the 2D image plane and comparing the calculated image coordinates for the reprojected points on the 2D image plane to actual coordinates of corresponding observed points obtained directly from the image data selected to compute a plurality of re-projection errors; and

minimizing a weighted sum of the re-projection errors for the selected sets.

19. The non-transitory computer readable storage medium of claim 18, wherein the selecting further includes selecting image data and pose information in which IMU readings for static images is [0,0,1g] thereby aligning z-axis of the image data coordinates with gravity.

20. The non-transitory computer readable storage medium of claim 18, wherein the minimizing further includes constraining the pose to moving along a plane when the mobile device is mounted on a vehicle with planar motion.

21. The non-transitory computer readable storage medium of claim 20, wherein the constraining includes applying a pose moving along a plane constraint to a cost function.

22. The non-transitory computer readable storage medium of claim 18, implementing the method further comprising refining the multilayered hybrid point grid by:

using ray tracing, determining cells in each layer of the occupancy grid to update based upon the points in the refined the point cloud of features by;

determining an array between a point in the refined point cloud of features and a corresponding point located by a cell of an x-y layer of the occupancy grid such that the array is substantially parallel to a direction of gravity in a coordinate system of the occupancy grid; and

marking the cell as containing an object;

finding cells along the ray between the point in the refined point cloud of features and the corresponding point in the x-y layer of the occupancy grid and marking the cells found as empty; and

finding cells between the point in the refined point cloud of features and a point from which the image data is captured and marking the cells as empty.

23. The non-transitory computer readable storage medium of claim 17, implementing the method further comprising providing pose information with respect to the point cloud of features to track position of the mobile device that captured images used to build the point cloud of features.

24. The non-transitory computer readable storage medium of claim 17, wherein the triangulating further includes one selected from (i) triangulating new feature points across images from a current keyrig, (ii) triangulating new feature points across images from two different keyrigs, wherein the two different keyrigs are not necessarily in sequence, and (iii) triangulating new feature points from images in keyrigs chosen based upon a criterion.