Dynamic visual-inertial slam system based on smooth flow

Abstract

The application provides a dynamic visual inertial SLAM system based on smooth optical flow, generates a key frame Fr by performing feature extraction and feature matching on an image; calculates an optical flow vector of a feature point after performing smoothing processing on the key frame Fr, and obtains a feature dynamic index through an evaluation function; performs pre-integration processing on IMU data to obtain an IMU residual error, and jointly performs sliding window optimization on the feature dynamic index, a visual residual error and the IMU residual error to obtain a pose state estimation; performs closed loop detection and global BA optimization on the obtained pose to obtain an updated map, and realizes a positioning and mapping function. The application solves the problem of robustness and mapping accuracy decline of a current visual system in a dynamic environment.

Description

Technical Field

[0001] This application relates to the field of machine vision technology, specifically to a dynamic visual inertial SLAM system based on smooth optical flow. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) enables robots and other vehicles to autonomously locate, map, and navigate during movement. SLAM systems collect environmental information using sensors such as cameras and inertial measurement units (IMUs), and then perform localization and mapping based on the collected data. Current visual-inertial SLAM systems exhibit high robustness and mapping accuracy in static environments due to the fusion of visual and inertial information. However, they suffer from low dynamic feature recognition rates in dynamic environments, leading to a decrease in robustness and mapping accuracy.

[0003] Therefore, a new dynamic visual-inertial SLAM system based on smooth optical flow is needed. Summary of the Invention

[0004] In view of this, embodiments of this specification provide a dynamic visual inertial SLAM system based on smooth optical flow.

[0005] The embodiments in this specification provide the following technical solutions:

[0006] This specification provides an embodiment of a dynamic visual inertial system based on smooth optical flow, comprising the following steps:

[0007] Step (1) Perform feature extraction and feature matching on the image to generate keyframes Fr;

[0008] Step (2) After smoothing the keyframe Fr, calculate the optical flow vector of the feature points and obtain the feature dynamic index through the evaluation function;

[0009] Step (3) pre-integrates the IMU data to obtain the IMU residual, and performs joint sliding window optimization on the feature dynamic index, visual residual and IMU residual to obtain the pose state estimate;

[0010] Step (4) performs closed-loop detection and global BA optimization on the obtained pose to obtain an updated map, so as to realize the localization and mapping function.

[0011] Compared with the prior art, the beneficial effects that at least one technical solution adopted in the embodiments of this specification can achieve include at least:

[0012] The dynamic visual inertial system based on smooth optical flow in the embodiments of this specification effectively solves the problems of decreased system robustness and poor positioning and mapping accuracy in the current technology under dynamic environments. Attached Figure Description

[0013] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0014] Figure 1 This is a flowchart illustrating a dynamic visual-inertial system based on smooth optical flow according to this application.

[0015] Figure 2 This is a system block diagram of a dynamic visual inertial system based on smooth optical flow, as described in this application. Detailed Implementation

[0016] The embodiments of this application will now be described in detail with reference to the accompanying drawings.

[0017] The following specific examples illustrate the implementation of this application. Those skilled in the art can easily understand other advantages and effects of this application from the content disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. This application can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of this application. It should be noted that, in the absence of conflict, the following embodiments and features in the embodiments can be combined with each other. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0018] It should be noted that various aspects of embodiments within the scope of the appended claims are described below. It will be apparent that the aspects described herein can be embodied in a wide variety of forms, and any particular structure and / or function described herein is merely illustrative. Based on this application, those skilled in the art will understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number and aspects set forth herein can be used to implement the device and / or practice the method. Additionally, this device and / or method can be implemented using structures and / or functionalities other than one or more of the aspects set forth herein.

[0019] It should also be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of this application. The drawings only show the components related to this application and are not drawn according to the actual number, shape and size of the components in the actual implementation. In the actual implementation, the form, quantity and proportion of each component can be arbitrarily changed, and the layout of the components may also be more complex.

[0020] Additionally, specific details are provided in the following description to facilitate a thorough understanding of the examples. However, those skilled in the art will understand that practice can be carried out without these specific details.

[0021] In existing technologies, SLAM systems collect environmental information through sensors such as cameras and inertial measurement units (IMUs) and perform localization and mapping based on the collected data. Current visual-inertial SLAM systems, based on the fusion of visual and inertial information, have high robustness and mapping accuracy in static environments. However, as the environment changes, i.e., in dynamic environments, the dynamic feature recognition rate is relatively low.

[0022] In light of this, the inventors discovered that joint sliding window optimization can be performed by incorporating evaluation functions such as dynamic feature indices into the backend joint optimization. Unlike traditional vision-inertial joint optimization, the introduction of dynamic feature indices further improves the optimization effect. This addresses the current problems of reduced robustness and mapping accuracy of vision systems in dynamic environments.

[0023] Based on this, the embodiments of this specification propose a new dynamic visual inertial system based on smooth optical flow, including the following steps: Step (1) extracting and matching features from the image to generate a key frame Fr; Step (2) smoothing the key frame Fr and calculating the optical flow vector of the feature points, and obtaining the feature dynamic index through the evaluation function; Step (3) pre-integrating the IMU data to obtain the IMU residual, and performing joint sliding window optimization on the feature dynamic index, visual residual and IMU residual to obtain the pose state estimate; Step (4) performing loop closure detection and global BA optimization on the obtained pose to obtain the updated map, thereby realizing the localization and mapping function.

[0024] The technical solutions provided by the various embodiments of this application are described below with reference to the accompanying drawings.

[0025] This embodiment discloses a dynamic visual inertial system based on smooth optical flow, the overall process of which is shown in the attached figure. Figure 1 As shown in Figure 2, the steps include:

[0026] Step 1: Extract and match features from the images captured by the robot's onboard vision sensors to generate keyframes Fr;

[0027] Feature extraction and feature matching specifically include the following steps:

[0028] Extract FAST keypoints, determine the orientation of each keypoint using the gray-scale centroid method, calculate the SteeredBRIEF descriptor to complete feature extraction, and then perform feature matching to generate keyframes Fr.

[0029] Step 11: The steps for extracting FAST key points are as follows:

[0030] Select a pixel k in the image, and denote its grayscale value as I. k A threshold T is set to measure the difference between the gray values ​​of two pixels. Here, based on engineering experience, it is set to I. k 20%. Take a circle with radius 3 centered at pixel k, containing 16 pixels. If N consecutive pixels satisfy:

[0031] I>I P +T or I p -T(1.1)

[0032] Pixel k can be defined as the FAST key point, and N can be set to 9 based on engineering experience to speed up the detection process.

[0033] Step 12: The steps for determining the orientation of feature points using the gray-scale centroid method are as follows:

[0034] Define a circular region of diameter R in the image, containing the keypoint k from step 11, and set its center to O. Then calculate the gray-level centroid B. Define the moments of this circular region as follows:

[0035]

[0036] p and q are either 0 or 1, I(x,y) is the gray value of the pixel at coordinates (x,y), and m pq Let be the moments of the image.

[0037] According to Equation 1.2, the sum of the image moments and pixel gray values ​​along the x and y coordinate axes of the circular region is:

[0038]

[0039]

[0040]

[0041] According to Equation 1.2, the image grayscale centroid B is obtained as follows:

[0042]

[0043] Then the feature point direction of key point k is the direction vector from the center O of the circle to the gray centroid B. ​

[0044] Step 13: The calculation steps for the Steered BRIEF descriptor are as follows:

[0045] After determining the key point k according to step 11, take an image window of a certain size centered on k, randomly select point pairs a and b within the window, compare the pixel values ​​of the two points, and assign binary values ​​to the point pairs according to the following rules:

[0046] If the pixel value of point a is less than that of point b, the value is 1; otherwise, the value is 0. After randomly selecting N pairs of random points, an N-dimensional Steered BRIEF descriptor is obtained (N = 128).

[0047] Step 2: After smoothing the keyframe Fr, calculate the optical flow vector of the feature points, and obtain the feature dynamic index Q through the evaluation function;

[0048] Smoothing and solving for characteristic dynamic indices include the following steps:

[0049] After obtaining the keyframe Fr, the image is Gaussian smoothed to remove noise and some high-frequency components, resulting in smoothed pixel values ​​I(u,v). Then, the optical flow vector is calculated using an optical flow algorithm to obtain the vector magnitude r. The optical flow vector is then processed by an evaluation function to obtain the feature dynamic threshold Q as a dynamic index.

[0050] Step 21: The Gaussian smoothing process is as follows:

[0051]

[0052] In Equation 1.5, f(u+m,v+n) is the keyframe input image, h(m,n) is the convolution kernel, and after summation, the smoothed output pixel value I(u,v) is obtained. Since Gaussian smoothing can ensure the same smoothness in all directions, it can avoid directional shift in subsequent optical flow calculations.

[0053] Step 22: Optical flow calculation method:

[0054] Let feature point k in keyframe Fr have coordinates (x, y) in the previous keyframe, and let its grayscale value at time t be I(x, y, t). Then, its grayscale value at time t+dt in the current frame is I(x+dx, y+dy, t+dt). Based on the assumption of grayscale invariance, we have:

[0055] I(x+dx,y+dy.t+dt)=I(x,y,t)(1.6)

[0056] A Taylor expansion of equation 1.6 yields:

[0057]

[0058]

[0059]

[0060] Assuming that the pixels have the same motion, the optical flow vector can be solved according to Equation 1.7. Writing it in polar coordinates, we can obtain the direction θ of the optical flow vector.

[0061] Step 23: Design the evaluation function:

[0062]

[0063] The denominator is the mean of the optical flow vector direction tangent values ​​traversing the image frame. The dynamic index Q is used to determine the dynamic degree of the feature point and is added to the backend joint optimization.

[0064] Step 3: Perform pre-integration processing on the data collected by the IMU sensor on the robot to obtain the IMU residual. Perform joint sliding window optimization on the feature dynamic index, visual residual and IMU residual to obtain the robot's pose state estimate.

[0065] The joint optimization process includes the following steps:

[0066] Pre-integration of the IMU data yields the IMU measurement residuals, including accelerometer settings and gyroscope biases, resulting in the IMU measurement residual r. b Compared with the feature dynamic index Q and visual observation residual r obtained in the previous steps c Construct a tightly coupled optimization model for joint sliding window optimization:

[0067] Step 31: The objective function for joint optimization is as follows:

[0068]

[0069] χ represents the state vectors in the sliding window. For camera extrinsic parameters, ρ c The Cauchyro bar kernel function for removing outliers, ρ h The Huber kernel function is designed to reduce the matching error rate. Unlike traditional vision-inertial joint optimization, a dynamic feature exponent is introduced to further improve the optimization effect.

[0070] Step 4: Perform closed-loop detection and global BA optimization on the obtained robot pose to obtain an updated map, thus realizing the localization and mapping function;

[0071] Loop closure detection and global business intelligence (BA) optimization include the following:

[0072] Perform loop closure detection on the keyframes corresponding to the pose obtained in step 3 to determine whether they are the same map point. If the detection is successful, global BA optimization is performed to obtain the updated map to realize the localization and mapping function.

[0073] Step 41: Loop Closed-Loop Detection

[0074] After the pose is optimized by step (3), the corresponding candidate keyframe and the keyframes connected before and after it form a candidate group. Traverse the candidate group detected in the previous loop closure. If there is a common frame in the two groups, determine their continuity. Increment the length of the candidate group this time by 1 and update it to the loop closure detection group chain.

[0075] Step 42: Global BA Optimization:

[0076] After completing the loop closure detection, global BA is executed to optimize all keyframe poses and map points in the map. The keyframe poses in the global map are traversed and updated. Each map point is traversed and its position is updated with the updated keyframe poses.

[0077] It should be noted that the terms "first," "second," "third," "fourth," etc. (if present) in the specification, claims, and accompanying drawings of this invention 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 the invention described herein can be implemented in orders other than those illustrated or described herein.

[0078] The same or similar parts between the various embodiments in this specification can be referred to interchangeably. Each embodiment focuses on describing the differences from other embodiments. In particular, the product embodiments described later are relatively simple in description since they correspond to the methods, and relevant parts can be referred to the descriptions in the system embodiments.

[0079] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A dynamic visual-inertial system based on smooth optical flow, characterized in that... Includes the following steps: Step (1) Perform feature extraction and feature matching on the image to generate keyframes Fr; Step (2) After smoothing the keyframe Fr, calculate the optical flow vector of the feature points and obtain the feature dynamic index through the evaluation function; Step 23: Design the evaluation function: （1.8） The denominator is the mean of the optical flow vector direction tangent values ​​traversing the image frame, and the dynamic index Q is obtained to determine the dynamic degree of the feature point, which is then added to the backend joint optimization. Step (3) pre-integrates the IMU data to obtain the IMU residual, and performs joint sliding window optimization on the feature dynamic index, visual residual and IMU residual to obtain the pose state estimate; Step (3), step 31: The objective function of the joint optimization model is as follows: (1.9) Let be the state vectors in the sliding window. For camera external parameters, The Cauchy bar kernel function is used to remove outliers. Huber kernel function to reduce matching error rate The residuals are measured by the IMU, and Q is the characteristic dynamic index. For visual observation residuals; Step (4) performs closed-loop detection and global BA optimization on the obtained pose to obtain an updated map, so as to realize the localization and mapping function.

2. The dynamic visual-inertial system based on smooth optical flow according to claim 1, characterized in that: Step (1) specifically includes the following steps: extract FAST key points, obtain the direction of the feature point according to the gray centroid method, calculate the Steered BRIEF descriptor to complete feature extraction, and then perform feature matching to generate keyframe Fr.

3. The dynamic visual-inertial system based on smooth optical flow according to claim 2, characterized in that: Step 11 in step (1): The steps for extracting FAST key points are as follows: Select a pixel k in the image and record its grayscale value as k. A threshold T is set to measure the difference between the gray values ​​of two pixels. Here, it is set to a threshold based on engineering experience. 20%; Take a circle with radius 3 centered at pixel k, containing 16 pixels. If there are N consecutive points among the pixels that satisfy: （1.1） Pixel k is defined as the FAST key point, and N is set to 9 based on engineering experience to speed up the detection process.

4. The dynamic visual-inertial system based on smooth optical flow according to claim 2, characterized in that: Step 12 in step (1): The steps for determining the orientation of feature points based on the gray-scale centroid method are as follows: Delineate a circular region of diameter R in the image, containing the keypoint k from step 11, and set its center to O. Then calculate the grayscale centroid B, and define the moments of this circular region as follows: （1.2） Where p and q are either 0 or 1. Coordinates are The grayscale value of the pixel, The moments of the image; According to Equation 1.2, the sum of the image moments and pixel gray values ​​along the x and y coordinate axes of the circular region is: （1.3） According to Equation 1.2, the image grayscale centroid B is obtained as follows: （1.4） Then the feature point direction of key point k is the direction vector from the center O of the circle to the gray centroid B. .

5. The dynamic visual-inertial system based on smooth optical flow according to claim 2, characterized in that: Step 13 in step (1): The calculation steps for the Steered BRIEF descriptor are as follows: After determining the key point k according to step 11, take an image window of a preset size centered on k, randomly select point pairs a and b within the window, compare the pixel values ​​of the two points, and assign binary values ​​to the point pairs according to the following rules: If the pixel value of point a is less than that of point b, the value is 1; otherwise, the value is 0. After randomly selecting N pairs of random points, an N-dimensional Steered BRIEF descriptor is obtained (N=128).

6. The dynamic visual-inertial system based on smooth optical flow according to claim 1, characterized in that: Step (2) specifically involves performing Gaussian smoothing on the image after obtaining the keyframe Fr to remove noise and some high-frequency components, thus obtaining smoothed pixel values. Then, the optical flow vector is calculated using the optical flow algorithm to obtain the vector size r. The optical flow vector is then processed by the evaluation function to obtain the feature dynamic threshold Q as a dynamic index. Step 21, the Gaussian smoothing process, is as follows: （1.5） In Equation 1.5 The image input for the keyframe. The convolution kernel is used to sum the pixel values ​​to obtain a smooth output. .

7. The dynamic visual-inertial system based on smooth optical flow according to claim 6, characterized in that: Step (2), step 22: Optical flow calculation method: Let feature point k in keyframe Fr have coordinates (x, y) in the previous keyframe, and let its grayscale value at time t be I(x, y, t). Then, its grayscale value at time t+dt in the current frame is I(x+dx, y+dy, t+dt). Based on the assumption that grayscale remains unchanged, we have: （1.6） A Taylor expansion of equation 1.6 yields: （1.7） Assuming that the pixels have the same motion, the optical flow vector can be solved according to Equation 1.

7. ; Writing it in polar coordinates gives the direction of the optical flow vector. .

8. The dynamic visual-inertial system based on smooth optical flow according to claim 6, characterized in that: Step (3) specifically involves: Pre-integration of the IMU data yields the IMU measurement residuals, including accelerometer settings and gyroscope biases. Compared with the feature dynamic index Q and visual observation residual obtained in the previous steps Construct a tightly coupled optimization model for joint sliding window optimization.

9. The dynamic visual-inertial system based on smooth optical flow according to claim 1, characterized in that: Step (4) is as follows: The pose-corresponding keyframes obtained in step (3) are subjected to loop closure detection to determine whether they are the same map point. After successful detection, global BA optimization is performed to obtain the updated map to realize the localization and mapping function. Step 41: Loop Closed-Loop Detection: After the pose is optimized by step (3), the corresponding candidate keyframe and the keyframes connected before and after it form a candidate group. Traverse the candidate group detected in the previous loop closure. If there is a common frame in the two groups, determine their continuity. Increment the length of the candidate group this time by 1 and update it to the loop closure detection group chain. Step 42: Global Business Optimization: After completing the loop closure detection, global BA is executed to optimize the poses of all keyframes and map points in the map. The keyframe poses in the global map are traversed and updated. Each map point is traversed and its position is updated with the updated keyframe poses to achieve the localization and mapping function.

Images