Robust visual inertial odometry method and device in dynamic scenes

Abstract

This invention discloses a robust visual inertial odometry method and device for dynamic scenes, belonging to the field of visual inertial odometry technology. The method includes: Step 1, acquiring grayscale images, depth images, and IMU information and preprocessing them to obtain depth information; Step 2, performing coarse feature matching based on depth information and inter-frame IMU integration, constructing corresponding descriptors and matching functions using the grayscale information and depth information of feature points; Step 3, detecting sway states using IMU information, and upon detecting sway states, re-extracting feature points and uniformly distributing the re-extracted feature points using a gridding method; Step 4, constructing an inverse depth residual using the re-extracted feature points and the triangulated depth information, and designing an optimization function. This invention enables stable positioning in complex dynamic scenes indoors using a depth camera, greatly improving the robustness of positioning.

Description

Technical Field

[0001] The present invention belongs to the technical field of visual inertial odometry, and in particular relates to a robust visual inertial odometry method and device in dynamic scenes, which can be used for robust positioning of visual inertial odometry in complex dynamic scenes indoors. Background Art

[0002] After years of development, visual inertial odometry has become a stable and reliable positioning solution. However, in dynamic scenes, the image blur caused by camera shake has a great impact on the front-end visual features: on the one hand, the image grayscale gradient changes are not obvious due to image blur, which leads to a small number of high-quality feature points extracted; on the other hand, the blurred image also has a great impact on the feature tracking schemes between frames such as the optical flow method or the descriptor method. In addition, the back-end of the visual inertial odometry generally adopts a nonlinear optimization scheme, which is based on the principle of gradient descent for calculation. The high-speed motion of the camera is contrary to the low-speed motion assumption of the gradient descent scheme, resulting in poor final optimization effect. The current mainstream method is to use a deep learning solution to restore the blurred image or introduce specialized sensors such as event cameras to ensure stable posture solution under dynamic conditions.

[0003] Existing deep learning solutions are mainly divided into two approaches: one is to combine IMU information to construct the corresponding motion kernel function to perform convolution deblurring on the image, but this type of method is often time-consuming and cannot meet the real-time requirements of visual inertial odometers; the other is to directly use deep learning solutions to deblur images or extract features, but they are often not generalizable and cannot be applied in most scenarios, and often require pre-training, which is costly and not very timely.

[0004] In addition, additional sensors such as depth cameras and event cameras are introduced. Depth images are less affected in shaking scenes than grayscale images. Therefore, the solution based on depth point cloud performs better in scenes with small depth of field such as offices, but it often cannot stably locate in scenes with greater depth such as corridors. Event cameras can only collect dynamic points in the scene and can still stably extract features in dynamic scenes, but they are completely unusable in static scenes. In addition, the high price also limits its further promotion and use. Summary of the invention

[0005] To solve the above technical problems, the present invention provides a robust visual inertial odometry method and device in dynamic scenes, which improves the overall performance through front-end feature extraction and tracking of the visual inertial odometry and nonlinear optimization of the back-end: a hybrid descriptor based on grayscale image and depth image is designed at the front end, and the matching function is dynamically adjusted according to the spatial depth of the feature points in the scene to ensure the stability of feature point extraction and tracking in shaking scenes; for scenes with a large depth such as corridors, a corresponding depth mask is constructed, the feature points in the scene are eliminated and re-extracted, and the feature points are distributed using a grid uniformization scheme to ensure the stable solution of the depth information of the feature points; finally, the depth information provided by the depth point cloud and the depth information obtained by binocular triangulation solution are used to construct the corresponding residual, and an optimization function is designed at the back end to improve the performance of the back-end optimization.

[0006] To achieve the above purpose, the technical solution adopted by the present invention is as follows:

[0007] A robust visual inertial odometry method in a dynamic scene, the method comprising:

[0008] Step 1: Obtain grayscale image, depth image and IMU information and perform preprocessing to obtain depth information;

[0009] Step 2: Perform rough feature matching based on depth information and inter-frame IMU integration, and use the grayscale information and depth information of feature points to construct corresponding descriptors and matching functions;

[0010] Step 3, using IMU information to detect the shaking state. When the shaking state is detected, the depth image is used to extract the area that meets the depth requirement in the grayscale image, and the feature points that do not meet the depth condition in the area that meets the depth requirement are removed, and the feature points are re-extracted, and the re-extracted feature points are evenly distributed according to the gridding method;

[0011] Step 4: construct an inverse depth residual using the re-extracted feature points and the depth information obtained by the triangulation, and use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation and translation of the camera that obtains the depth image and the grayscale image, and design an optimization function.

[0012] On the other hand, the present invention provides a robust visual inertial odometer device in a dynamic scene, comprising:

[0013] An information acquisition unit, used to acquire grayscale images, depth images and IMU information and perform preprocessing to acquire depth information;

[0014] The hybrid descriptor matching unit is used to perform rough feature matching based on the depth information and the inter-frame IMU integral, and to construct the corresponding descriptor and matching function using the grayscale information and depth information of the feature points;

[0015] A feature extraction unit is used to detect the shaking state using the IMU information. When the shaking state is detected, the depth image is used to extract the area that meets the depth requirement in the grayscale image, and the feature points that do not meet the depth condition in the area that meets the depth requirement are removed, and the feature points are re-extracted, and the re-extracted feature points are evenly distributed according to the gridding method;

[0016] The back-end optimization unit is used to construct an inverse depth residual using the re-extracted feature points and the depth information obtained by the triangulation, and use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation and translation of the camera for obtaining the depth image and the grayscale image, and design an optimization function.

[0017] In a third aspect, the present invention provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; wherein, when the one or more programs are executed by the one or more processors, the one or more processors implement the aforementioned robust visual inertial odometer method in dynamic scenarios.

[0018] In a fourth aspect, the present invention provides a computer-readable storage medium having executable instructions stored thereon, which, when executed by a processor, enables the processor to implement the aforementioned robust visual inertial odometer method in dynamic scenarios.

[0019] The beneficial effects of the present invention are:

[0020] The present invention uses a uniform velocity model combined with depth information to roughly align the positions of feature points between previous and next frames, then uses the depth image and the grayscale image to construct a corresponding hybrid descriptor and designs a matching scheme for feature tracking between frames, which effectively improves the feature extraction and tracking effects in shaking scenes; uses depth information to set a mask, removes and extracts feature points during the shaking process, and uses a grid distribution scheme to evenly distribute the feature points to improve the accuracy of pose solution during the shaking process; uses a depth point cloud to provide depth information, and constructs an inverse depth residual with the depth information solved by a stereo camera, and obtains the Jacobian for the camera's pose and the transformation matrix from IMU to the camera to improve the final positioning performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 The figure is a flow chart of the robust visual inertial odometer method in dynamic scenes of the present invention. DETAILED DESCRIPTION

[0022] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0023] like Figure 1As shown, it is a flow chart of the robust visual inertial odometer method in dynamic scenes of the present invention, which specifically includes:

[0024] Step 1: Obtain grayscale image, depth image and IMU information and perform preprocessing; including:

[0025] Obtain grayscale images, depth images, and IMU information, register the grayscale images with the depth images, and align the image information carried by the grayscale images and depth images with the IMU information in time for subsequent shake detection and feature point position estimation;

[0026] Step 2: Perform rough feature matching based on depth information and inter-frame IMU integration, and use the grayscale information and depth information of the feature points to construct the corresponding descriptor and matching function.

[0027] Step 2.1: Extract the corner points of the grayscale image based on the Shi-tomasi corner point algorithm, use a stereo camera to triangulate the grayscale image to obtain depth information, and combine it with the depth information carried by the depth image to optimize the depth estimation and obtain the corresponding depth of the feature points.

[0028] Step 2.2: Estimate the position of the feature point in the previous frame in the next frame based on the corresponding depth of the feature point and the IMU integration information between frames.

[0029] Step 2.3: For the feature point positions in the previous frame and the estimated feature point positions in the next frame, use their respective depth images and grayscale images to construct corresponding hybrid descriptors for the corner points, and dynamically adjust the matching weights according to the corresponding depths of the feature points to perform matching, thereby completing the optimization of feature extraction and matching in shaking scenes.

[0030] Step 3: Use IMU information to detect the shaking state. When the shaking state is detected, use the depth image to extract the area that meets the depth requirements in the grayscale image, remove the feature points that do not meet the depth conditions in the area that meets the depth requirements, and re-extract the feature points. According to the gridding method, the re-extracted feature points are evenly distributed to complete the feature extraction based on the depth mask.

[0031] Step 3.1. According to the input IMU information, a corresponding sliding window is given. When the amount of data in the sliding window meets the set threshold, the mean and variance of the acceleration and angular velocity are calculated. When the mean and variance, as well as the duration of the variance meeting the threshold, meet the preset values, it is regarded as a shaking state.

[0032] Step 3.2: After detecting that the shaking state has been entered, the current feature points are filtered to retain only the valid feature points whose depth values ​​are less than the range of the depth camera. The depth mask is used to re-extract feature points from the grayscale image in the area that meets the range of the depth camera to ensure the number of feature points.

[0033] Step 3.3: For the re-extracted feature points, a gridding scheme is used. Based on the number of re-extracted feature points and the area size of the grayscale image in step 3.2 that meets the range of the depth camera, the width and length of the cell are dynamically set to ensure uniform distribution of feature points.

[0034] Step 4: construct an inverse depth residual using the re-extracted feature points and the depth information obtained by the triangulation, and use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation and translation of the camera providing the depth image and the grayscale image, and design an optimization function.

[0035] Step 4.1, constructing an inverse depth residual based on the depth information provided by the depth image corresponding to the position of the re-extracted feature point and the depth information obtained by the triangulation;

[0036] Step 4.2: Use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation matrix and translation vector of the camera that provides the depth image and the grayscale image, and design a corresponding optimization function in the backend to improve the performance of the backend optimization.

[0037] Furthermore, in step 2, at the front end, the image corners are extracted based on the Shi-tomasi corner point scheme, and the depth image and the grayscale image are aligned. The depth information of the triangulated corners obtained by using a stereo camera is fused with the depth information of the corresponding position in the depth image using a Bayesian probability estimation scheme to improve the accuracy of the estimation. After that, the IMU information is combined with the corner point depth information to predict the position of the current corner point (the corner point here is the feature point) in the next frame according to the uniform velocity model. This process is called coarse registration (the depth image has holes and noise information, and the IMU measurement has noise, so the feature point position estimation is not very reliable). On this basis, the corresponding corner points in the previous and next frames are used to calculate their mixed descriptors using the depth information and grayscale information, and the corresponding matching method is designed to obtain accurate feature point matching estimates. Specifically,

[0038] A. Rough registration;

[0039] Assume the posture matrix of the current frame of the grayscale image is , the posture matrix of the previous frame is , then the posture matrix of the next frame Using the uniform velocity model, it can be estimated as:

[0040] ,

[0041] Here What is calculated is the transformation matrix from the previous frame to the current frame, and then the transformation matrix is ​​used to predict the state from the current frame to the next frame. The superscript -1 indicates the inverse operation.

[0042] In the coarse registration stage, the position coordinates of the feature points in the previous grayscale image are used. , , The normalized position coordinates of the feature points in the camera coordinate system are calculated based on the pixel coordinate system and the camera's intrinsic parameter information. , , Represent the normalized position coordinates of the feature points in the camera coordinate system, is the normalized depth information,

[0043] ,

[0044] in and is the focal length of the camera, and is the optical center coordinate of the camera. Then the normalized coordinate in the camera coordinate system is Convert to world coordinate system:

[0045] ,

[0046] is the estimated depth of the feature point, and are the rotation and translation matrices in the transformation matrix from IMU to camera, respectively. and are the pose matrix and position vector of the reference frame (the frame where the feature point is first observed).

[0047] Afterwards, transform the feature points from the world coordinate system back to the camera coordinate system of the next frame:

[0048] ,

[0049] and They are The transpose and translation parts of the rotation part, is a 4×4 matrix, which contains a 3×3 rotation matrix and a translation vector of size 3×1 . is the position of the predicted feature point in the camera coordinate system in the next frame, Represents the coordinates in the normalized camera coordinate system. Then, the feature point is transformed to the pixel plane in the opposite way to the previous one. So far, the position of the feature point in the previous frame in the next frame has been estimated, and the rough registration process has been completed.

[0050] For the feature points in the previous frame and the feature points of the current frame obtained by the rough registration method, the hybrid descriptors are extracted respectively, and the corresponding matching scheme is designed. The specific process is as follows: Set the key point set to ,in is the number of key points. For each key point ,First, use the ORB algorithm to calculate the descriptor of each key point on the grayscale image , the length of the descriptor vector is :

[0051] ,

[0052] For each key point , calculate its local normal vector in the depth image D, which can be obtained by calculating the depth gradient. The normal vector is :

[0053] ,

[0054] in, and The depth map is and Direction gradient.

[0055] Based on the above two information, a new hybrid descriptor is constructed , the length is , the specific form is:

[0056] ,

[0057] In order to achieve successful matching of descriptors, the weighted Euclidean distance between two descriptors is used to measure and The distance between them. The distance measurement formula is defined as:

[0058] ,

[0059] in and It is Descriptor With Descriptor The depth value of is the weight parameter of the depth camera, which changes with the depth.

[0060] When two descriptors are matched, the average weight of their depth values ​​is used:

[0061] ,

[0062] The final distance measurement formula for descriptor matching is:

[0063] ,

[0064] Furthermore, in step 3, for scenes with a large depth of field such as corridors, due to the limited range of the depth camera, the range in which it can provide effective depth information is smaller than that in scenes such as offices. Therefore, the depth information for extracting corner points can only be provided by triangulation of the stereo camera pair. When the camera shakes, the feature points extracted by the left and right cameras are rarely matched successfully, so the depth information obtained is not reliable, and the final solved posture is very likely to diverge.

[0065] A. Shaking state detection;

[0066] Based on the IMU information, the current motion state is judged, and the variance between the accelerometer and gyroscope between frames is used to determine whether it has entered a shaking state. First, a sliding window is given When the amount of data in the sliding window meets the set threshold, the mean of the acceleration and angular velocity is calculated. , and variance , , only when the mean, variance and duration all meet the threshold, it is considered as a shaking state.

[0067] B. Feature elimination and re-extraction based on depth mask;

[0068] After detecting the shaking state, the current feature points are filtered and only the points with depth values ​​less than the range of the depth camera are retained. On this basis, the depth mask is set using the depth image information, that is, the range for extracting feature points from the grayscale image is divided, and feature extraction is performed in the area where the depth meets the range to ensure that the feature points have reliable depth information.

[0069] C. Gridding and uniform distribution;

[0070] Using the gridding method, the extracted feature points are evenly distributed, as shown below:

[0071] ,

[0072] ,

[0073] ,

[0074] The maximum number of feature points to be used The resulting grid size, and are the width and length of the cell respectively. Based on the above information, the extracted feature points are evenly distributed. and Represents the width and height of the image, and the specific coordinates of the feature points on the pixel plane are as follows:

[0075] ,

[0076] ,

[0077] Furthermore, in step 4, define is the transformation matrix from IMU to world coordinate system, is the transformation matrix from the camera to the IMU coordinate system, is the position of the feature point in the camera coordinate system, is the position of the feature point in the IMU coordinate system, is the position of the feature point in the world coordinate system, It is the inverse depth in world coordinates. The depth value corresponding to the feature point provided by the depth camera, The depth information solved by the stereo camera pair is calculated based on the above information. The current inverse depth residual is calculated as follows:

[0078] ,

[0079] Based on the residual, the Jacobian is calculated for the rotation and translation of the camera respectively. The Jacobian for translation is calculated as follows:

[0080] ,

[0081] In the formula is a unit vector ,

[0082] The Jacobian calculation for the rotational part is as follows:

[0083] ,

[0084] The Jacobian solution for the translation and rotation parts of the camera-to-IMU transformation matrix is ​​similar to the above, as shown below. The Jacobian solution for the translation part of the camera-to-IMU transformation matrix for the inverse depth residual is as follows:

[0085] ,

[0086] The Jacobian solution for the rotation part is as follows:

[0087] ,

[0088] Based on the above information, the corresponding residual matrix is ​​constructed for overall optimization.

[0089] In summary, compared to the requirements of existing visual inertial odometers for carrier motion constraints, the present invention can achieve stable positioning in complex dynamic scenes using a depth camera indoors, greatly improving the robustness of positioning. Compared to the existing visual inertial odometer solutions that integrate depth information, the present invention starts from the front and back ends of the algorithm, uses the fusion information of grayscale images and depth images, designs a hybrid descriptor and a corresponding dynamic matching solution, effectively improves the feature tracking effect in shaking scenes, and in order to further optimize the positioning effect in shaking scenes, the depth information is used to construct the corresponding residual term at the back end, and the positioning result is further optimized from the overall perspective.

[0090] On the other hand, the present invention provides a robust visual inertial odometer device in a dynamic scene, wherein each unit included in the device can implement each of the aforementioned steps, including:

[0091] An information acquisition unit, used to acquire grayscale images, depth images and IMU information and perform preprocessing to acquire depth information;

[0092] The hybrid descriptor matching unit is used to perform rough feature matching based on the depth information and the inter-frame IMU integral, and to construct the corresponding descriptor and matching function using the grayscale information and depth information of the feature points;

[0093] A feature extraction unit is used to detect the shaking state using the IMU information. When the shaking state is detected, the depth image is used to extract the area that meets the depth requirement in the grayscale image, and the feature points that do not meet the depth condition in the area that meets the depth requirement are removed, and the feature points are re-extracted, and the re-extracted feature points are evenly distributed according to the gridding method;

[0094] The back-end optimization unit is used to construct an inverse depth residual using the re-extracted feature points and the depth information obtained by the triangulation, and use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation and translation of the camera for obtaining the depth image and the grayscale image, and design an optimization function.

[0095] In a third aspect, the present invention provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; wherein, when the one or more programs are executed by the one or more processors, the one or more processors implement the aforementioned robust visual inertial odometer method in dynamic scenarios.

[0096] In a fourth aspect, the present invention provides a computer-readable storage medium having executable instructions stored thereon, which, when executed by a processor, enables the processor to implement the aforementioned robust visual inertial odometer method in dynamic scenarios.

[0097] The specific embodiments described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A robust visual inertial odometry method in dynamic scenes, characterized in that: The method comprises: Step 1: Obtain grayscale image, depth image and IMU information and perform preprocessing to obtain depth information; Step 2: Perform rough feature matching based on depth information and inter-frame IMU integration, and use the grayscale information and depth information of feature points to construct corresponding descriptors and matching functions; Step 3, using IMU information to detect the shaking state. When the shaking state is detected, the depth image is used to extract the area that meets the depth requirement in the grayscale image, and the feature points that do not meet the depth condition in the area that meets the depth requirement are removed, and the feature points are re-extracted, and the re-extracted feature points are evenly distributed according to the gridding method; Step 4: construct an inverse depth residual using the re-extracted feature points and the depth information obtained by triangulation, and use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation and translation of the camera that obtains the depth image and the grayscale image, and design an optimization function.

2. The robust visual inertial odometry method in dynamic scenes according to claim 1, characterized in that: The step 1 includes: acquiring a grayscale image, a depth image and IMU information, registering the grayscale image with the depth image, and aligning the image information carried by the grayscale image and the depth image with the IMU information in time.

3. The robust visual inertial odometry method in a dynamic scene according to claim 1, characterized in that: The step 2 comprises: Step 2.1, extract the corner points of the grayscale image based on the Shi-tomasi corner point algorithm, use the stereo camera to triangulate the grayscale image to obtain depth information, and combine it with the depth information carried by the depth image to optimize the depth estimation, and obtain the corresponding depth of the corner point, that is, the corresponding depth of the feature point; Step 2.2, based on the corresponding depth of the feature point and the IMU integration information between frames, the position of the feature point in the previous frame in the next frame is estimated; Step 2.3: For the feature point positions in the previous frame and the estimated feature point positions in the next frame, use their respective depth images and grayscale images to construct corresponding hybrid descriptors for the corner points, and dynamically adjust the matching weights according to the corresponding depths of the feature points to perform matching, thereby completing the optimization of feature extraction and matching in shaking scenes.

4. The robust visual inertial odometry method in a dynamic scene according to claim 1, characterized in that: The step 3 comprises: Step 3.1: Determine whether the vehicle is in a shaking state based on the input IMU information; Step 3.2: After detecting that the image is in a shaking state, filter the current feature points to retain only valid feature points whose depth values ​​are less than the range of the depth camera, and use the depth mask to re-extract feature points from the grayscale image in the area that meets the range of the depth camera. Step 3.3: Utilize a gridding scheme for the re-extracted feature points, and dynamically set the width and length of the cell based on the number of re-extracted feature points and the size of the area of ​​the grayscale image that conforms to the range of the depth camera.

5. The robust visual inertial odometry method in dynamic scenes according to claim 1, characterized in that: The step 4 comprises: Step 4.1, constructing an inverse depth residual based on the depth information provided by the depth image corresponding to the position of the re-extracted feature point and the depth information obtained by the triangulation; Step 4.2: Utilize the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation matrix and translation vector of the camera that obtains the depth image and the grayscale image, and design a corresponding optimization function at the back end.

6. The robust visual inertial odometry method in a dynamic scene according to claim 4, characterized in that: The step 3.1 includes, given a corresponding sliding window, when the amount of data in the sliding window meets the set threshold, calculating the mean and variance of the acceleration and angular velocity respectively, and when the duration of the mean and variance meeting the threshold meets the preset value, it is regarded as a shaking state.

7. The robust visual inertial odometer method in a dynamic scene according to claim 4, characterized in that: The step 3.3 includes using a gridding method to evenly distribute the extracted feature points, as shown below: ， ， ， The maximum number of feature points to be used The resulting grid size, and are the width and length of the cell respectively, and Indicates the width and height of the grayscale image.

8. A robust visual inertial odometer device in a dynamic scene, characterized in that: include: An information acquisition unit, used to acquire grayscale images, depth images and IMU information and perform preprocessing to acquire depth information; The hybrid descriptor matching unit is used to perform rough feature matching based on the depth information and the inter-frame IMU integral, and to construct the corresponding descriptor and matching function using the grayscale information and depth information of the feature points; A feature extraction unit is used to detect the shaking state using the IMU information. When the shaking state is detected, the depth image is used to extract the area that meets the depth requirement in the grayscale image, and the feature points that do not meet the depth condition in the area that meets the depth requirement are removed, and the feature points are re-extracted, and the re-extracted feature points are evenly distributed according to the gridding method; The back-end optimization unit is used to construct an inverse depth residual using the re-extracted feature points and the depth information obtained by triangulation, and use the inverse depth residual to obtain the corresponding Jacobian matrix for the rotation and translation of the camera that obtains the depth image and the grayscale image, and design an optimization function.

9. An electronic device, characterized in that: include: one or more processors; A memory for storing one or more programs; Wherein, when one or more programs are executed by the one or more processors, the one or more processors implement the robust visual inertial odometer method in a dynamic scene as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: Executable instructions are stored thereon, which, when executed by a processor, enable the processor to implement the robust visual inertial odometer method in a dynamic scene as described in any one of claims 1-7.

Images