Visual inertia fusion positioning method and device and binocular visual inertia system

Abstract

The embodiment of the invention provides a visual inertia fusion positioning method, a visual inertia fusion positioning device and a binocular visual inertia system. The method comprises the following steps: calibrating an initialized object based on acquired binocular vision sensor data and inertial sensor data to generate a vision and inertia combined calibration result; performing feature point extraction and object tracking according to a visual inertia joint calibration result to generate an optical flow tracking result; performing pose nonlinear optimization based on an optical flow tracking result and the obtained integral residual term to generate a camera pose and a road sign point position; and carrying out information matrix calculation on the camera pose and the road sign point position so as to carry out visual inertia fusion positioning. According to the technical scheme provided by the embodiment of the invention, visual inertial fusion positioning is carried out through joint calibration, feature point calibration, optical flow tracking, nonlinear optimization and information matrix calculation of binocular visual sensor data and inertial sensor data, so that the precision, real-time performance, accuracy and high efficiency of navigation positioning are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning, and in particular, to a visual-inertial fusion positioning method, apparatus, and binocular visual-inertial system. Background Art

[0002] A mobile robot needs simultaneous localization and mapping (SLAM) technology to complete autonomous navigation and movement in an unknown environment, establish a model of the environment during movement, and simultaneously estimate its own motion state. According to the different sensors mounted on the mobile robot body, SLAM can be divided into laser SLAM and visual SLAM. Since visual SLAM has low requirements for sensors and is easy to implement while ensuring a certain accuracy, visual SLAM technology has gradually been popularized in many application scenarios. However, the physical characteristics and data output form of the visual sensor itself determine that the illumination and texture of the real scene significantly affect the accuracy of navigation and positioning, and its processing of image data to estimate its own motion state results in high computing power requirements, making it difficult to ensure the real-time, accurate, and efficient positioning. Summary of the Invention

[0003] In view of this, embodiments of the present invention provide a visual-inertial fusion positioning method, apparatus, and binocular visual-inertial system to improve the accuracy, real-time, accuracy, and efficiency of navigation and positioning.

[0004] On the one hand, embodiments of the present invention provide a visual-inertial fusion positioning method, including: Generating a visual-inertial joint calibration result based on the acquired binocular visual sensor data and inertial sensor data for calibration initialization objects; Performing feature point extraction and object tracking based on the visual-inertial joint calibration result to generate an optical flow tracking result; Performing pose non-linear optimization based on the optical flow tracking result and the acquired integral residual term to generate the camera pose and the landmark position; Calculating the information matrix for the camera pose and the landmark position for visual-inertial fusion positioning.

[0005] Optionally, the generating a visual-inertial joint calibration result based on the binocular visual sensor data and inertial sensor data for calibration initialization objects includes: Calculating the binocular visual sensor data and the inertial sensor data based on the PNP algorithm to generate the first camera pose parameter; Calculating the feature depth based on the camera pose parameter by the binocular triangulation algorithm to generate the second camera pose parameter; Calculate the error between the first camera pose parameter and the second camera pose parameter to generate a pose error remainder; Perform bias correction calculation on the inertial sensor data to generate corrected data; Calculate the error between the corrected data and the inertial sensor data to generate an inertial error remainder; Calculate the pose error remainder and the inertial error remainder based on the local bundle adjustment method to generate the visual-inertial joint calibration result.

[0006] Optionally, the extracting feature points and object tracking according to the visual-inertial joint calibration result to generate an optical flow tracking result includes: Accelerate the SURF feature point extraction of the visual-inertial joint calibration result based on CUDA to generate an extraction result; Perform optical flow tracking calculation on the extraction result to generate an optical flow tracking result.

[0007] Optionally, the accelerating the SURF feature point extraction of the visual-inertial joint calibration result based on CUDA to generate an extraction result includes: Perform Gaussian blur processing on the binocular vision sensor data to generate a preprocessed image; Load the preprocessed image into the GPU memory and parallelly construct a multi-scale image pyramid; Calculate the main direction of the key points according to the extreme points found in the multi-scale image pyramid; Obtain multiple image feature points in the binocular vision sensor data; Perform integral and summation calculations on each image feature point within its surrounding window based on the main direction of the key points to generate a feature descriptor; Screen each image feature point according to the feature descriptor to generate an extraction result.

[0008] Optionally, the integral residual term includes a pose error remainder and / or an inertial error remainder.

[0009] Optionally, the optical flow tracking result includes the reprojection error of the image feature points obtained by optical flow tracking. The generating the camera pose and the landmark position by performing pose non-linear optimization based on the optical flow tracking result and the obtained integral residual term includes: Perform non-linear optimization on the reprojection error and the integral residual term, construct a least squares problem during the non-linear optimization process, and minimize the reprojection error to generate the camera pose and the landmark position.

[0010] Optionally, the calculating the information matrix of the camera pose and the landmark position for visual-inertial fusion positioning includes: Based on the error function and the Jacobian matrix, an information matrix is constructed for the camera pose and the landmark position to calculate the information matrix and generate the pose parameters and landmark parameters. Based on CUDA acceleration, iterative calculations of Schur complement and Cholesky decomposition are performed on the pose parameters and the landmark parameters to generate pose optimization parameters and landmark optimization parameters for visual-inertial fusion positioning.

[0011] On the other hand, an embodiment of the present invention provides a visual-inertial fusion positioning device, including: A first generation module, configured to generate a visual-inertial joint calibration result for a calibration initialization object based on the acquired binocular vision sensor data and inertial sensor data; A second generation module, configured to perform feature point extraction and object tracking according to the visual-inertial joint calibration result to generate an optical flow tracking result; A third generation module, configured to perform pose non-linear optimization based on the optical flow tracking result and the acquired integration residual term to generate a camera pose and a landmark position; A positioning module, configured to perform information matrix calculation on the camera pose and the landmark position for visual-inertial fusion positioning.

[0012] On the other hand, an embodiment of the present invention provides a storage medium, where the storage medium includes a stored program, and when the program runs, it controls the device where the storage medium is located to execute the above-mentioned visual-inertial fusion positioning method.

[0013] On the other hand, an embodiment of the present invention provides a binocular vision-inertial system, including a memory and a processor, where the memory is used to store information including program instructions, and the processor is used to control the execution of the program instructions. When the program instructions are loaded and executed by the processor, the steps of the above-mentioned visual-inertial fusion positioning method are implemented.

[0014] In the technical solution provided by the embodiment of the present invention, a visual-inertial joint calibration result is generated for a calibration initialization object based on the acquired binocular vision sensor data and inertial sensor data; feature point extraction and object tracking are performed according to the visual-inertial joint calibration result to generate an optical flow tracking result; pose non-linear optimization is performed based on the optical flow tracking result and the acquired integration residual term to generate a camera pose and a landmark position; information matrix calculation is performed on the camera pose and the landmark position for visual-inertial fusion positioning. In the technical solution provided by the embodiment of the present invention, visual-inertial fusion positioning is performed through joint calibration, feature point calibration, optical flow tracking, non-linear optimization, and information matrix calculation of binocular vision sensor data and inertial sensor data, improving the accuracy, real-time performance, accuracy, and efficiency of navigation positioning. Description of the Drawings

[0015] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for the embodiments will be briefly introduced below. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other accompanying drawings can be obtained based on these drawings.

[0016] Figure 1 It is a flowchart of a visual-inertial fusion positioning method provided by an embodiment of the present invention; Figure 2 It is a flowchart of generating a visual-inertial joint calibration result provided by an embodiment of the present invention; Figure 3 It is a flowchart of generating an optical flow tracking result provided by an embodiment of the present invention; Figure 4 It is a flowchart of calculating the information matrix for the camera pose and the landmark position for visual-inertial fusion positioning provided by an embodiment of the present invention; Figure 5 It is a schematic diagram of a visual-inertial fusion positioning device provided by an embodiment of the present invention; Figure 6 It is a schematic diagram of a binocular visual-inertial system provided by an embodiment of the present invention. Detailed implementation manners

[0017] To better understand the technical solutions of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0018] It should be clear that the described embodiments are only some embodiments of the present invention, not all of them. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0019] The terms used in the embodiments of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The singular forms "a", "the" and "said" used in the embodiments of the present invention and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise.

[0020] It should be understood that the term " / and" used herein is only a description of the association relationship of associated objects, indicating that three relationships may exist. For example, a and / or b may represent: a exists alone, a and b exist simultaneously, and b exists alone. In addition, the character " / " herein generally represents an "or" relationship between the associated objects before and after.

[0021] Inertial sensors use gyroscopes and accelerometers to calculate displacement, velocity, and acceleration, which has good reliability. However, when used alone, the positioning accuracy drifts severely, and it can provide effective positioning assistance for binocular vision sensors. Compared with monocular vision, binocular vision sensors have stronger depth perception ability, wider viewing angle coverage, and stronger robustness. However, the system's task volume for visual data processing also increases significantly. Under the limited computing power resources of the robot, the key to achieving the efficient and accurate operation of visual SLAM technology is to accelerate the front-end feature point extraction and tracking process and the back-end optimization process of the SLAM framework. Ensuring the robustness of feature points can better resist the interference of noise to data, which is the basis of most feature point extraction methods. Traditional binocular vision inertial positioning systems extract rotation-robust feature points and mainly use the central processing unit for calculation. Their parallel computing ability for feature point tracking and motion state estimation is weak, and the occupation of system computing resources is obvious. Therefore, it is difficult to balance accuracy and efficiency. The feature points extracted by existing binocular vision inertial systems generally have poor robustness, high occupancy rate of computing resources, and are prone to problems of low accuracy and efficiency, unable to ensure the real-time performance and accuracy of robot positioning. Moreover, robots often use embedded devices, and compared with personal computers or industrial control computers, their power consumption, volume, and weight are greatly reduced, making it difficult to meet the real-time operation requirements of visual inertial positioning systems.

[0022] In view of the above problems, an embodiment of the present invention proposes a visual inertial fusion positioning method based on the acceleration of the Compute Unified Device Architecture (CUDA for short). It can extract Speeded Up Robust Features (SURF for short) to ensure the robustness of feature points against rotation, scale, and illumination changes, and use CUDA to call the Graphics Processing Unit (GPU for short) to parallelize the calculation of feature point extraction and motion state estimation, achieving a good balance between accuracy and efficiency.

[0023] Figure 1 As shown in the flowchart of a visual inertial fusion positioning method provided by an embodiment of the present invention, Figure 1 as follows, the method includes: Step 102, generate a visual inertial joint calibration result based on the obtained binocular vision sensor data and inertial sensor data for calibration initialization of the object.

[0024] In the embodiments of the present invention, each step is executed by a binocular vision inertial system, which includes a binocular vision sensor and an inertial sensor (Inertial Measurement Unit, abbreviated as IMU). Among them, binocular vision sensor data can be obtained through the binocular vision sensor, and the binocular vision sensor data includes image data or video data. Inertial sensor data is obtained through the inertial sensor, and the inertial sensor data includes physical quantities such as the motion state, angle change, and acceleration of an object.

[0025] In the embodiments of the present invention, first, the binocular vision sensor and the inertial sensor are jointly calibrated and initialized to match landmark feature points. The Perspective-n-Point (PnP) algorithm is used to solve the pose of the current image frame (the first camera pose parameter), and the feature depth is directly obtained by binocular triangulation to generate the second camera pose parameter. Subsequently, the bias of the inertial sensor is corrected, and the pose error residuals of all image frames within the sliding window and the inertial error residuals of the inertial sensor are optimized by the local bundle adjustment method to obtain the visual-inertial joint calibration result.

[0026] Specifically, Figure 2 is a flowchart for generating a visual-inertial joint calibration result provided by an embodiment of the present invention. As Figure 2 shown, step 102 includes: Step 1022: Calculate the binocular vision sensor data and the inertial sensor data based on the PNP algorithm to generate the first camera pose parameter.

[0027] In the embodiments of the present invention, the binocular vision sensor data includes landmark feature points (Landmark FeaturePoints). Among them, landmark feature points refer to points with unique and recognizable features in the environment in computer vision and robot navigation. These points can be detected by the vision system and used for positioning and map construction. In a binocular vision inertial system, landmark feature points usually refer to those points that can be stably detected in consecutive video frames, and they are very useful for estimating the position and pose of the camera.

[0028] Among them, the landmark feature points can include one or any combination of corner points (Corner Points), edge points (Edge Points), textured points (Textured Points), salient regions (Salient Regions), or artificial landmarks (Artificial Landmarks).

[0029] Corner points are the places where two edges intersect in an image, usually having high contrast and clarity, such as the corners of buildings and windows.

[0030] Edge points are points on the edges in an image. These edges can be straight lines, curves, or other shapes, such as the edges of roads, the outlines of furniture, etc.

[0031] Texture points are points in regions with rich texture in an image, such as the texture of tree bark, the pattern of a brick wall, etc.

[0032] Salient regions are significant or prominent regions in an image, such as signboards, billboards, etc.

[0033] Artificial features are feature points set artificially, such as QR codes, specific marks or labels, which are usually used in Augmented Reality (AR) applications.

[0034] In a binocular vision inertial system, these road sign feature points are detected and tracked to help the system understand the movement of the camera in space and construct or update a map of the environment. By matching the positions of these feature points at different time points, the system can calculate the displacement and rotation of the camera, thus achieving precise positioning and navigation.

[0035] In the embodiments of the present invention, it is necessary to jointly calibrate the binocular vision sensor and the inertial sensor to ensure that the data of the two sensors can be mutually calibrated and fused. This step is to improve the measurement accuracy and reliability of the overall system. In the binocular vision system, road sign feature points are detected and matched through image processing techniques. These feature points are specific points in the environment, which are recognized and tracked in consecutive image frames to help estimate the pose of the camera.

[0036] In the embodiments of the present invention, the PnP algorithm is a commonly used algorithm in computer vision for estimating the pose of a camera from a set of 2D-3D point correspondences. In this step, the algorithm uses known 3D space points (obtained by matching road sign feature points) and their 2D projection points on the image to calculate the first camera pose parameters.

[0037] Step 1024: Calculate the feature depth based on the binocular triangulation algorithm for the camera pose parameters to generate the second camera pose parameters.

[0038] In the embodiments of the present invention, after obtaining the first camera pose parameters, the depth information (feature depth) of the feature points can be calculated by using the principle of the binocular vision system through the difference between the perspectives of the two cameras. This step directly obtains the three-dimensional depth of the feature points through the binocular triangulation algorithm.

[0039] In the embodiment of the present invention, the purposes of steps 1022 and 1024 are to calculate the pose of the current image frame using the PnP algorithm by combining the binocular vision sensor data and the inertial sensor data, and further obtain the depth information of the feature points by using the binocular triangulation method, so as to provide basic data for subsequent visual SLAM and positioning.

[0040] Step 1026: Calculate the error between the first camera pose parameter and the second camera pose parameter to generate a pose error remainder.

[0041] Step 1028: Perform bias correction calculation on the inertial sensor data to generate corrected data.

[0042] In the embodiment of the present invention, the bias correction calculation includes: bias identification, static state measurement, bias estimation, and bias application. Among them, bias identification: First, it is necessary to identify the biases of the inertial sensors (such as gyroscopes and accelerometers). These biases are non-zero values output by the inertial sensors in the static state, and they will affect the measurement accuracy. The biases may be caused by imperfect manufacturing of the inertial sensors or environmental factors (such as temperature changes). Static state measurement: To identify the biases, it is usually necessary to perform measurements when the inertial sensors are static. At this stage, theoretically, the output of the inertial sensors should be zero (for accelerometers) or a constant value (for gyroscopes, representing the earth's rotation rate). Bias estimation: The bias values can be estimated through the data collected in the static state. This usually involves calculating the average value of the measured values and regarding it as the bias. Bias application: Once the biases are estimated, these bias values can be subtracted in subsequent data processing to obtain more accurate inertial sensor readings, that is, corrected data.

[0043] In the embodiment of the present invention, in the preparation stage of this step: ensure that the IMU is calibrated in an environment with no movement or minimal movement to reduce external interference. Fix the IMU at a stable position to ensure that it does not move during the calibration process. In the data collection stage: let the IMU run for a period of time in a stationary state to collect data from the accelerometer and gyroscope. Record the output values in the stationary state, and these values should be close to zero (for the accelerometer) or close to the Earth's rotation rate (for the gyroscope). In the bias identification stage: analyze the data collected in the stationary state to identify the biases of the accelerometer and gyroscope. For the accelerometer, the bias is the output value at rest, which should theoretically be zero (excluding the acceleration due to gravity). For the gyroscope, the bias is the output value at rest, which should theoretically be zero (excluding the Earth's rotation rate). In the bias estimation stage: calculate the average value of the data in the stationary state, and this average value can be used as an estimated value of the bias. In the bias correction stage: in subsequent data processing, subtract the estimated bias value from the original inertial sensor readings. In the verification and adjustment stage: after calibration, conduct a verification test to ensure the effectiveness of the bias correction. If systematic errors still exist in the data after bias correction, it may be necessary to re - estimate and correct the bias.

[0044] Step 1030: Calculate the error between the corrected data and the inertial sensor data to generate an inertial error remainder.

[0045] In this step, the inertial sensor data is the raw data read from inertial sensors (such as accelerometers and gyroscopes).

[0046] Step 1032: Calculate the pose error remainder and the inertial error remainder based on the local bundle adjustment method to generate a visual - inertial joint calibration result.

[0047] In the embodiment of the present invention, the bundle adjustment method is a commonly used SLAM back - end optimization technology. It optimizes the camera pose and landmark positions by minimizing the reprojection error of image feature points in all observed frames.

[0048] In the embodiment of the present invention, the pose error remainder and the inertial error remainder are part of the optimization problem, and the goal is to minimize the sum of the squares of all measurement errors.

[0049] In the embodiment of the present invention, the visual - inertial joint calibration result refers to a set of parameters and models obtained through a series of calibration and optimization processes by combining the data of binocular vision sensors and inertial sensors. These parameters and models can accurately describe the performance of the sensors and are used to improve the accuracy of subsequent positioning and map construction.

[0050] Step 104: Extract feature points and track objects according to the visual - inertial joint calibration result to generate an optical flow tracking result.

[0051] In the embodiments of the present invention, SURF feature points are obtained from the acquired binocular vision sensor data. The image data to be processed is loaded into the GPU memory, and a multi-scale image pyramid is constructed in parallel. Gaussian blur and difference calculations are performed through CUDA acceleration to obtain a Difference of Gaussians Pyramid (abbreviated as DoG). The multi-core parallelism of the GPU is used to find the extreme points in the DoG pyramid, calculate the main direction of the key points, and accelerate the process of extracting SURF feature points, reducing the system resource occupancy while improving the robustness of the feature points, providing a prerequisite guarantee for subsequent feature point tracking and pose solution steps. After obtaining the feature points in the image, for each feature point, integral and summation operations are performed within the window around it to calculate the gradient and other statistical information of the pixels within the window. According to the pixels within the window and their gradient information, a system of linear equations is solved to determine the displacement of the feature point in the next frame. This process can be accelerated through the parallel matrix operations and linear algebra library of CUDA. Among them, the result of CUDA-accelerated SURF feature point extraction can continue to perform optical flow tracking in the GPU memory, avoiding the large amount of time consumed by the transmission and access between the GPU memory and the host memory, and giving full play to the effect of CUDA acceleration.

[0052] Specifically, SURF feature point extraction based on CUDA-accelerated visual-inertial joint calibration results can be performed to generate extraction results; optical flow tracking calculations are performed on the extraction results to generate optical flow tracking results.

[0053] In the embodiments of the present invention, the SURF algorithm is a feature point detection and description algorithm, which is particularly suitable for real-time applications because it is optimized in terms of computational efficiency. The SURF algorithm quickly finds feature points by approximately calculating the determinant of the Hessian matrix and generates feature descriptors by constructing filters with specific shapes.

[0054] Specifically, Figure 3 is a flowchart for generating optical flow tracking results provided by an embodiment of the present invention. As Figure 3 shown, step 104 includes: Step 1042: Perform Gaussian blur processing on the binocular vision sensor data to generate a preprocessed image.

[0055] In the embodiments of the present invention, Gaussian blur processing is performed on the binocular vision sensor data to reduce the influence of noise. This step helps to improve the robustness of feature point detection.

[0056] Step 1044: Load the preprocessed image into the GPU memory and construct a multi-scale image pyramid in parallel.

[0057] In the embodiments of the present invention, a multi-scale image pyramid is constructed so that the algorithm can detect feature points at different scales, thereby enabling the detection of features of different sizes.

[0058] Step 1046: Calculate the main direction of the key point based on the extreme points found in the multi-scale image pyramid.

[0059] In the embodiments of the present invention, for each pixel point, the response value of its Hessian matrix is calculated. The Hessian matrix is a second-order derivative matrix used to describe the local curvature of the image brightness function. Local maximum and minimum points of the response value of the Hessian matrix are found in the multi-scale image pyramid, and these points are potential feature points. For each determined feature point, calculate the pixel gradient direction within its neighborhood to determine the main direction of this feature point, that is, the main direction of the key point. This step helps to make the feature descriptor invariant to rotational changes.

[0060] Specifically, the preprocessed image can be subjected to multi-scale processing to construct a Difference of Gaussian (DoG) pyramid in the scale space. The images on each scale layer are obtained by subtracting two Gaussian blurred images with different scales. Search for local extreme points in the DoG pyramid. These extreme points are potential key point positions, which are local maxima or minima within a 3x3x3 neighborhood (i.e., within a 3x3 region of the current scale and adjacent scales). For the found extreme points, further verify whether they are true extreme points. This usually involves quadratic discrimination of the DoG values around the extreme points. For each key point, calculate the gradient direction within its neighborhood. This usually involves calculating a gradient histogram within a small window (e.g., 16x16 pixels) around the key point. The gradient histogram is usually divided into several directions (such as 8 or 16 directions), and the gradient magnitudes in each direction are accumulated into the corresponding direction histogram. The direction corresponding to the maximum value in the histogram is selected as the main direction. If the difference between the maximum value and the second-largest value is not significant, multiple directions may be selected.

[0061] Assume G x and G y are the gradients of the preprocessed image in the x and y directions respectively, and θ is the direction of the gradient. Then the main direction α of the key point p can be calculated through the following steps: 1. Calculate the gradient magnitude and direction: .

[0062] 2. Quantize the gradient direction to the nearest histogram direction.

[0063] 3. Calculate the sum of the gradient magnitudes in each direction to obtain the gradient histogram.

[0064] 4. Find the direction corresponding to the maximum value in the gradient histogram as the main direction of the key point.

[0065] In the embodiments of the present invention, the main direction of the key point is used to construct a feature descriptor, so that the feature descriptor is invariant to rotational changes. When constructing the descriptor, the gradient direction of each interval will be adjusted relative to the main direction of the key point. During the feature matching process, the main direction of the key point is used to align the feature descriptor, so that even when the image rotates, the feature points can be accurately matched. In optical flow tracking, the main direction of the key point can help estimate the motion direction of the image feature points in consecutive frames, thereby more accurately calculating the displacement of the image feature points.

[0066] Step 1048: Obtain multiple image feature points from the binocular vision sensor data.

[0067] Step 1050: Integrate and sum each image feature point within its surrounding window based on the main direction of the key point to generate a feature descriptor.

[0068] In the embodiments of the present invention, a feature descriptor can be generated for each image feature point. The feature descriptor is a vector that contains the image information around the image feature point. The feature descriptor of SURF usually consists of gradient histograms of multiple intervals, and these histograms are robust to scale and rotational changes.

[0069] Step 1052: Screen each image feature point according to the feature descriptor to generate an extraction result.

[0070] In the embodiments of the present invention, during the feature point detection process, multiple image feature points may be detected at very close positions. Through non-maximum suppression, only the image feature point with the largest response value within the local area is retained to reduce redundancy.

[0071] Step 1054: Perform optical flow tracking calculation on the extraction result to generate an optical flow tracking result.

[0072] In the embodiments of the present invention, in visual tracking and optical flow calculation, the displacement of image feature points between consecutive image frames is usually processed. This process can be modeled by establishing a linear equation system based on the pixel intensity and gradient information around the image feature points.

[0073] Assume that the position of the image feature point P in the current frame is (x, y), and the position in the next frame is (x + Δx, y + Δy). An equation can be established according to the assumption of brightness constancy of the image, that is, the gray value of the image feature point remains unchanged in consecutive frames.

[0074] Brightness constancy equation: I(x, y) = I(x + Δx, y + Δy), where I is the image intensity function.

[0075] Perform a Taylor expansion on the right side of the brightness constancy equation, \(I(x,y)\approx I(x,y)+I_x\Delta x + I_y\Delta y\), where \(I_x\) and \(I_y\) are the partial derivatives of the image intensity function at \((x,y)\).

[0076] Rearrange the above equation to get \(I_x\Delta x+I_y\Delta y = 0\). In practical applications, multiple pixel points within a window are usually considered, resulting in a system of linear equations.

[0077] In the embodiments of the present invention, the displacements \(\Delta x\) and \(\Delta y\) between consecutive image frames can be used to update the positions of feature points in the next frame, thereby achieving the tracking of feature points. It can be used to estimate the motion of the camera or the motion of objects in the scene, which is very important for visual SLAM and 3D reconstruction. By calculating the displacements of all image feature points, an optical flow field of the entire scene can be constructed, which helps to understand the dynamic changes of the scene. It can be used to optimize algorithms, such as non-linear least squares optimization, to improve the accuracy of displacement estimation. In addition, it can also be used in filtering algorithms, such as Kalman filtering, to smooth the displacement estimation and reduce the influence of noise. Between multiple frames or multiple viewpoints, the displacements can help to match the same image feature points, which is useful for tasks such as image stitching and object recognition.

[0078] Step 106: Perform non-linear optimization of the pose based on the optical flow tracking results and the obtained integrated residual terms to generate the camera pose and the positions of the landmarks.

[0079] In the embodiments of the present invention, the integrated residual terms include pose error residual terms and / or inertial error residual terms.

[0080] Specifically, based on the optical flow tracking results and inertial sensor data, a least squares problem can be constructed in the non-linear optimization to minimize the pose error residual terms to generate the camera pose and the positions of the landmarks.

[0081] In the embodiments of the present invention, the optical flow tracking results include the reprojection errors of the image feature points obtained by optical flow tracking.

[0082] In the embodiments of the present invention, the reprojection errors and the integrated residual terms obtained by IMU pre-integration can be passed into the system backend for non-linear optimization of the pose. A least squares problem is constructed during the non-linear optimization process to minimize the reprojection errors to solve for the camera pose and the positions of the landmarks.

[0083] In the embodiments of the present invention, the estimated 3D positions of the camera pose and the landmark positions can be used to calculate the projected positions of these landmark positions in the image frame. The calculated projected positions are compared with the actually observed feature point positions to obtain the reprojection errors.

[0084] Specifically, a cost function can be constructed. The cost function is the aggregation of all reprojection errors and is usually defined as the sum of the squares of these errors, serving as the minimization objective in the optimization process. In the backend optimization of SLAM, the camera pose and the positions of the landmarks are optimized by minimizing the cost function (i.e., the sum of the squares of all integrated residual terms). This typically involves solving a non-linear least squares problem, where the Jacobian matrix and the information matrix can be used to guide the optimization process.

[0085] Step 108: Calculate the information matrix for the camera pose and the positions of the landmarks for visual-inertial fusion localization.

[0086] Figure 4 The following is a flowchart for calculating the information matrix for the camera pose and the positions of the landmarks for visual-inertial fusion localization provided by an embodiment of the present invention. As Figure 4 shown, step 108 includes: Step 1082: Calculate the information matrix for the camera pose and the positions of the landmarks based on the error function and the Jacobian matrix to generate pose parameters and landmark parameters.

[0087] In an embodiment of the present invention, the error function of the position of the j-th landmark in the i-th frame is defined as , where represents the reprojection error of the feature point, represents the camera pose of the i-th frame, represents the position of the j-th landmark, represents the pixel coordinates of the projection of the j-th landmark in the i-th frame.

[0088] In an embodiment of the present invention, the cost function can be defined as the square of the reprojection errors of multiple landmarks in multiple frames: , where m is one of the multiple frames and n is one of the positions of the multiple landmarks. Perform a first-order approximation of the error function, solve for the increment using the LM method with a confidence interval, and take the information matrix as , where is the Jacobian matrix of the error with respect to the pose, λ is the damping factor or penalty factor in the LM algorithm, and I is the identity matrix. Due to the sparsity of the H matrix, partition the information matrix and use the marginalized Schur elimination method to first find the conditional probability under the fixed landmarks and then obtain the marginal probability of the landmarks.

[0089] In an embodiment of the present invention, the matrix usually refers to the transpose of the Jacobian matrix of the error with respect to the parameter multiplied by the Jacobian matrix, that is, the information matrix or the Hessian matrix. This matrix is used for parameter update in the optimization process. In a visual SLAM system, the matrix is typically very large and sparse, as it contains the partial derivatives of all observed feature points with respect to all pose parameters and landmark position parameters. The matrix can be partitioned into several submatrices, which is usually for computational efficiency during the optimization process. These submatrices include: H pp , H pl and H ll . Specifically, where H pp is the covariance matrix of pose parameters, H pl is the covariance matrix between pose parameters and landmark parameters, and H ll is the covariance matrix of landmark parameters, thus obtaining pose parameters and landmark parameters.

[0090] Step 1084: Based on CUDA acceleration, perform iterative calculations of Schur complement and Cholesky decomposition on pose parameters and landmark parameters to generate pose optimization parameters and landmark optimization parameters for visual-inertial fusion localization.

[0091] In the embodiments of the present invention, the Schur complement operation can be performed using a linear algebra library on the GPU, and the Cholesky decomposition is accelerated by the multi-core parallelism of the GPU.

[0092] Specifically, a system equation can be constructed, and a linear system equation Hx = b is constructed using the Jacobian matrix and the error vector, where H is the information matrix (or its Schur complement), x is the parameter vector to be solved, and b is a linear combination of error vectors. Perform Cholesky decomposition on the information matrix H, decomposing it into the product of two lower triangular matrices. This step is to utilize the numerical stability and computational efficiency of Cholesky decomposition to solve the linear system. Solve the decomposed linear system through forward and backward substitution to obtain the optimal estimate of the parameters.

[0093] In the embodiments of the present invention, the pose optimization parameters include: the position and orientation of each camera frame (usually represented by a rotation matrix and a translation vector). The landmark optimization parameters include: the three-dimensional coordinates of the image feature points in the scene.

[0094] In the technical solution provided by the embodiment of the present invention, parallel processing is applied in steps 102 to 108. Among them, SURF feature points are obtained from the acquired binocular vision image data. This step performs Gaussian blur and difference calculation through CUDA acceleration to obtain the DoG pyramid, and uses the multi-core parallelism of the GPU to find the extreme points in the DoG pyramid and calculate the main direction of the key points. This step utilizes the parallel processing ability of the GPU to accelerate the feature point extraction process. After obtaining the image feature points, integral and summation operations are performed within the window around each image feature point to calculate the gradient and other statistical information of the pixels within the window. According to the pixels within the window and their gradient information, a system of linear equations is solved to determine the displacement of the feature point in the next frame. This process can be accelerated through the parallel matrix operations and linear algebra library of CUDA.

[0095] The common problems existing in current binocular vision inertial systems are as follows: during the operation of the binocular vision sensor, the quality of the image data deteriorates due to light or vibration, resulting in a reduction in the number of extracted feature points expressing image information, thus significantly reducing the accuracy of the subsequent tracking and positioning processes. Due to the limited computing power of the embedded device on the robot and the low operation efficiency of feature point extraction, optical flow tracking, and sparse matrix solution, it is difficult to ensure the real-time performance and accuracy of positioning. However, the solution proposed in the embodiment of the present invention can solve the above problems, and can efficiently extract highly robust feature points during the positioning operation, thus ensuring the stability of the binocular vision inertial system.

[0096] The technical solution provided by the embodiment of the present invention mainly targets the positioning systems of binocular vision sensors and inertial sensors, extracts and accelerates robust features at the front end, and uses CUDA parallelization to accelerate the processes of feature point extraction, optical flow tracking, and sparse matrix solution, enabling real-time and accurate positioning of the system under noise interference and computing power limitations.

[0097] Traditional VI-SLAM algorithms are often designed based on the Central Processing Unit (CPU). When directly porting them to such embedded devices, the GPU modules on the devices are not utilized. To be able to run the VI-SLAM algorithm in real time on such embedded devices with GPUs and effectively utilize the GPU modules of the devices instead of leaving them idle, it is necessary to implement a parallelized VI-SLAM algorithm based on the GPU so that the VI-SLAM algorithm can run on a heterogeneous computing model composed of a CPU and a GPU. In the VI-SLAM algorithm, feature extraction and matching of images have natural advantages in achieving parallelization, and the GPU has better parallel computing capabilities compared to the CPU. To improve the utilization rate of the GPU module when executing the VI-SLAM algorithm on an embedded device, it is possible to implement the parallelization of image feature extraction and optical flow tracking based on the GPU and migrate it to the GPU device for operation. At the same time, the non-linear least squares optimization process in the VI-SLAM algorithm occupies a relatively large amount of computing resources in the entire SLAM algorithm. Performing parallelization processing of the non-linear least squares optimization module in the algorithm based on the GPU and transplanting it to the GPU side for operation can also greatly improve the utilization rate of the GPU on the embedded device.

[0098] An embodiment of the present invention proposes a feature-enhanced efficient binocular visual inertial system accelerated based on the Compute Unified Device Architecture, which can fully utilize the computing power of the GPU side, extract highly robust feature points to achieve real-time and accurate positioning. Its visual inertial fusion positioning method performs optical flow tracking positioning by extracting accelerated robust features from binocular visual sensor data, combines the motion state estimated by IMU pre-integration, and uses CUDA to call GPU resources for parallel acceleration of the front end and back end of the system, and solves the motion state estimation optimization problem under the condition of data noise and limited computing power, and finally obtains real-time and accurate positioning results, contributing to the research and development in the field of visual perception positioning, and at the same time improving the automation level of the system.

[0099] In the technical solution provided by the embodiment of the present invention, a visual inertial joint calibration result is generated based on the obtained binocular visual sensor data and inertial sensor data for calibration initialization of the object; feature point extraction and object tracking are performed based on the visual inertial joint calibration result to generate an optical flow tracking result; pose non-linear optimization is performed based on the optical flow tracking result and the obtained integration residual term to generate the camera pose and the position of the landmark point; information matrix calculation is performed on the camera pose and the position of the landmark point for visual inertial fusion positioning. In the technical solution provided by the embodiment of the present invention, visual inertial fusion positioning is performed through the joint calibration, feature point calibration, optical flow tracking, non-linear optimization, and information matrix calculation of the binocular visual sensor data and the inertial sensor data, improving the accuracy, real-time performance, accuracy, and efficiency of navigation positioning.

[0100] A visual-inertial fusion positioning device provided by an embodiment of the present invention Figure 5 is a schematic diagram of a visual-inertial fusion positioning device provided by an embodiment of the present invention, as Figure 5 shown. The device includes: a first generation module 11, a second generation module 12, a third generation module 13, and a positioning module 14.

[0101] The first generation module 11 is used to generate a visual-inertial joint calibration result based on the obtained binocular vision sensor data and inertial sensor data for calibration initialization of the object.

[0102] The second generation module 12 is used to perform feature point extraction and object tracking according to the visual-inertial joint calibration result to generate an optical flow tracking result.

[0103] The third generation module 13 is used to perform pose non-linear optimization based on the optical flow tracking result and the obtained integral residual term to generate the camera pose and the position of the road marking point.

[0104] The positioning module 14 is used to calculate the information matrix for the camera pose and the position of the road marking point to perform visual-inertial fusion positioning.

[0105] In an embodiment of the present invention, the first generation module 11 is specifically used for: Calculating the binocular vision sensor data and the inertial sensor data based on the PNP algorithm to generate the first camera pose parameter; Calculating the feature depth based on the binocular triangulation algorithm for the camera pose parameter to generate the second camera pose parameter; Calculating the error between the first camera pose parameter and the second camera pose parameter to generate a pose error remainder; Performing bias correction calculation on the inertial sensor data to generate corrected data; Calculating the error between the corrected data and the inertial sensor data to generate an inertial error remainder; Calculating the pose error remainder and the inertial error remainder based on the local bundle adjustment method to generate the visual-inertial joint calibration result.

[0106] In an embodiment of the present invention, the second generation module 12 is specifically used for accelerating the SURF feature point extraction of the visual-inertial joint calibration result based on CUDA to generate an extraction result; performing optical flow tracking calculation on the extraction result to generate an optical flow tracking result.

[0107] In an embodiment of the present invention, the second generation module 12 is specifically used for: Performing Gaussian blur processing on the binocular vision sensor data to generate a preprocessed image; Load the preprocessed image into the GPU memory and construct a multi-scale image pyramid in parallel; Calculate the main direction of the key points based on the extreme points of the multi-scale image pyramid found; Obtain multiple image feature points from the binocular vision sensor data; Perform integral and summation calculations on each of the image feature points within its surrounding window based on the main direction of the key points to generate a feature descriptor; Filter each of the image feature points according to the feature descriptor to generate an extraction result.

[0108] In the embodiments of the present invention, the integral residual term includes a pose error residual term and / or an inertial error residual term.

[0109] In the embodiments of the present invention, the optical flow tracking result includes the reprojection error of the image feature points obtained by optical flow tracking. The third generation module 13 is specifically configured to perform nonlinear optimization on the reprojection error and the integral residual term, construct a least squares problem during the nonlinear optimization process, minimize the reprojection error, and generate the camera pose and the landmark position.

[0110] In the embodiments of the present invention, the positioning module 14 is specifically configured to calculate the information matrix based on the error function and the Jacobian matrix for the camera pose and the landmark position to generate pose parameters and landmark parameters; perform iterative calculations of Schur complement and Cholesky decomposition on the pose parameters and the landmark parameters based on CUDA acceleration to generate pose optimization parameters and landmark optimization parameters for visual-inertial fusion positioning.

[0111] In the technical solution provided by the embodiments of the present invention, a visual-inertial joint calibration result is generated based on the obtained binocular vision sensor data and inertial sensor data; feature point extraction and object tracking are performed according to the visual-inertial joint calibration result to generate an optical flow tracking result; pose nonlinear optimization is performed based on the optical flow tracking result and the obtained integral residual term to generate the camera pose and the landmark position; information matrix calculation is performed on the camera pose and the landmark position for visual-inertial fusion positioning. In the technical solution provided by the embodiments of the present invention, visual-inertial fusion positioning is performed through the joint calibration, feature point calibration, optical flow tracking, nonlinear optimization, and information matrix calculation of the binocular vision sensor data and the inertial sensor data, improving the accuracy, real-time performance, accuracy, and efficiency of navigation positioning.

[0112] The visual-inertial fusion positioning device provided by the embodiments of the present invention can be used to implement the above Figure 1 visual-inertial fusion positioning method. For specific descriptions, reference can be made to the embodiments of the above visual-inertial fusion positioning method, and details will not be repeated here.

[0113] An embodiment of the present invention provides a storage medium, which includes a stored program. When the program runs, it controls the device where the storage medium is located to execute the steps of the embodiments of the above-mentioned visual-inertial fusion positioning method. For specific descriptions, reference can be made to the embodiments of the above-mentioned visual-inertial fusion positioning method.

[0114] An embodiment of the present invention provides a binocular visual-inertial system, which includes a memory and a processor. The memory is used to store information including program instructions, and the processor is used to control the execution of the program instructions. When the program instructions are loaded and executed by the processor, the steps of the embodiments of the above-mentioned visual-inertial fusion positioning method are implemented. For specific descriptions, reference can be made to the embodiments of the above-mentioned visual-inertial fusion positioning method. Among them, the binocular visual-inertial system can be applied to fields such as mobile robots, autonomous driving, intelligent manufacturing, quality inspection, virtual reality, and augmented reality.

[0115] Figure 6 It is a schematic diagram of a binocular visual-inertial system provided by an embodiment of the present invention. As Figure 6 shown, the binocular visual-inertial system 20 of this embodiment includes: a processor 21, a memory 22, and a computer program 23 stored in the memory 22 and executable on the processor 21. When the computer program 23 is executed by the processor 21, it implements the application to the visual-inertial fusion positioning method in the embodiment. To avoid repetition, details are not described here one by one. Alternatively, when the computer program is executed by the processor 21, it implements the functions of each model / unit in the visual-inertial fusion positioning device in the embodiment. To avoid repetition, details are not described here one by one.

[0116] The binocular visual-inertial system 20 includes, but is not limited to, a processor 21 and a memory 22. Those skilled in the art can understand that Figure 6 this is only an example of the binocular visual-inertial system 20 and does not constitute a limitation on the binocular visual-inertial system 20. It may include more or fewer components than shown, or combine certain components, or different components. For example, the binocular visual-inertial system may also include input / output devices, network access devices, buses, etc.

[0117] The so-called processor 21 may be a Central Processing Unit (CPU), or may also be other general-purpose processors, Digital Signal Processors (DSPs), Application Specific Integrated Circuits (ASICs), Field-Programmable Gate Arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0118] The memory 22 may be an internal storage unit of the binocular vision inertial system 20, such as the hard disk or memory of the binocular vision inertial system 20. The memory 22 may also be an external storage device of the binocular vision inertial system 20, such as a plug-in hard disk equipped on the binocular vision inertial system 20, a Smart Media Card (SMC), a Secure Digital (SD) card, a Flash Card, etc. Further, the memory 22 may also include both the internal storage unit of the binocular vision inertial system 20 and the external storage device. The memory 22 is used to store computer programs and other programs and data required by the binocular vision inertial system. The memory 22 may also be used to temporarily store data that has been output or is to be output.

[0119] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the systems, devices, and units described above can refer to the corresponding processes in the foregoing method embodiments and will not be elaborated herein.

[0120] In several embodiments provided by the present invention, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is only a logical function division, and there may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other may be through some interfaces, and the indirect couplings or communication connections of the devices or units may be in electrical, mechanical, or other forms.

[0121] The unit described as a separation component may or may not be physically separated. The component shown as a unit may or may not be a physical unit, that is, it may be located in one place or distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0122] In addition, each functional unit in various embodiments of the present invention may be integrated in a processing unit, may exist separately as individual physical units, or two or more units may be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of a hardware plus software functional unit.

[0123] The above-mentioned integrated unit implemented in the form of a software functional unit can be stored in a computer-readable storage medium. The above-mentioned software functional unit stored in a storage medium includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor to execute some steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical discs that can store program codes.

[0124] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A visual-inertial fusion positioning method, characterized in that, Including: Based on the acquired binocular vision sensor data and inertial sensor data, calibrate and initialize the object to generate a visual-inertial joint calibration result; According to the visual-inertial joint calibration result, perform feature point extraction and object tracking to generate an optical flow tracking result; Based on the optical flow tracking result and the acquired integrated residual term, perform pose non-linear optimization to generate the camera pose and the position of the road marking points; Calculate the information matrix for the camera pose and the position of the road marking points to perform visual-inertial fusion positioning.

2. The method according to claim 1, characterized in that, The step of calibrating and initializing the object based on the binocular vision sensor data and inertial sensor data to generate a visual-inertial joint calibration result includes: Calculate the first camera pose parameter based on the PNP algorithm for the binocular vision sensor data and the inertial sensor data; Calculate the feature depth based on the binocular triangulation algorithm for the camera pose parameter to generate the second camera pose parameter; Calculate the error between the first camera pose parameter and the second camera pose parameter to generate a pose error residual term; Perform bias correction calculation on the inertial sensor data to generate corrected data; Calculate the error between the corrected data and the inertial sensor data to generate an inertial error residual term; Based on the local bundle adjustment method, calculate the pose error residual term and the inertial error residual term to generate the visual-inertial joint calibration result.

3. The method according to claim 1, wherein The step of performing feature point extraction and object tracking according to the visual-inertial joint calibration result to generate an optical flow tracking result includes: Accelerate the SURF feature point extraction of the visual-inertial joint calibration result based on CUDA to generate an extraction result; Perform optical flow tracking calculation on the extraction result to generate an optical flow tracking result.

4. The method according to claim 3, characterized in that, The step of accelerating the SURF feature point extraction of the visual-inertial joint calibration result based on CUDA to generate an extraction result includes: Perform Gaussian blur processing on the binocular vision sensor data to generate a preprocessed image; Load the preprocessed image into the GPU memory and parallelly construct a multi-scale image pyramid; Calculate the main direction of the key points according to the extreme points found in the multi-scale image pyramid; Obtain multiple image feature points in the binocular vision sensor data; Based on the main direction of the key points, perform integral and summation calculations for each image feature point within its surrounding window to generate a feature descriptor; Screen each image feature point according to the feature descriptor to generate an extraction result.

5. The method according to claim 1, wherein The integrated residual term includes a pose error residual term and / or an inertial error residual term.

6. The method according to claim 1, characterized in that, The optical flow tracking result includes the reprojection error of the image feature points obtained by optical flow tracking. The step of performing pose non-linear optimization based on the optical flow tracking result and the acquired integrated residual term to generate the camera pose and the position of the road marking points includes: Perform non-linear optimization on the reprojection error and the integrated residual term, construct a least squares problem during the non-linear optimization process, and minimize the reprojection error to generate the camera pose and the position of the road marking points.

7. The method according to claim 1, characterized in that, The step of calculating the information matrix for the camera pose and the position of the road marking points to perform visual-inertial fusion positioning includes: Perform information matrix calculation on the camera pose and the road landmark position based on the error function and the Jacobian matrix to generate pose parameters and road landmark parameters; Based on CUDA acceleration, perform iterative calculations of Schur complement and Cholesky decomposition on the pose parameters and the road landmark parameters to generate pose optimization parameters and road landmark optimization parameters for visual-inertial fusion positioning.

8. A visual-inertial fusion positioning device, characterized in that, Including: A first generation module, configured to generate a visual-inertial joint calibration result for a calibration initialization object based on the acquired binocular vision sensor data and inertial sensor data; A second generation module, configured to perform feature point extraction and object tracking according to the visual-inertial joint calibration result to generate an optical flow tracking result; A third generation module, configured to perform pose non-linear optimization based on the optical flow tracking result and the acquired integration residual term to generate a camera pose and a road landmark position; A positioning module, configured to perform information matrix calculation on the camera pose and the road landmark position for visual-inertial fusion positioning.

9. A storage medium, characterized in that, The storage medium includes a stored program, wherein when the program runs, it controls the device where the storage medium is located to execute the visual-inertial fusion positioning method according to any one of claims 1 to 7.

10. A binocular vision inertial system, comprising a memory and a processor, the memory being used for storing information including program instructions, and the processor being used for controlling the execution of the program instructions, characterized in that, When the program instructions are loaded and executed by a processor, the steps of the visual-inertial fusion positioning method according to any one of claims 1 to 7 are implemented.

Images