Visual-inertial odometer method and device based on sparse TOF depth fusion

Abstract

The application discloses a kind of sparse TOF depth fusion visual inertial odometry method and device, it is related to multi-sensor fusion navigation positioning technical field.The method comprises: using sparse TOF sensor to collect the target point cloud data of autonomous mobile device, using RGB camera to collect the RGB color image of autonomous mobile device, using inertial measurement unit to collect the multidimensional motion data of autonomous mobile device;Target point cloud data is projected to RGB color image, and semi-dense depth map and semi-dense depth mask are obtained;From the RGB color image, a plurality of feature points are extracted, and the constraint type of each feature point is determined according to semi-dense depth mask;According to a plurality of feature points, a plurality of constraint types, multidimensional motion data, RGB color image, construct target optimization function, solve the state vector corresponding to autonomous mobile device.Solve the problem that existing fusion depth visual inertial odometry depth constraint strategy fails, effectively and accurately utilize depth constraint strategy, so as to improve the positioning accuracy of VIO.

Description

Technical Field

[0001] This application relates to the field of multi-sensor fusion navigation and positioning technology, and more specifically, to a visual inertial odometry method and device based on TOF deep fusion. Background Technology

[0002] With the continuous development of navigation and positioning technology, pure visual SLAM technology has been widely used. However, pure visual SLAM methods (such as ORB-SLAM and PTAM) rely on feature point triangulation to recover depth, which has problems such as scale ambiguity, stringent triangulation conditions, difficulty in initialization, and degradation in weak textured scenes. Monocular vision cannot determine the true physical scale, resulting in scale drift in pose estimation; triangulation requires the camera to have a sufficient baseline, which will fail in slow motion or pure rotational scenes; initialization requires a specific motion trajectory, which cannot be started when stationary or in uniform motion; and in weak textured environments such as corridors and white walls, feature points are sparse, making tracking prone to failure.

[0003] To overcome the limitations of purely visual methods, researchers have proposed a core technology for autonomous localization and navigation in mobile robots, drones, and AR / VR devices: Visual-Inertial Odometry (VIO). In the field of autonomous localization and navigation, VIO achieves real-time estimation of device pose by fusing visual information from cameras and motion information from inertial measurement units (IMUs). For example, VINS-Mono achieves robust monocular VIO by fusing visual reprojection errors and IMU pre-integration errors through a tightly coupled optimization framework, but it still suffers from weak scale observability. VINS-Fusion (2019) extends support to support binocular and binocular + IMU configurations, providing scale constraints through stereo vision, but it requires accurate binocular calibration and is costly. ORB-SLAM3 (University of Zaragoza, 2020) supports monocular, binocular, RGB-D, and multi-map fusion, but its RGB-D mode assumes dense coverage of depth maps. Therefore, introducing a depth sensor can directly provide depth information and solve the scale ambiguity problem of monocular VIO, namely the existing RGB-D fusion method, such as the VINS-RGBD scheme (a navigation system that combines vision and IMU).

[0004] However, VINS-RGBD mainly relies on dense depth information to optimize the positioning accuracy of visual odometry. However, under sparse depth constraints, it cannot effectively utilize the sparse depth constraints, causing the constraint strategy to fail and thus reducing the positioning accuracy of VIO. Existing RGB-D fusion methods (such as VINS-RGBD) assume that the depth sensor provides a dense depth map that densely covers the RGB image (coverage greater than 90%). VINS-RGBD reads the depth value at each feature point pixel position (applying depth constraints). It assumes that the depth map has effective measurements at the feature point positions. When a low-cost TOF depth sensor (such as Lingming Photonics ADS6401) only provides about 900 depth measurement points, the coverage on the 640x480 RGB image plane is only 0.3%, that is, only 3 out of 200-300 feature points have depth information. 99.7% of RGB pixels have no direct depth information. The constraints are severely insufficient, making the distribution of feature points with depth constraints extremely uneven, the optimization unstable, and the utilization rate of depth information extremely low (less than 1%), which fails to make the sensor function.

[0005] There is currently no effective solution to the problem of reduced positioning accuracy of VIO caused by the failure of existing visual inertial odometry depth constraint strategies for fusion depth. Summary of the Invention

[0006] The main objective of this application is to provide a visual inertial odometry method and device based on TOF depth fusion to solve the problem of failure of existing visual inertial odometry depth constraint strategies with fusion depth, and to effectively and accurately utilize the depth constraint strategy to improve the positioning accuracy of VIO.

[0007] To achieve this objective, in a first aspect, this application proposes a visual inertial odometry method based on sparse TOF deep fusion, characterized by its application to autonomous mobile devices. The method includes: acquiring target point cloud data of the autonomous mobile device at a target time stamp using a sparse TOF sensor; acquiring RGB color images of the autonomous mobile device at the target time stamp using an RGB camera; and acquiring multidimensional motion data of the autonomous mobile device at the target time stamp using an inertial measurement unit (IMU). The target point cloud data is projected onto the RGB color images to obtain a semi-dense depth map and a corresponding semi-dense depth mask. Multiple feature points are extracted from the RGB color images, and the constraint type of each feature point is determined based on the semi-dense depth mask. Based on the multiple feature points, multiple constraint types, multidimensional motion data, and the RGB color images, a target optimization function is constructed to solve for the state vector corresponding to the autonomous mobile device, wherein the state vector includes the pose of the autonomous mobile device at the target time stamp.

[0008] Secondly, a visual inertial odometry device based on sparse TOF deep fusion is also provided. The device is characterized by being applied to an autonomous mobile device and includes: a data acquisition unit for acquiring target point cloud data of the autonomous mobile device at a target time stamp using a sparse TOF sensor, acquiring RGB color images of the autonomous mobile device at the target time stamp using an RGB camera, and acquiring multidimensional motion data of the autonomous mobile device at the target time stamp using an inertial measurement unit; a projection unit for projecting the target point cloud data onto the RGB color image to obtain a semi-dense depth map and a semi-dense depth mask corresponding to the semi-dense depth map; an extraction unit for extracting multiple feature points from the RGB color image and determining the constraint type of each feature point based on the semi-dense depth mask; and a construction unit for constructing a target optimization function based on multiple feature points, multiple constraint types, multidimensional motion data, and the RGB color image, and solving for the state vector corresponding to the autonomous mobile device, wherein the state vector includes the pose of the autonomous mobile device at the target time stamp.

[0009] Thirdly, a computer-readable storage medium is provided, which stores computer instructions for causing a computer to execute the above-described visual inertial odometry method based on sparse TOF deep fusion.

[0010] Fourthly, an electronic device is also provided, comprising: at least one processor and a memory communicatively connected to the at least one processor; wherein the memory stores a computer program executable by the at least one processor, the computer program being executed by the at least one processor to cause the at least one processor to perform the visual inertial odometry method based on sparse TOF deep fusion as described above.

[0011] In this application, a visual inertial odometry (VIO) method based on sparse TOF deep fusion is proposed to solve the problem of failure of existing fusion-depth visual inertial odometry depth constraint strategies. This method effectively and accurately utilizes depth constraint strategies, thereby improving the positioning accuracy of VIO. Attached Figure Description

[0012] The accompanying drawings, which form part of this application, are used to provide a further understanding of the application and to make other features, objects, and advantages of the application more apparent. The illustrative embodiments and descriptions of this application are used to explain the application and do not constitute an undue limitation of the application. In the drawings:

[0013] Figure 1 A flowchart of an optional visual inertial odometry method based on sparse TOF deep fusion provided in this application. Detailed Implementation

[0014] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort should fall within the scope of protection of the present application.

[0015] It should be noted that the terms "first," "second," etc., used in this application specification, claims, and drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of this application described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus. Without conflict, the embodiments and features in the embodiments of this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0016] When fusing extremely sparse TOF sensors with partially overlapping fields of view in VIO, there are still problems such as uneven hole distribution and inability to handle partially overlapping fields of view. Uneven hole distribution: For example, VINS-RGBD directly uses the original depth map without considering the interpolation of depth holes. Sparse TOF data has large-area holes of hundreds of pixels. Simple global interpolation (such as nearest neighbor interpolation or bilinear interpolation) will cross object boundaries, introducing geometric distortion (such as foreground depth contaminating the background) and edge blurring. Smoothing abrupt depth changes lose scene structure information and introduce a large number of incorrect depth measurements, resulting in incorrect constraints. Inability to handle partially overlapping fields of view: For example, VINS-RGBD assumes that RGB and depth maps are perfectly aligned and have the same field of view. Low-cost sparse TOF sensors (such as ADS6401 with a field of view of 45 degrees × 45 degrees) have different fields of view from wide-angle RGB cameras (90 degrees). There is no depth measurement in the RGB edge areas (outside 45 degrees). If interpolation is performed on these areas, incorrect depth will be extrapolated. If feature points in these areas are discarded, a lot of visual information will be lost. Forced interpolation will introduce incorrect depth constraints.

[0017] To address the aforementioned issues, this application provides a visual inertial odometry method based on sparse Time-of-Flight (TOF) deep fusion. Please refer to [reference needed]. Figure 1The document illustrates a flowchart of a visual inertial odometry method based on sparse TOF deep fusion according to an embodiment of this application. The method includes: S102, acquiring target point cloud data (sparse 3D point cloud data) of an autonomous mobile device at a target time stamp using a sparse TOF sensor, acquiring an RGB color image of the autonomous mobile device at the target time stamp using an RGB camera, and acquiring multidimensional motion data of the autonomous mobile device at the target time stamp using an inertial measurement unit; S104, projecting the target point cloud data onto the RGB color image to obtain a semi-dense depth map and a semi-dense depth mask corresponding to the semi-dense depth map; S106, extracting multiple feature points from the RGB color image and determining the constraint type of each feature point based on the semi-dense depth mask; S108, constructing a target optimization function based on multiple feature points, multiple constraint types, multidimensional motion data, and the RGB color image, and solving for the state vector corresponding to the autonomous mobile device, the state vector including the pose of the autonomous mobile device at the target time stamp.

[0018] In S102, a sparse TOF sensor can be understood as a sensor that can only provide a limited number of depth measurement points for images containing a large number of pixels. An autonomous mobile device can be understood as a device that can move autonomously (or automatically) through technologies such as intelligent positioning and navigation. To achieve the goal of using a sparse TOF sensor, an RGB camera, and an IMU to collect data (i.e., target point cloud data, RGB color images, and multi-dimensional motion data) from the autonomous mobile device at the target timestamp—as described in S102—the spatial transformation relationship between multiple acquisition units (sparse TOF sensor, RGB camera, and IMU) must be defined before data acquisition, and the time difference between these units must be eliminated. Before acquiring data (target point cloud data, RGB color image, and multi-dimensional motion data), the process of eliminating time and spatial differences also includes: S1, spatially aligning the TOF sensor and the RGB camera to obtain their spatial transformation relationship, which is used to convert the target point cloud data to the RGB camera coordinate system; S2, synchronizing the TOF sensor and the RGB camera in time based on the unit attribute information of the inertial measurement unit.

[0019] The operations in S1-S2 are to solve the spatial alignment and time synchronization problem between the sparse TOF sensor and the RGB-IMU system (i.e., the system composed of RGB and an inertial measurement unit (IMU), providing accurate coordinate transformation relationships for subsequent depth projection. The operation in S1 is the process of determining the spatial transformation relationships between multiple acquisition units. The process of spatial alignment in S1 to obtain the spatial transformation relationships includes: S11-1, defining the rigid body transformation relationship from the sparse TOF sensor (hereinafter referred to as TOF sensor or TOF) coordinate system to the RGB camera (hereinafter referred to as RGB) coordinate system as follows: , for The rotation matrix represents the TOF transformation relative to RGB. Translation vector, which is the position offset of TOF relative to RGB (unit: meters (can also be set according to requirements)). For one The zero vector; S11-2, define the offset between the TOF timestamp and the RGB timestamp as: , For TOF timestamps, For RGB timestamps.

[0020] S12, RGB camera calibration, specifically: using an asymmetrical circular calibration plate, with specifications as follows: (That is, the calibration plate has 9 circular markings in the horizontal direction and 12 in the vertical direction), the distance between the centers of the circles is... (The distance between the centers of adjacent circular markers is) ), circle diameter (The diameter of each circular marker is) Simultaneously acquire RGB calibration images, TOF point clouds, and IMU data, and collect approximately 20-30 sets of calibration board data in different poses (calibration board data in different poses helps improve the accuracy and robustness of calibration because the camera can obtain more comprehensive information by observing the calibration board from different angles); use Zhang Zhengyou's calibration method to solve for the RGB camera intrinsic parameters: , and These are the focal lengths (in pixels) of RGB in the x and y directions, respectively. and These are the coordinates of the principal point of the image (i.e., the coordinates of the intersection of the optical axis and the image plane in the pixel coordinate system). The distortion coefficients are then calculated. , It is the radial distortion coefficient (used to describe the degree of bending of light rays away from the center of the image). Here are the tangential distortion coefficients (used to describe the distortion caused by the non-parallelism between the camera plane and the lens plane); construct the calibration optimization function (for camera calibration): , The number of calibration board images acquired (i.e., the number of different poses); The number of circular markers on each calibration plate image (for The calibration plate, M= ), For the first The first image The actual detected pixel coordinates of each circular marker point For a distorted projection function, The first calibration board The function calculates the 3D coordinates (world coordinate system) of the center of the nth circular marker on the calibration plate. The 3D coordinates of the center of each circular marker (in the world coordinate system, denoted as...) (Through the camera's internal parameters) Distortion coefficient Rotation matrix Translation vector The transformation is performed, projecting the pixel coordinates onto the image plane. The calculation process of this function considers the camera's imaging model and distortion model, and the specific calculations are quite complex. It typically involves transforming 3D points to the camera coordinate system through rotation and translation, then projecting them onto the image plane using an intrinsic parameter matrix, and finally applying a distortion model to correct the distortion of the projected points. The purpose of the above camera calibration optimization function is to minimize... To optimize In other words, by adjusting these parameters, the accuracy of camera calibration is improved. The purpose of camera calibration in this application is to solve for the camera's intrinsic parameter matrix and distortion coefficients, as well as the pose (rotation matrix and translation vector) of the calibration board relative to the camera in each image, by collecting calibration board data in different poses, thereby ensuring that the information obtained from the images is accurate and reliable.

[0021] S13, TOF point cloud calibration board detection, specifically including: S13-1, plane fitting: for each frame of TOF point cloud The calibration plate plane is fitted using the RANSAC (Random Sample Consensus) algorithm: the plane equation is expressed as: , It is a plane normal vector (unit vector). For TOF point clouds, This represents the distance from a point in the point cloud to the plane; the specific distance is: The specific process of RANSAC includes: (1) Random sampling: Randomly sample 3 points to fit the initial plane; (2) Distance calculation and interior point determination: Calculate the distance from all points to the plane; Interior point determination: If the distance is less than the interior point, then the interior point is determined by the distance from the point to the plane. (threshold) (3) Iteration and plane selection: Repeat 1000 times and select the plane with the most interior points; use all interior points to optimize the plane parameters using the least squares method. The plane optimization objective is: , This indicates that the constraints are met. Represents the plane normal vector. As a unit vector, the objective of this plane optimization is to adjust... , making To achieve the minimum, the optimal fit of the plane is obtained. S13-2, 3D coordinate recovery of the center: (1) Projection to the plane coordinate system: The points in the TOF point cloud are orthogonally projected onto the calibration plate plane obtained by fitting, and a point on the plane is used as the reference. and two orthogonal unit vectors in the plane , Establish a local two-dimensional plane coordinate system to obtain the two-dimensional coordinates of a point in that plane coordinate system. The projection formula is: (2) Depth image generation: Generate 2D depth images in a planar coordinate system. Values ​​for each pixel: (i.e., take the point projected onto the pixel area) arrive (3) Circle detection: For the generated depth image, the minimum distance is used as the pixel value); Thresholding segmentation is performed, the center of the circular marker is detected through the RGB color image, and the corresponding 3D position is recovered by combining the TOF plane fitting results, represented by a binarization function: , The mean of the background depth. Then, connected component analysis was used to extract circular regions, and the centroid of each circle was calculated. (4) Convert back to 3D coordinates: 3D coordinates of the center of the circle (TOF coordinate system) (that is, convert the coordinates of the detected center of the circle in the planar coordinate system back to the 3D coordinates in the ToF coordinate system), the formula is:

[0022] In step S13-2, the plane fitting step accurately fits the calibration board plane from the TOF point cloud using the RANSAC algorithm, eliminating the influence of outliers. The 3D coordinate recovery of the circle center projects the point cloud onto a planar coordinate system, generating a depth image. Through threshold segmentation and connected component analysis, circles in the calibration board are detected and their centroids are calculated. Finally, the centroid coordinates are transformed back into 3D space, providing accurate information about the circle center position for subsequent steps.

[0023] S14, Initial estimation of extrinsic parameters (the process of establishing relationships through corresponding points and solving pose using the EPnP algorithm, and then optimizing extrinsic parameters): (1) Establishment of corresponding points: establish the RGB center through the topological relationship of the calibration plate circle. With the center of TOF The correspondence. For the first... Frame, known RGB center pixel coordinates: 3D coordinates of the TOF center: . Indicates the number of multiple marker points. (2) Solve the pose of the calibration plate in the RGB camera coordinate system. : The least-squares objective function of this problem is solved using the EPNP algorithm (an efficient pose estimation method): , It is a projection function that projects 3D points onto a 2D image plane. and Rotate the matrix and translate the vector respectively. (3) Hand-eye calibration: The pose of the calibration board in each frame is known in two coordinate systems: RGB coordinate system: In the TOF coordinate system: (Determined by the plane normal vector and the distribution of the circle center), under ideal conditions, the extrinsic parameters should satisfy the following relationship: The expanded form is as follows: , and The extrinsic parameters to be solved, namely the rotation and translation from the TOF coordinate system to the RGB coordinate system, are solved using multi-frame data. Considering the errors in center detection, PnP pose estimation, and TOF plane fitting, the following residual term is constructed and least-squares optimization is performed using multi-frame data. The optimization objective is:

[0024] Used to minimize , making Optimal Here, Frobenius norm is used to measure the difference between matrices. The core of S14 is to solve for the pose under the RGB camera using the EPnP algorithm through corresponding points on the calibration board. Then, through hand-eye calibration, it optimizes the extrinsic parameters from the TOF coordinate system to the RGB coordinate system using multi-frame data, thereby ensuring accurate alignment and fusion of TOF and RGB data in a unified coordinate system. In the above formula, , , , All of these estimations originate from PnP, plane fitting, and circle center detection, and contain noise and errors. Therefore, it is necessary to solve for the optimal extrinsic parameters through multi-frame least squares. Only when there is no noise and the extrinsic parameters are completely correct can the residual be zero.

[0025] S15, ICP (Iterative Closest Point) fine alignment: (1) Point cloud transformation and depth back projection: a. Point cloud transformation: using initial values ( Rotation matrix Translation matrix The combination of, that is Transform the TOF point cloud to the RGB coordinate system using the following formula: , For rotation matrix, It is a translation vector. b. Number of points in the TOF point cloud. (This is the number of points in the TOF point cloud.) Depth back projection: (This refers to the depth of the circle detected by RGB.) , Using the depth back projection of the PnP results into 3D space: the formula is: Depth value From PnP pose and world coordinate system The calculation yielded: The ICP algorithm converges when the following condition is met: the Frobenius norm of the rotation increment. Norm of translation increment (2) The objective function of ICP (minimizing the alignment error between two point clouds) is: . and These are the increments for rotation and translation, and the weights. Adaptive adjustment based on point-to-point distance , It is the residual vector. It is a parameter that controls the weight decay. (3) Residual vector and iterative update: residual vector , representing the difference between the current TOF point cloud and the corresponding RGB points. The iterative update formula (using Lie algebra or small angle approximation to represent the rotation increment) is: , is the antisymmetric matrix of the rotation increment. The ICP fine alignment process in S15 is optimized iteratively by continuously adjusting the transformation parameters (rotation and translation) from the TOF point cloud to the RGB coordinate system to minimize the alignment error between the two point clouds. The use of weights makes the algorithm robust to outliers, while the convergence condition ensures that the algorithm stops iterating when it reaches sufficient fineness.

[0026] S16, Time Synchronization: This application employs IMU-assisted time synchronization, utilizing the IMU's high-frequency measurements (e.g., 200Hz) as a time reference to estimate the time offset of RGB and TOF relative to the IMU, thereby indirectly obtaining the time offset. , The time offset of TOF relative to IMU. This refers to the time offset of RGB relative to the IMU. That is, the high-frequency measurements of the IMU (Inertial Measurement Unit) are used as the time reference. The IMU can provide accurate time information at a high frequency. By estimating the time offset of the RGB camera and the TOF sensor relative to the IMU separately, the time offset between RGB and TOF is indirectly obtained, thus unifying the data from different sensors onto the same time reference.

[0027] S17, Verification of calibration results, including: (1) Reprojection error, calculated using the following formula:

[0028] It is the number of calibration board images acquired. It is the number of feature points (such as the number of circular markers) on each calibration board image. It is the first The first image The actual detected pixel coordinates of each feature point. It is a projection function that projects 3D points onto the image plane using camera intrinsics and pose to obtain theoretical pixel coordinates. The pass / fail criterion for verifying calibration results through RGB reprojection error is... This error reflects the accuracy with which the calibrated camera model projects 3D points onto the 2D image plane. The smaller the error, the more accurate the calibration result, and the more realistically the camera model reflects the actual imaging process. (2) 3D distance error, the formula is: , It is a pose transformation from Time-of-Flight (TOF) to RGB that converts 3D points in TOF mode. Convert to RGB coordinates These are the actual 3D point coordinates in RGB mode. The passing standard for 3D distance error verification is: (At a distance of 1-3 meters). (3) Time synchronization error: , It is the estimated time offset. This is the actual time offset. The passing criterion for this error verification is: (For a 30Hz sensor, less than one frame time).

[0029] The semi-dense depth mask in S104 is used to indicate whether each pixel in the semi-dense depth map has a valid depth value. S106 includes: if the mask flag corresponding to a feature point is determined to be a valid mask flag based on the semi-dense depth mask, the constraint type of the feature point is determined to be a first depth constraint type; if the mask flag corresponding to a feature point is determined to be an invalid mask flag based on the semi-dense depth mask, the constraint type of the feature point is determined to be a second depth constraint type. The first depth constraint type indicates that the feature point has a depth constraint, and the second depth constraint type indicates that the feature point has no depth constraint.

[0030] The implementation method of this application adopts a visual inertial odometry method based on sparse TOF deep fusion, which solves the problem of failure of the depth constraint strategy of existing visual inertial odometry with fusion depth, and effectively and accurately utilizes the depth constraint strategy, thereby improving the positioning accuracy of VIO.

[0031] As an optional implementation, S104 includes: S1, converting the target point cloud data to the RGB camera coordinate system to obtain reference point cloud data; S2, projecting the reference point cloud data onto an RGB color image, and marking the depth value of each projected pixel to obtain the projected image and the pixel coordinates corresponding to each projected pixel; S3, discretizing and conflict processing the projected image based on multiple pixel coordinates to obtain a sparse depth map, and updating the initial depth mask corresponding to the RGB color image according to the depth value marking; S4, performing guided filtering to complete the sparse depth map to obtain a semi-dense depth map and a semi-dense depth mask.

[0032] The transformation (rigid body transformation) formula in S1 is: , To make the RGB version The coordinates of each point are the transformed target coordinates. for The rotation matrix represents the TOF transformation relative to RGB. This is a translation vector, representing the positional offset of the TOF relative to RGB. Depth value definition: This refers to the distance of the point along the optical axis in the RGB camera coordinate system. In the RGB coordinate system, the optical axis is typically defined as the Z-axis. For points converted to RGB... ,That coordinates This represents the distance from the point to the camera along the camera's optical axis. In S2, a pinhole projection model is used to project the reference point cloud data (3D spatial points) onto an RGB color image (2D pixel plane). The projection formula (homogeneous coordinate form) is: , This is the depth scaling factor. These are homogeneous coordinates on a two-dimensional pixel plane. This represents the coordinates of a 3D point in RGB (i.e., reference point cloud data). To represent the camera's intrinsic parameter matrix, homogeneous coordinates are introduced to facilitate the representation of projection transformations using matrix multiplication, allowing translation and other operations to be handled uniformly through matrix multiplication. Pixels coordinate Pixels coordinate , , Focal length (unit: pixels). , Principal point coordinates (the coordinates of the intersection of the camera's optical axis and the image plane on the pixel plane).

[0033] After projecting onto the RGB color image, a validity check is performed on the reference point cloud. The check formula is as follows:

[0034] For minimum effective depth, validity check ( This is used to filter out invalid points behind or too close to the camera. and : and These represent the width and height of the image (in pixels). These two conditions ensure that the pixel coordinates obtained from the projection are within the valid range of the image, meaning that the point can correctly fall on the image plane. Only points that simultaneously satisfy the depth condition and the pixel coordinate range condition are considered valid projection points, and valid projection points are the projected pixels.

[0035] S3 specifically includes: (1) Discretization (used to round the floating-point pixel coordinates to the nearest integer pixel), the formula is: , .

[0036] In the pinhole projection model, the obtained pixel coordinates and Typically, floating-point numbers are used, while pixel positions in an image are represented by integer coordinates. The discretization process maps floating-point coordinates to discrete integer pixel coordinates in order to determine the pixel position corresponding to the TOF point in the RGB color image.

[0037] (2) Conflict handling: Since the number of sparse TOF points is much smaller than the number of RGB pixels, multiple TOF points may be projected onto the same pixel in the RGB color image, so conflict handling is required. This application adopts a strategy of preserving closer depth, that is, nearby objects occlude distant objects, preserving closer depth conforms to the visibility constraint, and the noise of TOF measurement increases with distance, so close distance measurement (noise) is more reliable.

[0038] The initial depth mask indicates the initial depth value and initial depth validity index for each pixel in the RGB color image. An initial depth value of 0 (no depth value) and an initial depth validity index of 0 (indicating an invalid depth value) are used. When the reference point cloud data is projected onto the RGB color image, the pixel projected onto the RGB color image is marked with a depth value based on the TOF depth value of the projection point (i.e., the depth value corresponding to the pixel is marked). For the same pixel, a depth value is marked for each projection point projected onto that pixel. For example, if three TOF points are projected onto the same pixel A, the pixel will be marked with the depth values ​​corresponding to these three TOF points. After the TOF point cloud is projected onto the RGB color image, each pixel in the RGB color image that has been projected onto a projection pixel has a depth value mark (one or more). At this time, conflict handling is required. When the first mask is updated, each pixel is double-updated. The first mask update includes: filtering multiple target pixels from the RGB color image based on depth value markers (i.e., filtering out pixels with depth value markers); for each target pixel, performing the following operations: determining the number of depth value markers corresponding to the target pixel; when the number of depth value markers is 1, updating the initial depth value in the initial depth mask corresponding to the target pixel to the depth value corresponding to the depth value marker, and updating the initial validity index in the initial depth mask corresponding to the target pixel from 0 to 1; when the number of depth value markers is greater than 1, determining any depth value marker as the current depth value marker, and updating the initial validity index in the initial depth mask corresponding to the target pixel from 0 to 1; determining multiple first depth values ​​corresponding to multiple depth value markers, determining the minimum depth value among the multiple depth values ​​as the second depth value; and updating the initial depth value in the initial depth mask corresponding to the target pixel to the second depth value. The depth value update in the first mask update can also be performed using the following formula:

[0039] As a measure of depth effectiveness, If the condition is met, it means that there is no valid depth value for that pixel. In this case, the current TOF depth value is directly assigned to the depth value at that pixel location. When the value of a pixel is found to be valid, the original TOF depth value of the projection point corresponding to that pixel is compared. The strategy involves taking the smaller of the two depth values ​​(the one already present at the pixel location) as the new depth value for that pixel, thus preserving the closer depth point.

[0040] S4 includes: S41 performing grayscale processing on the RGB color image to obtain a grayscale guide map; S42 performing guided filtering and completion operation on the sparse depth map based on the grayscale guide map and the reference depth mask to obtain a semi-dense depth map and a semi-dense depth mask. S42 specifically includes: S42-1, constructing multiple reference windows based on the first preset window size, with each sparse pixel in the sparse depth map as the window center, and determining any one of the reference windows as the current reference window; S42-2, constructing a linear relationship between the current predicted image and the grayscale guide image for guided filtering based on the reference depth mask, and constructing an objective function based on the linear relationship, the linear relationship including the first local linear coefficient and the second local linear coefficient corresponding to the current reference window; S42-3, solving the objective function using the least squares method to obtain the first coefficient value corresponding to the first local linear coefficient and the second coefficient value corresponding to the second local linear coefficient, and determining the current predicted image based on the first coefficient value and the second coefficient value; S42-4, determining any reference window other than the current reference window as the current reference window; S42-5, determining a reference image and the first depth mask corresponding to the reference image based on the multiple predicted images and the reference depth mask, and selectively completing the reference image based on the first depth mask to obtain a semi-dense depth map and a semi-dense depth mask.

[0041] Grayscale conversion can be achieved using the following formula:

[0042] The weights (0.299, 0.587, 0.114) for converting RGB images to grayscale are based on the ITU-R BT.601 standard. These weights reflect the human eye's sensitivity to the brightness of different colors. Representing coordinates The pixel value of the red channel; Representing coordinates The pixel value of the red channel; Representing coordinates The pixel values ​​of the red channel. Weighted filtering can convert the rich color information in a color image to grayscale in the floating-point range of [0,1]. S42-1 to S42-2 can be understood as: guided filtering on the already obtained grayscale guided image. Under the guidance of sparse depth maps Processing is performed to achieve edge-preserving smoothing and interpolation. A window centered on a specific pixel. The output of the guided filter (e.g., within a 5x5 neighborhood). With guide map There exists a linear relationship (i.e., a linear equation): , (First local linear coefficients) and (Second local linear coefficients) are the window The local linear coefficients within the range. The objective function is: , Input depth (representing the input sparse depth map in pixels) (depth value at the location) Regularization parameter (typically value is 1) The objective function aims to find a suitable (optimal) one. and This makes it possible to display in the window. Within this model, the sum of squared errors between the linear relationship based on the grayscale guide image and the input depth is minimized. Regularization terms can prevent Too big, too big This can amplify noise and affect the filtering effect. By minimizing the objective function, stability can be maintained while fitting depth data, avoiding overfitting to noise. Solving for the objective function yields the optimal linear coefficients within the window: , This represents the average grayscale guide image value within the window, reflecting the average brightness level of the grayscale guide image within the local window. The variance of the guide image within the window reflects the degree of brightness variation of the grayscale guide image within a local window. When the size is larger (the more drastic the change, such as at the edge), Output (Close to local mean) because in the edge region, in order to maintain the discontinuity of depth (i.e. allow depth discontinuity) and avoid erroneous smooth transitions, making the output close to the local mean can better preserve edge information and prevent different surface depth values ​​from being incorrectly mixed. When the grayscale is small (flat, like a single surface), A larger output follows the changes in the guide map, achieving smooth interpolation. That is, in flat areas, the grayscale changes in the guide map are smaller, utilizing a larger output. This allows the output to be smoothly adjusted based on subtle changes in the guide graph, thereby effectively interpolating and supplementing the sparse depth map while maintaining consistency with the guide graph structure. Enter the average depth within the window. The value of is influenced by factors such as the statistical properties of the grayscale guide image (mean, variance) and the mean of the product of the input depth and the grayscale guide image. When the variance is large (e.g., in edge regions), The value of helps preserve edge structure and enables smooth depth interpolation when variance is small (in flat regions). The optimal bias term (second linear coefficient) within the window. , The bias term is used to adjust the linear model. Determining the reference image includes: Prediction calculation: averaging the predictions over all overlapping windows. Because windows overlap, each pixel belongs to multiple windows. To improve robustness, the average of all window predictions is taken for all containing pixels. The predicted values ​​of the windows are averaged. Information from multiple local windows is considered comprehensively to reduce errors that may be introduced by a single window prediction and improve the robustness of the prediction results. Since most areas in the sparse depth map lack effective depth measurements, directly applying guided filtering would spread regions with pixel depth values ​​of 0 to the effective depth pixel region, causing interference and destruction of the effective depth information and degrading the quality of the final depth map. Therefore, preprocessing is required. The effective pixel depth set is as follows: Only in calculation , When calculating statistics, invalid pixels are ignored. The depth mean is calculated using the formula for effective pixels (pixels). ,and and The calculation remains unchanged.

[0043] S42-5 includes: S42-5-1, determining the center pixel of the window according to a preset completion rule, constructing a target window according to a second preset window size and the center pixel of the window, and determining the neighboring window corresponding to the target window according to a preset step size; S42-5-2, determining the center pixel mask of the center pixel of the window according to a first depth mask, and determining the number of effective pixels corresponding to the neighboring window according to the first depth mask; S42-5-3, updating the first depth mask corresponding to the reference image according to the center pixel mask and the number of effective pixels, to obtain a semi-dense depth map and a semi-dense depth mask.

[0044] Selective completion: If guided filtering is applied globally, interpolation will be performed on all hole regions. However, for large holes (such as non-overlapping areas of the field of view), interpolation is unreliable. Therefore, selective completion is performed using the number of effective neighboring points as a criterion. The number of effective depth pixels within the domain window. , In pixels Center point window, Used to mark pixels Does a valid depth measurement mask exist (1 for valid, 0 for invalid)? Complete the criterion: Complete

[0045] Experience points .

[0046] Depth and mask updates:

[0047] Small holes (neighborhood valid points greater than or equal to 5): guide the reliable filtering interpolation and complete the hole; Large voids (less than 5 valid neighborhood points): interpolation is unreliable, maintaining a sparse state; Avoid false depth filling in non-overlapping areas of the field of view (RGB edges).

[0048] The sparse depth map obtained in this application has the following data characteristics: number of effective pixels: Coverage: Specific data characteristics of semi-dense depth maps: (Depending on hole distribution), compared to sparse depth maps, the effective pixel count has been significantly improved. Spatial distribution: RGB central region (within the TOF field of view): dense distribution, local coverage. RGB edge region (outside the TOF field of view): completely sparse. .

[0049] Through the implementation method of this application, a semi-dense depth map and a semi-dense depth mask that can be used for depth constraints can be accurately obtained, so that depth constraint strategies can be effectively and accurately utilized in the future.

[0050] As an optional implementation, S108 includes: S1, acquiring multi-frame acquisition data corresponding to multiple reference timestamps included in a preset sliding window, with each frame of acquisition data corresponding to a reference timestamp; S2, constructing a depth constraint residual based on the multi-frame acquisition data, multiple feature points, and multiple constraint types; S3, constructing a target optimization function based on the depth constraint residual, multi-frame acquisition data, multiple feature points, multi-dimensional motion data, and an RGB color image. S3 includes: S3-1, constructing a visual reprojection residual based on the multi-frame acquisition data, multiple feature points, and an RGB color image; S3-2, constructing a motion data residual based on the multi-frame acquisition data, multiple feature points, and multi-dimensional motion data; S3-3, constructing a marginalization prior residual based on the multi-frame acquisition data; S3-4, constructing a target optimization function based on the depth constraint residual, visual reprojection residual, motion data residual, and marginalization prior residual.

[0051] Multiple reference timestamps are multiple consecutive timestamps before the target timestamp. The data collected for each frame includes the reference RGB image, reference multidimensional motion data, reference pose, etc., corresponding to that frame. That is, before constructing the target optimization function, this application needs to obtain historical data under multiple consecutive timestamps earlier than the target timestamp, in addition to obtaining the semi-dense depth map and the semi-dense depth mask corresponding to the semi-dense depth map. In summary, the construction of the target optimization function in this application includes (1) RGB image sequence (including RGB color images and reference RGB images corresponding to multiple reference timestamps) and extracted feature points. ,in: : No. Frame RGB color image; : No. The set of feature points extracted from the frame; : No. The first frame The pixel coordinates of each feature point; : No. Number of feature points in the frame. (2) Semi-dense depth map sequence (including the obtained semi-dense depth map and semi-dense depth mask, and the reference semi-dense depth map and reference semi-dense depth mask corresponding to each reference timestamp): , (3) IMU measurement sequence (including multidimensional motion data and reference multidimensional motion data included in each frame of acquired data): , The reference timestamp and target timestamp are the accelerometer and gyroscope measurements respectively (i.e., reference multidimensional motion data and multidimensional motion data); (4) the initial pose is represented as (From initialization or optimization results from the previous moment). After constructing and solving the target optimization function in this application, we can obtain: (1) the optimized pose sequence (i.e., the pose under the target timestamp): (2) Optimized speed and bias (3) Optimized feature point depth .

[0052] When constructing the objective optimization function, a sliding window strategy is adopted, retaining the state of the most recent 10 frames (i.e., multiple reference timestamps) for optimization. Finally, the optimized data for each frame is obtained. S1 to S3 include: acquiring the data in S1; and before constructing the residuals, a state vector is defined, containing the state and feature point depth of all frames within the sliding window. That is, the state vector contains the state information of each frame within the sliding window and the inverse depth information of the feature points, and the state vector of a single frame. Includes pose, velocity, and IMU bias; pose , Let be the rotation matrix, describing the first... Pose transformation from the frame body (which can be understood as, but is not limited to, the autonomous mobile device) coordinate system to the world coordinate system. The rotation matrix belongs to the special orthogonal group. , Pixel coordinates are two-dimensional vectors. (depth), (Inverse Depth) (Scale factor) is a scalar; Let be the translation vector, representing the first... The position and velocity of the frame body coordinate system origin in the world coordinate system. : No. Velocity in the frame body coordinate system : Accelerometer bias, Gyroscope bias. Inverse depth parameterization: , For feature points Depth in the camera coordinate system of the first observation frame. Inverse depth parameterization makes the uncertainty of distant points more consistent with a Gaussian distribution.

[0053] The construction of depth-constrained residuals is to construct depth-constrained residuals for feature points with TOF depth measurement in each frame: (1) through semi-dense depth masks This implements a unified optimization framework for both deep and shallow regions, determining the depth constraint type of feature points: such as feature points In the The formula for determining the constraint type of a frame is: If This proves that the constraint type is a depth constraint. This proves that the constraint type is without depth constraints. With depth constraints, both visual and depth residuals are applied simultaneously; without depth constraints, only visual residuals are applied. In the objective function, the summation range of the depth constraint residuals is (only those with depth constraints are summed): Automatically skip features without depth, requiring no additional judgment logic. (2) Depth measurement, depth measurement value , For a semi-dense depth map, this formula represents the method for obtaining the first... Feature points in the frame The depth measurement value. (3) Depth prediction value (Euclidean distance), calculated from the state variables, the feature point at the th depth. Frame depth prediction , , Feature points calculated from state variables In the Calculate the modulus (Euclidean distance) of the 3D coordinates in the camera coordinate system to obtain the feature points. To the The distance from the camera origin is used as the depth prediction value. This step combines the system's state variables, transforming state information into a prediction of feature point depth. The constructed depth-constrained residual is... .

[0054] This application constructs an objective optimization function and sets adaptive weights for the depth-constrained residuals. The weights of the depth-constrained residuals are determined by the information matrix and the TOF noise model, achieving adaptive weighting: weights close range (such as) ): Weight (High weight); Medium distance (e.g.) ): Weight (Medium weight); Long distance (e.g.) ): Weight (Low weight), Information Matrix: The TOF noise model is as follows: , (Basic noise) (Linear coefficients) (Quadratic coefficient).

[0055] Visual reprojection residuals, such as for feature points In the The observation of frames and the construction of visual reprojection residuals include: (1) normalization of feature point coordinates, feature points In the first observation frame Its normalized planar coordinates are expressed as: , (2) 3D calculation of feature points: The 3D position of the feature point in the camera coordinate system of the first observation frame: 3D position in world coordinate system: , Indicates the first First observation of feature points At frame rate, pose transformation from body coordinate system to world coordinate system. For the fixed extrinsic parameter transformation from the camera coordinate system to the body coordinate system. (3) Projection of feature points in other frame camera coordinate systems: The point is projected onto the first frame. 3D position under frame camera: Pixel coordinate prediction Projection function The constructed visual reprojection residual: , For feature points In the The actual observed pixel coordinates of the frame.

[0056] IMU pre-integration is performed based on accelerometer and gyroscope measurements between two adjacent frames to obtain position pre-integration, velocity pre-integration, and rotation pre-integration. The IMU pre-integration residual is then constructed based on these three pre-integration values.

[0057] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0058] This application also provides a visual inertial odometry device based on sparse TOF deep fusion for implementing a visual inertial odometry method. The device includes: an acquisition unit for performing step S102; a projection unit for performing step S104; an extraction unit for performing step S106; and a construction unit for performing step S108. The specific execution methods of each unit in the device embodiments have been described in detail in the embodiments related to the method, and will not be elaborated further here.

[0059] This invention also provides an electronic device for implementing a visual inertial odometry method based on sparse Time-of-Flight (TOF) deep fusion. The electronic device can be a terminal device or a server. Taking a terminal device as an example: the electronic device includes: at least one processor; and a memory communicatively connected to the at least one processor; the memory stores a computer program executable by the at least one processor, which is executed by the at least one processor to cause the at least one processor to perform the steps in any of the method embodiments. The electronic device may be located in at least one of a plurality of network devices in a computer network. The processor is configured to execute the visual inertial odometry method based on sparse TOF deep fusion via the computer program.

[0060] The above are merely preferred embodiments of this application and are not intended to limit this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A visual inertial odometry method based on sparse TOF deep fusion, characterized in that, Applied to autonomous mobile devices, the method includes: The autonomous mobile device is equipped with a sparse TOF sensor to collect target point cloud data at the target time point, an RGB camera to collect RGB color images of the autonomous mobile device at the target time point, and an inertial measurement unit to collect multidimensional motion data of the autonomous mobile device at the target time point. The target point cloud data is projected onto the RGB color image to obtain a semi-dense depth map and a semi-dense depth mask corresponding to the semi-dense depth map. Multiple feature points are extracted from the RGB color image, and the constraint type of each feature point is determined according to the semi-dense depth mask; Based on the multiple feature points, multiple constraint types, multidimensional motion data, and the RGB color image, a target optimization function is constructed to solve for the state vector corresponding to the autonomous mobile device, wherein the state vector includes the pose of the autonomous mobile device at the target timestamp.

2. The method according to claim 1, characterized in that, Projecting the target point cloud data onto the RGB color image to obtain a semi-dense depth map and a corresponding semi-dense depth mask includes: The target point cloud data is converted to the RGB camera coordinate system to obtain reference point cloud data; The reference point cloud data is projected onto the RGB color image, and a depth value is marked for each projected pixel to obtain the projected image and the pixel coordinates corresponding to each projected pixel. The projected image is discretized and conflict-handled based on multiple pixel coordinates to obtain a sparse depth map. The initial depth mask corresponding to the RGB color image is updated based on the depth value label to obtain a reference depth mask. The sparse depth map is completed by guided filtering to obtain the semi-dense depth map and the semi-dense depth mask.

3. The method according to claim 2, characterized in that, Guided filtering is performed on the sparse depth map to complete the semi-dense depth map and the semi-dense depth mask, including: The RGB color image is converted to grayscale to obtain a grayscale guide image; Based on the gray-level guiding image and the reference depth mask, a guided filtering completion operation is performed on the sparse depth image to obtain the semi-dense depth image and the semi-dense depth mask.

4. The method according to claim 3, characterized in that, Based on the gray-level guiding image and the reference depth mask, a guided filtering completion operation is performed on the sparse depth map to obtain the semi-dense depth map and the semi-dense depth mask, including: Based on the first preset window size, multiple reference windows are constructed with each sparse pixel in the sparse depth map as the window center, and any reference window is determined as the current reference window. Based on the reference depth mask, a linear relationship is constructed between the current predicted image of the guided filter and the grayscale guided image of the current reference window, and an objective function is constructed based on the linear relationship. The linear relationship includes the first local linear coefficient and the second local linear coefficient corresponding to the current reference window. The objective function is solved using the least squares method to obtain the first coefficient value corresponding to the first local linear coefficient and the second coefficient value corresponding to the second local linear coefficient, and the current predicted image is determined based on the first coefficient value and the second coefficient value. Determine any reference window other than the current reference window as the current reference window; A reference image is determined based on multiple predicted images and the reference depth mask, and a first depth mask corresponding to the reference image is obtained. The reference image is then selectively completed based on the first depth mask to obtain the semi-dense depth map and the semi-dense depth mask.

5. The method according to claim 4, characterized in that, Selective completion of the reference image based on the first depth mask to obtain the semi-dense depth map and the semi-dense depth mask includes: The center pixel of the window is determined according to the preset completion rule, the target window is constructed according to the second preset window size and the center pixel of the window, and the neighboring window corresponding to the target window is determined according to the preset step size. The center pixel mask of the window center pixel is determined based on the first depth mask, and the number of effective pixels corresponding to the neighboring window is determined based on the first depth mask. The first depth mask corresponding to the reference image is updated based on the center pixel mask and the number of effective pixels to obtain the semi-dense depth map and the semi-dense depth mask.

6. The method according to claim 1, characterized in that, The constraint type of each feature point is determined based on the semi-dense depth mask, including: If the mask flag corresponding to the feature point is determined to be a valid mask identifier based on the semi-dense depth mask, the constraint type of the feature point is determined to be the first depth constraint type; If the mask flag corresponding to the feature point is determined to be an invalid mask flag based on the semi-dense depth mask, the constraint type of the feature point is determined to be the second depth constraint type.

7. The method according to claim 6, characterized in that, Based on multiple feature points, multiple constraint types, multidimensional motion data, and the RGB color image, the objective optimization function is constructed, including: Acquire multiple frames of data corresponding to multiple reference timestamps included in the preset sliding window, wherein each frame of data corresponds to one reference timestamp; A deep constraint residual is constructed based on the acquired data from multiple frames, multiple feature points, and multiple constraint types. The target optimization function is constructed based on the depth constraint residual, the acquired data from multiple frames, the multiple feature points, the multidimensional motion data, and the RGB color image.

8. The method according to claim 7, characterized in that, Based on the depth constraint residual, multiple frames of acquired data, multiple feature points, the multidimensional motion data, and the RGB color image, the objective optimization function is constructed, including: Visual reprojection residuals are constructed based on the acquired data from multiple frames, the multiple feature points, and the RGB color image. Motion data residuals are constructed based on the acquired data from multiple frames, the multiple feature points, and the multidimensional motion data. Construct edge-optimized prior residuals based on the acquired data from multiple frames; The objective optimization function is constructed based on the depth constraint residual, the visual reprojection residual, the motion data residual, and the marginalization prior residual.

9. The method according to claim 1, characterized in that, Before acquiring target point cloud data at the target timestamp using a sparse TOF sensor, the method further includes: The TOF sensor and the RGB camera are spatially aligned to obtain the spatial transformation relationship between the TOF sensor and the RGB camera. The spatial transformation relationship is used to transform the target point cloud data into the RGB camera coordinate system. Based on the unit attribute information of the inertial measurement unit, the TOF sensor and the RGB camera are synchronized in time.

10. A visual inertial odometry device based on sparse TOF deep fusion, characterized in that, Applied to autonomous mobile devices, the device includes: The acquisition unit is used to acquire target point cloud data of the autonomous mobile device at the target time point using a sparse TOF sensor, acquire RGB color images of the autonomous mobile device at the target time point using an RGB camera, and acquire multidimensional motion data of the autonomous mobile device at the target time point using an inertial measurement unit. A projection unit is used to project the target point cloud data onto the RGB color image to obtain a semi-dense depth map and a semi-dense depth mask corresponding to the semi-dense depth map. An extraction unit is used to extract multiple feature points from the RGB color image and determine the constraint type of each feature point according to the semi-dense depth mask; The construction unit is used to construct a target optimization function based on multiple feature points, multiple constraint types, multidimensional motion data, and the RGB color image, and solve for the state vector corresponding to the autonomous mobile device, wherein the state vector includes the pose of the autonomous mobile device at the target timestamp.

Images