# US 2021/0174097 A1 · Visual-Inertial Positional Awareness for Autonomous and Non-Autonomous Mapping

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2021/0174097 A1
- Document ID: US20210174097A1
- Status: Published application
- Kind: A1
- Application number: 17/182,073
- Filed: 2021-02-22
- Published: 2021-06-10
- Earliest claimed: 2016-08-29
- Assignees: Trifo, Inc.
- Applicants: Trifo, Inc.
- Inventors: Grace TSAI; Shaoshan LIU; Zhe ZHANG
- Web page: https://patentgrep.com/patent/US20210174097A1

## CPC

- 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)
- G06V 20/56: exterior to a vehicle by using sensors mounted on the vehicle
- G01B 11/24: for measuring contours or curvatures
- G02B 27/0172: {characterised by optical features}
- G02B 27/0179: {Display position adjusting means not related to the information to be displayed}
- G05D 1/2435: {Extracting 3D information}
- G05D 1/245: using dead reckoning
- G05D 1/2464: {using an occupancy grid}
- G06T 7/60: Analysis of geometric attributes
- G06T 7/70: Determining position or orientation of objects or cameras (camera calibration G06T7/80)
- H04N 23/57: Mechanical or electrical details of cameras or camera modules specially adapted for being embedded in other devices
- B60R 2300/80: characterised by the intended use of the viewing arrangement
- G02B 2027/0138: {comprising image capture systems, e.g. camera}
- G02B 2027/014: {comprising information/image processing systems}
- G02B 2027/0178: {Eyeglass type (eyeglass details G02C)}
- G05D 2105/30: for social or care-giving applications
- G05D 2109/10: Land vehicles
- G05D 2111/52: {generated by inertial navigation means, e.g. gyroscopes or accelerometers}
- G05D 2111/64: {taken simultaneously from spaced apart sensors, e.g. stereovision}
- G05D 2111/67: Sensor fusion
- G06T 2207/10028: Range image; Depth image; 3D point clouds
- G06T 2207/30244: Camera pose
- G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G01B: MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G02B: OPTICAL ELEMENTS, SYSTEMS OR APPARATUS
- G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- H04N: PICTORIAL COMMUNICATION, e.g. TELEVISION
- B60R: VEHICLES, VEHICLE FITTINGS, OR VEHICLE PARTS, NOT OTHERWISE PROVIDED FOR

## 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

### Claim 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 platform 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 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 platform, 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; and
responsive to receiving a command to travel to a location, using the at least one layer of a multilayered 2D occupancy grid to plan a path of travel to a location commanded and contemporaneously using the point cloud of features while traveling the planned path to avoid colliding with obstructions.

### Claim 2 (depends on 1)

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.

### Claim 3 (depends on 2)

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.

### Claim 4 (depends on 2)

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

### Claim 5 (depends on 4)

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

### Claim 6 (depends on 2)

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.

### Claim 7 (depends on 1)

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

### Claim 8 (depends on 1)

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.

### Claim 9 (depends on 1)

The system of claim 1, wherein creating a multilayered hybrid point grid further includes:
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.

### Claim 10

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 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; and
responsive to receiving a command to travel to a location, using the at least one layer of a multilayered 2D occupancy grid occupancy grid to plan a path of travel to a location commanded and contemporaneously using the point cloud of features while traveling the planned path to avoid colliding with obstructions.

### Claim 11 (depends on 10)

The method of claim 10, 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.

### Claim 12 (depends on 11)

The method of claim 11, 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.

### Claim 13 (depends on 11)

The method of claim 11, 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.

### Claim 14 (depends on 13)

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

### Claim 15 (depends on 11)

The method of claim 11, 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.

### Claim 16 (depends on 10)

The method of claim 10, 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.

### Claim 17 (depends on 10)

The method of claim 10, 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.

### Claim 18 (depends on 10)

The method of claim 10, wherein creating a multilayered hybrid point grid further includes:
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.

### Claim 19

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 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; and
responsive to receiving a command to travel to a location, using the at least one layer of a multilayered 2D occupancy grid to plan a path of travel to a location commanded and contemporaneously using the point cloud of features while traveling the planned path to avoid colliding with obstructions.

### Claim 20 (depends on 18)

The non-transitory computer readable storage medium of claim 18, wherein creating a multilayered hybrid point grid further includes:
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.
