# US 12,181,888 B2 · Monocular modes for autonomous platform guidance systems with auxiliary sensors > Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt - Number: US 12,181,888 B2 - Document ID: US12181888B2 - Status: Granted - Kind: B2 - Application number: 18/241,113 - Filed: 2023-08-31 - Published: 2024-03-07 - Granted: 2024-12-31 - Earliest claimed: 2017-06-14 - Assignees: Trifo, Inc. - Applicants: Trifo, Inc. - Inventors: Zhe Zhang; Grace Tsai; Shaoshan Liu - Web page: https://patentgrep.com/patent/US12181888B2 ## CPC - G05D 1/0246: {using a video camera in combination with image processing means} - G05D 1/00: Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots (drive control systems specially adapted for autonomous road vehicles B60W60/00) - G05D 1/027: {comprising intertial navigation means, e.g. azimuth detector (inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165)} - G05D 1/0274: {using mapping information stored in a memory device (navigation using map-matching G01C21/30)} - G05D 1/227: Handing over between remote control and on-board control; Handing over between remote control arrangements - G05D 1/24: Arrangements for determining position or orientation - G05D 1/245: using dead reckoning - G05D 1/246: using environment maps, e.g. simultaneous localisation and mapping [SLAM] - G05D 1/249: from positioning sensors located off-board the vehicle, e.g. from cameras - G06F 18/251: {of input or preprocessed data} - G06T 1/20: Processor architectures; Processor configuration, e.g. pipelining - G06T 1/60: Memory management - G06T 7/207: for motion estimation over a hierarchy of resolutions (multi-resolution motion estimation or hierarchical motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/53) - G06T 7/246: using feature-based methods, e.g. the tracking of corners or segments - G06T 7/248: {involving reference images or patches} - G06T 7/50: Depth or shape recovery - G06T 7/73: using feature-based methods - G06T 7/74: {involving reference images or patches} - G06V 10/40: Extraction of image or video features - G06V 10/60: relating to illumination properties, e.g. using a reflectance or lighting model - G06V 10/803: {of input or preprocessed data} - G06V 10/955: {using specific electronic processors} - G06V 10/993: {Evaluation of the quality of the acquired pattern} - 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/40: in video content (extracting overlay text G06V20/62; video retrieval G06F16/70; processing of video elementary streams in video servers H04N21/234; processing of video elementary streams in video clients H04N21/44) - G06V 20/56: exterior to a vehicle by using sensors mounted on the vehicle - G06V 20/647: {by matching two-dimensional images to three-dimensional objects} - G06T 2207/10016: Video; Image sequence - G06T 2207/10028: Range image; Depth image; 3D point clouds - G06T 2207/30244: Camera pose - G05D: SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES - G06F: ELECTRIC DIGITAL DATA PROCESSING (computer systems based on specific computational models G06N) - G06T: IMAGE DATA PROCESSING OR GENERATION, IN GENERAL - G06V: IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING ## Abstract The described positional awareness techniques employing 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. The sensory data are gathered from an operational camera and one or more auxiliary sensors. ## Claims ### Claim 1 A method of guiding a mobile unit including a visual sensor having at least one operational camera and at least one auxiliary sensor, including one or more selected from a multi-axis IMU, a wheel odometry sensor and a depth sensor, the method including: obtaining a location of the mobile unit and perspective, including view direction, of a camera, referred to collectively as an initial pose; obtaining information indicating travel Pl including a rotational portion and a translational portion of movement of the mobile unit; obtaining a propagated pose from the initial pose using information indicating travel Pl; obtaining a corrected pose, the corrected pose being obtained from a captured image frame by: extracting features from the captured image frame; matching the features extracted to retrieved feature points, producing a list of matching features; determining a scale factor using information from the translational portion of information indicating travel Pl and a translational portion of positions of matching features in the list of matching features; and calculating a corrected pose Pm using the scale factor applied to at least a magnitude of translational portion of positions of matching features to determine a corrected pose; and guiding the mobile unit using one or both of the propagated pose and the corrected pose. ### Claim 2 (depends on 1) The method of claim 1, wherein matching features extracted to retrieved feature points is based on (1) reuse of matched features from a previous frame and (2) matching of features in the captured image frame with reprojected feature positions from a 3D map onto a 2D view from a perspective of the propagated pose. ### Claim 3 (depends on 2) The method of claim 2, the visual sensor having at least one operational camera and a depth sensor, the method further including: receiving depth information from a depth sensor disposed at a position to sense depth information for features within a field of view of a camera; using the depth information, retrieving corresponding depth values for remaining two dimensional features; obtaining weighted average depth values for each remaining two-dimensional feature; populating the 3D map with remaining two-dimensional features using the weighted average depth values; and providing the 3D map as populated for guiding a mobile unit. ### Claim 4 (depends on 3) The method of claim 3, further including: tracking the mobile unit against the 3D map. ### Claim 5 (depends on 3) The method of claim 3, further including: adding additional poses by performing imu propagation to obtain a baseline between a latest to-be-added pose and a properly chosen previous pose; and using depth information from the depth sensor to correct depth information for triangulated map points. ### Claim 6 (depends on 5) The method of claim 5, further including: for 2D feature points which are not triangulated, using the depth information to determine additional 3D map points. ### Claim 7 (depends on 3) The method of claim 3, further including: removing from images two-dimensional features having no corresponding readings from the depth sensor. ### Claim 8 (depends on 3) The method of claim 3, further including: populating a table of depth values to pixel coordinates to calibrate the depth sensor to the camera. ### Claim 9 (depends on 2) The method of claim 2, further including guiding the mobile unit as the mobile unit explores while updating the 3D map with features. ### Claim 10 (depends on 1) The method of claim 1, wherein a translational portion of positions of matching features describes a difference in perspective from which the captured image frame was viewed by the camera relative to a previous frame viewed by the camera. ### Claim 11 (depends on 1) The method of claim 1, further including detecting that at least one camera is experiencing impaired visibility, and whenever at least one camera experiences impaired visibility, using information from an auxiliary sensor to determine the corrected pose whenever image information is unusable. ### Claim 12 (depends on 11) The method of claim 11, the method further including detecting that at least one camera is experiencing impaired visibility, and whenever at least one camera experiences impaired visibility, using information from the auxiliary sensor and previous images to determine the corrected pose. ### Claim 13 (depends on 12) The method of claim 12, wherein impaired visibility includes one of: diminished light in a spectral area used by the camera, camera being obstructed. ### Claim 14 (depends on 11) The method of claim 11, wherein the auxiliary sensor includes at least one of an inertial measurement unit (IMU) and a wheel odometry sensor, the method further including performing at least one of: determining information indicating travel to Pl including a rotational portion and a translational portion of movement of the mobile unit from IMU information by integrating at least one of linear acceleration and angular velocity; and determining information indicating travel Pl including a rotational portion and a translational portion of movement of the mobile unit from odometry information of a rotary encoder coupled to a wheel, from a ratio of a circumference of the wheel to resolution of the rotary encoder. ### Claim 15 (depends on 1) The method of claim 1, further including: examining a new frame using a criteria that includes: whether a count of 2D feature points of usable quality meets a threshold; whether a count of triangulated map points between a pairing of a propagated pose with a new image and a pairing of a current pose with an existing image meets a threshold; and including the new frame that meets the threshold. ### Claim 16 (depends on 1) The method of claim 1, wherein the mobile unit including at least two auxiliary sensors, including a first auxiliary sensor comprising a multi-axis IMU and a second auxiliary sensor selected from (a wheel odometry sensor and a depth sensor), the method further including: while waiting for a new frame, between successive camera frames, updating the initial pose using information from an IMU, to generate a first propagated pose; extracting new features from the new frame; determining a second propagated pose, updating the initial pose using information from a wheel odometer; determining an error between the first propagated pose and the second propagated pose; minimizing error of a translational portion of first and the second propagated poses and a rotational portion of first and the second propagated poses to find the corrected pose; and using the corrected pose for guiding a robot. ### Claim 17 (depends on 16) The method of claim 16, wherein the minimizing includes: receiving an initial translational portion and rotational portion of first and second propagated poses having a first error; determining a new translational portion and rotational portion for first and second propagated poses having a second error; selecting one from the new translational portion and rotational portion and the initial translational portion and rotational portion based upon comparing the second error with the first error as a new initial translational portion and rotational portion of first and second propagated poses; and iteratively repeating the determining and selecting until a translational portion and rotational portion for first and second propagated poses achieves a minimum error criterion. ### Claim 18 (depends on 1) The method of claim 1, wherein obtaining a location of the mobile unit and perspective, including view direction, of a camera, further includes: receiving via an interface information from at least one selected from: (i) an inertial measurement unit (IMU) encoding information from at least one of a gyroscopic sensor, and an accelerometer; (ii) a sensor encoding odometry information, and (iii) a sensor encoding depth information for at least some pixels sensed by a camera. ### Claim 19 A system including a processor and a computer readable storage unit storing instructions for guiding a mobile unit including a visual sensor having at least one operational camera and at least one auxiliary sensor, including one or more selected from a multi-axis IMU, a wheel odometry sensor and a depth sensor, which instructions, when executed by the processor perform: obtaining a location of the mobile unit and perspective, including view direction, of a camera, referred to collectively as an initial pose; obtaining information indicating travel Pl including a rotational portion and a translational portion of movement of the mobile unit; obtaining a propagated pose from the initial pose using information indicating travel Pl; obtaining a corrected pose, the corrected pose being obtained from a captured image frame by: extracting features from the captured image frame; matching the features extracted to retrieved feature points, producing a list of matching features; determining a scale factor using information from the translational portion of information indicating travel Pl and a translational portion of positions of matching features in the list of matching features; and calculating a corrected pose Pm using the scale factor applied to at least a magnitude of translational portion of positions of matching features to determine a corrected pose; and guiding the mobile unit using one or both of the propagated pose and the corrected pose. ### Claim 20 A non-transitory computer readable medium storing instructions for guiding a mobile unit including a visual sensor having at least one operational camera and at least one auxiliary sensor, including one or more selected from a multi-axis IMU, a wheel odometry sensor and a depth sensor, which instructions, when executed by a processor perform: obtaining a location of the mobile unit and perspective, including view direction, of a camera, referred to collectively as an initial pose; obtaining information indicating travel P/including a rotational portion and a translational portion of movement of the mobile unit; obtaining a propagated pose from the initial pose using information indicating travel Pl; obtaining a corrected pose, the corrected pose being obtained from a captured image frame by: extracting features from the captured image frame; matching the features extracted to retrieved feature points, producing a list of matching features; determining a scale factor using information from translational portion of information indicating travel Pl and a translational portion of positions of matching features in the list of matching features; and calculating a corrected pose Pm using the scale factor applied to at least a magnitude of the translational portion of positions of matching features to determine a corrected pose; and guiding the mobile unit using one or both of the propagated pose and the corrected pose.