A visual rapid positioning method, device and system based on point-surface feature fusion

Abstract

The present invention provides a method, device and system for rapid visual positioning based on point-surface feature fusion, and relates to the field of image processing technology. The method for rapid visual positioning based on point-surface feature fusion described in the present invention includes: acquiring image and posture information data, preprocessing the image and posture information data to obtain preprocessed data; performing internal point set identification and extraction on the preprocessed data to obtain an internal point set, performing plane fitting on the internal point set to obtain first plane information; performing dynamic plane update on the first plane information to obtain second plane information; updating the plane reprojection error according to the second plane information; performing PNP solution based on the internal point reprojection error, the updated plane reprojection error and IMU constraints to determine the optimal posture. Rapid visual positioning in a sparse texture environment is achieved by means of point-surface feature fusion and the like, and still has high accuracy and robustness when environmental texture features are lacking or even missing.

Description

Technical Field

[0001] The present invention relates to the field of image processing technology, and in particular to a visual rapid positioning method, device and system based on point-surface feature fusion. Background Art

[0002] With the advancement of science and technology and the development of the times, accurate and fast visual positioning and mapping in sparse texture environments is of great significance in the fields of intelligent unmanned driving, smart home, augmented reality and so on.

[0003] In the field of positioning and mapping in sparse feature environments, common methods are mainly positioning and mapping methods based on lidar, such as single-line 2D lidar, 24-line 3D lidar, and positioning and mapping methods using vision, such as using monocular, binocular cameras or depth cameras for simultaneous positioning and mapping based on optimization. With the rapid development of positioning and mapping algorithms, a variety of positioning and mapping methods based on the combination of multiple types of visual features have emerged, but there are generally problems such as poor robustness of the positioning process and inaccurate positioning results due to the difficulty in extracting environmental features in sparse texture environments. Summary of the invention

[0004] The problem solved by the present invention is how to achieve rapid visual positioning that meets robustness and high precision.

[0005] To solve the above problems, the present invention provides a visual rapid positioning method based on point-plane feature fusion, including: acquiring image and posture information data, preprocessing the image and posture information data to obtain preprocessed data; performing internal point set identification and extraction on the preprocessed data to obtain an internal point set, performing plane fitting on the internal point set to obtain first plane information; performing dynamic plane updating on the first plane information to obtain second plane information; updating the plane reprojection error according to the second plane information; and performing PNP solution based on the internal point reprojection error, the updated plane reprojection error and IMU constraints to determine the optimal posture.

[0006] The visual rapid positioning method based on point-surface feature fusion described in the present invention realizes visual rapid positioning in a sparse texture environment by means of point-surface feature fusion and the like. It does not rely on rich environmental texture features and still has high accuracy and robustness when environmental texture features are lacking or even missing.

[0007] Optionally, the preprocessing of the image and posture information data to obtain preprocessed data includes: extracting optical flow features from the image frames input by the camera, performing optical flow tracking by matching feature points between adjacent frames, and integrating the posture information input by the IMU so that the frequency of the integrated data is consistent with the image frame input frequency.

[0008] The visual rapid positioning method based on point-surface feature fusion described in the present invention extracts image features for subsequent inter-frame pose estimation and solution, and makes the integrated data frequency consistent with the image frame input frequency through IMU pre-integration, thereby facilitating system initialization and nonlinear optimization and improving the accuracy of data preprocessing.

[0009] Optionally, the inner point set is identified and extracted from the preprocessed data to obtain an inner point set, and the first plane information is obtained by plane fitting on the inner point set, including: removing outer points in the optical flow points based on a random sampling consistency method to obtain the inner point set, and solving plane parameters according to the set relationship between three or more inner points to obtain a fitting plane equation to determine the first plane information.

[0010] The visual rapid positioning method based on point-surface feature fusion described in the present invention removes external points in optical flow points through a random sampling consistency method to obtain an internal point set, which is beneficial to improving the accuracy of subsequent positioning and mapping, and detects and fits plane features in a sparse texture environment with planar structures, which is beneficial to improving the robustness of positioning and mapping.

[0011] Optionally, the dynamic plane update of the first plane information to obtain the second plane information includes: determining the consistency of the reprojection error based on the fitting plane equation, wherein the reprojection error consistency refers to the difference between the reprojection error of the point in the space outside the known plane and the reprojection error after the point in the space is projected onto the known plane; if the reprojection error consistency is less than a specified threshold, the point in the space is added to the known plane, and if the reprojection error consistency is greater than or equal to the specified threshold, the point in the space is separated from the known plane.

[0012] The visual rapid positioning method based on point-plane feature fusion described in the present invention is beneficial to improving the dynamic plane expansion speed by using the consistency of point-plane reprojection errors as a judgment condition for dynamic plane expansion or dynamic plane reduction.

[0013] Optionally, the visual rapid positioning method based on point-surface feature fusion also includes: screening out image frames containing key information from multiple frames of image and posture information data, and optimizing the key frames according to a sliding window strategy to retain the key frames closest to the current moment.

[0014] The visual rapid positioning method based on point-surface feature fusion described in the present invention selects image frames containing key information from multiple image frames, optimizes the key frames according to the sliding window strategy to retain the key frames closest to the current moment, thereby reducing the optimization pressure.

[0015] Optionally, the PNP solution based on the interior point reprojection error, the updated plane reprojection error and the IMU constraint to determine the optimal posture includes: performing PNP tracking based on the combination of visual inertial navigation for each image frame output after updating the plane reprojection error, and solving the optimal posture according to the error optimization.

[0016] The visual rapid positioning method based on point-surface feature fusion described in the present invention performs PNP tracking based on the combination of visual inertial navigation for each image frame output after updating the plane reprojection error, and solves the optimal posture according to the error optimization, thereby realizing visual rapid positioning based on point-surface feature fusion.

[0017] Optionally, the visual rapid positioning method based on point-plane feature fusion also includes: updating a global map according to the second plane information and the optimal posture.

[0018] The visual rapid positioning method based on point-surface feature fusion described in the present invention updates the global map according to the second plane information and the optimal posture, completes the BA optimization process, and realizes visual rapid positioning based on point-surface feature fusion.

[0019] The present invention also provides a visual rapid positioning device based on point-surface feature fusion, comprising: a preprocessing module for acquiring image and posture information data, preprocessing the image and posture information data to obtain preprocessed data; a first plane information module for identifying and extracting an inlier set on the preprocessed data to obtain an inlier set, and plane fitting the inlier set to obtain first plane information; a second plane information module for dynamically updating the first plane information to obtain second plane information; a plane reprojection error module for updating the plane reprojection error according to the second plane information; a posture solving module for performing PNP solving to determine the optimal posture based on the inlier reprojection error, the updated plane reprojection error and the IMU constraint. The visual rapid positioning device based on point-surface feature fusion has the same advantages as the above-mentioned visual rapid positioning method based on point-surface feature fusion over the prior art, which will not be repeated here.

[0020] The present invention also provides a visual rapid positioning system based on point-surface feature fusion, comprising a computer-readable storage medium storing a computer program and a processor, wherein when the computer program is read and executed by the processor, the visual rapid positioning method based on point-surface feature fusion as described above is implemented. The visual rapid positioning system based on point-surface feature fusion has the same advantages as the visual rapid positioning method based on point-surface feature fusion over the prior art, which will not be described in detail here.

[0021] The present invention also provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is read and executed by a processor, the visual rapid positioning method based on point-surface feature fusion as described above is implemented. The advantages of the computer-readable storage medium and the visual rapid positioning method based on point-surface feature fusion as described above over the prior art are the same, and are not described in detail here. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 The process of the visual rapid positioning method based on point-surface feature fusion according to the embodiment of the present invention is as follows Figure 1 ;

[0023] Figure 2 The process of the visual rapid positioning method based on point-surface feature fusion according to the embodiment of the present invention is as follows Figure 2 . DETAILED DESCRIPTION

[0024] The relevant prior art mentioned in the background technology is briefly described as follows.

[0025] The principle of the method based on laser radar positioning and mapping is to obtain the position information of the aircraft itself and the surrounding environment information by processing the laser radar reflection signal through data processing, clustering and feature extraction. This method has problems such as limited detection range, lack of semantic information in the map and special structural requirements for installation. At the same time, since high-precision multi-line 3D laser radar is expensive and is greatly affected by weather factors when working in outdoor scenes, it is not competent for the deployment and application of sparse feature texture environment positioning and mapping.

[0026] Visual positioning and mapping is another common positioning and mapping method. This method has no sensor detection distance limit, can extract visual features, has a simple structure and low cost, and has therefore been widely used. Most of these methods use feature point methods for tracking and matching, and rely more on rich environmental texture features. When environmental texture features are lacking or even missing, the accuracy of the above methods will be greatly affected, and robustness is also difficult to guarantee.

[0027] The localization and mapping algorithm based on multiple visual features is an emerging localization and mapping method that has gradually developed in recent years. Its principle is to add higher-dimensional visual features to the constraint relationship and perform local and global BA optimization in sequence. This method has a series of advantages such as better robustness, better real-time performance and higher accuracy.

[0028] With the rapid development of positioning and mapping algorithms, a variety of positioning and mapping methods based on the combination of multiple visual features have emerged. These methods all use a combination of visual feature points and feature lines. These methods reduce the dependence on environmental features to a certain extent and improve the accuracy and robustness of positioning and mapping. However, since this method is too dependent on line features, the line features extracted by the positioning and mapping method based on point-line fusion in an environment where some textures are missing are greatly reduced or even difficult to extract, which will greatly reduce the positioning accuracy of this method. There are also some methods that apply deep learning target detection to visual positioning and mapping methods with good accuracy, but since some labels are generated based on training, when applied to unfamiliar environments, labels are difficult to accurately identify, and the accuracy and robustness of positioning and mapping are also difficult to guarantee.

[0029] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0030] like Figure 1 As shown, an embodiment of the present invention provides a visual rapid positioning method based on point-plane feature fusion, including: acquiring image and posture information data, preprocessing the image and posture information data to obtain preprocessed data; performing internal point set identification and extraction on the preprocessed data to obtain an internal point set, performing plane fitting on the internal point set to obtain first plane information; performing dynamic plane updating on the first plane information to obtain second plane information; updating the plane reprojection error according to the second plane information; performing PNP solution based on the internal point reprojection error, the updated plane reprojection error and IMU constraints to determine the optimal posture.

[0031] Specifically, in this embodiment, the visual rapid positioning method based on point-surface feature fusion includes: obtaining image and posture information data input by the sensor, and preprocessing the image and posture information data to obtain preprocessed data. Figure 2 As shown, the camera image and IMU information are preprocessed separately, including image feature extraction tracking and IMU information pre-integration, so that the processed data can meet the requirements of initialization and back-end optimization for data frequency and accuracy. Data information preprocessing is closely related to the plane detection and update described in the present invention. The preprocessed information data is input into the system initialization part for visual IMU information alignment and visual SFM, and the uninitialized IMU and camera information are reinitialized.

[0032] Among them, the sensor information preprocessing includes image feature extraction and tracking and IMU information pre-integration. When extracting and tracking image features, in order to achieve fast and accurate data association, the present invention will first perform optical flow feature detection and construct an image pyramid, and then perform sparse optical flow tracking based on the image pyramid. In the feature detection part, in order to meet the real-time requirements, the present invention will use a Fast feature extractor, and at the same time, in order to improve the detection success rate and the robustness of tracking, the detection will be mainly performed on corner points. When constructing the image pyramid, the original image will be downsampled using a convolution kernel, and different levels of the image pyramid will be used for optical flow tracking and matching. After the above preprocessing is completed, the input initialization part will be subjected to visual IMU (Inertial Measurement Unit, i.e., inertial measurement unit, including accelerometer, gyroscope and magnetometer, outputting measured three-axis angular velocity and three-axis acceleration, used for object pose estimation and positioning) information alignment and visual SFM (Simulation Localization And Mapping, i.e., simultaneous positioning and mapping, specifically refers to the technology of building maps while the robot moves in an unknown environment). By constructing a mathematical model, the information data input by the sensor is better preprocessed.

[0033] The preprocessed data is subjected to interior point set identification and extraction to obtain an interior point set, and the interior point set is subjected to plane fitting to obtain the first plane information. Figure 2 As shown, the initialized data is input into the plane detection and fitting part for internal point set identification and extraction and plane detection and fitting. The plane obtained by detection and fitting can be used as prior information for camera pose optimization and global BA (Bundle Adjustment, specifically refers to constructing error optimization problems and solving the optimal state estimation based on camera pose and observation data).

[0034] The first plane information is dynamically updated to obtain the second plane information. The dynamic plane update consists of three parts: point-to-plane reprojection error consistency solution, point-to-plane reprojection error consistency judgment, and dynamic plane update strategy. Point-to-plane reprojection error consistency solution and judgment specifically refer to the difference between the reprojection error of an inner point outside a known plane and the reprojection error calculated after the inner point is projected onto a known plane. The difference is defined as the reprojection error consistency, which is used to determine whether the inner point is within the plane. Once the reprojection consistency error is less than the specified threshold, we assume that the point approximately falls within the plane, and a dynamic plane expansion update can be performed. Similarly, for a point within the plane, if the reprojection consistency error is greater than the specified threshold, a dynamic plane reduction update is performed.

[0035] The plane reprojection error is updated according to the second plane information. The plane reprojection error update refers to the need to recalculate the plane reprojection error according to the dynamic plane update result after the dynamic update according to the result of the reprojection error consistency judgment.

[0036] Based on the internal point reprojection error, the updated plane reprojection error and the IMU constraint, the PNP (Perspective-n-Point) solution is performed to determine the optimal pose. The PNP solution based on the three-party constraint refers to the camera pose solution based on the three prior conditions of the internal point reprojection error, the plane reprojection error and the IMU constraint obtained by the previous solution to obtain the optimal pose at the current moment.

[0037] The specific statement of the PNP problem is as follows: given the distance between any two feature points among the given n feature points and the angle between the two feature points and the optical center, the distance between each feature point and the optical center is solved. This is the PNP problem. The main use of PNP is to determine the coordinates of the n feature points on the target object in the camera coordinate system, and then calculate the coordinate values ​​of the feature points in the world coordinate system based on the internal and external parameters of the camera obtained by calibration, and finally give the target's posture information.

[0038] Compared with the prior art, this embodiment solves the problem of poor robustness of the positioning process and inaccurate positioning results caused by the difficulty in extracting environmental features in a sparse texture environment by designing a visual positioning method based on point-surface feature fusion. Even in an environment where environmental texture is missing, it can effectively implement visual rapid positioning and mapping based on point-surface fusion under the premise of ensuring the real-time positioning and mapping, meeting the robustness and accuracy requirements. Compared with the positioning and mapping method based on laser radar, it has the advantages of low cost, simple installation and strong adaptability, and will not reduce the positioning and mapping accuracy due to the influence of climate, environment and other factors.

[0039] In this embodiment, rapid visual positioning in a sparse texture environment is achieved by means of point-surface feature fusion and the like. It does not rely on rich environmental texture features and still has high accuracy and robustness when environmental texture features are lacking or even missing.

[0040] Optionally, the preprocessing of the image and posture information data to obtain preprocessed data includes: extracting optical flow features from the image frames input by the camera, performing optical flow tracking by matching feature points between adjacent frames, and integrating the posture information input by the IMU so that the frequency of the integrated data is consistent with the image frame input frequency.

[0041] Specifically, in this embodiment, the image and posture information data are preprocessed to obtain preprocessed data, including: extracting optical flow features from the image frames input by the camera, tracking optical flow by matching feature points between adjacent frames, and integrating the posture information input by the IMU, so that the integrated data frequency is consistent with the image frame input frequency. Data information preprocessing is to preprocess the camera image and IMU information separately, while meeting the requirements of initialization and back-end optimization for data frequency and accuracy. The structure mainly consists of two parts: image feature extraction and tracking and IMU information pre-integration. The image feature extraction part is to extract optical flow features from the image frames input by the camera, and to track optical flow by matching feature points between adjacent frames, so as to perform inter-frame posture estimation and solution later. The IMU information pre-integration part is to integrate the posture information input by the IMU, so that the integrated data frequency is consistent with the image frame input frequency, thereby facilitating system initialization and nonlinear optimization. System initialization consists of visual IMU information alignment, initialization detection, and visual SFM. Visual IMU information alignment mainly consists of three parts: IMU angular velocity, acceleration, and linear velocity pre-integration. Initialization detection consists of two parts: IMU frequency detection and camera frame detection. It can detect whether the current IMU and camera frequencies are consistent. Visual SFM mainly consists of four parts: feature matching, matrix calculation, 3D reconstruction, and parameter optimization. It can reinitialize the uninitialized IMU and camera information for subsequent optimization.

[0042] In this embodiment, image features are extracted to facilitate subsequent inter-frame pose estimation and solution, and IMU pre-integration is used to make the integrated data frequency consistent with the image frame input frequency, thereby facilitating system initialization and nonlinear optimization and improving the accuracy of data preprocessing.

[0043] Optionally, the inner point set is identified and extracted from the preprocessed data to obtain an inner point set, and the first plane information is obtained by plane fitting on the inner point set, including: removing outer points in the optical flow points based on a random sampling consistency method to obtain the inner point set, and solving plane parameters according to the set relationship between three or more inner points to obtain a fitting plane equation to determine the first plane information.

[0044] Specifically, in this embodiment, the preprocessed data is subjected to inlier set identification and extraction to obtain an inlier set, and the inlier set is subjected to plane fitting to obtain the first plane information, including: removing the outer points in the optical flow points based on the random sampling consistency method to obtain the inlier set, and solving the plane parameters according to the set relationship between three or more inliers to obtain the fitting plane equation to determine the first plane information. In view of the requirements of sparse texture environment for algorithm robustness, a static plane detection and fitting method based on a three-point standard inlier set is proposed, and the static plane detection and fitting consists of standard inlier set identification and extraction, plane detection and fitting.

[0045] The standard internal point set identification and extraction part consists of local internal point extraction, external point identification and elimination, and model error rate evaluation. The preprocessed optical flow points contain some external points, which are caused by unreliable sensor measurements and mismatched optical flow features. In order to facilitate the subsequent calculation of the essential matrix, triangulation, and PNP solution, a strategy for internal point extraction (external point elimination) based on the random sampling consistency method is proposed, as shown in Table 1:

[0046]

[0047] The plane detection and fitting scheme consists of plane fitting condition judgment and three-point method for fitting static planes. Plane fitting condition judgment is mainly used to judge whether the extracted internal point set can be used for plane fitting. Specifically, it is to judge whether the number of elements in the internal point set of the unit space is greater than the specified threshold. If it is greater than the specified threshold, it is considered that a plane appears in the space, and further plane fitting is performed. The present invention proposes a three-point plane fitting method based on internal points, which specifically refers to solving the plane parameters by using the set relationship between three or more internal points in the space, thereby solving the fitting plane equation. Parameterization can be based on infinite planes:

[0048]

[0049] Among them, c represents the plane, n c represents the normal vector of plane c, d c Represents the distance from the origin to plane c. For any point x on plane c, it satisfies:

[0050]

[0051] The above scheme can realize plane model construction and optimization, and further realize static plane fitting and solution. In the design of external point elimination, plane fitting condition judgment and three-point method for fitting static plane in the visual front-end, the outlier values ​​with excessive reprojection error will be eliminated to improve the accuracy of subsequent positioning and mapping; further, the plane features can be detected and fitted in sparse texture environments with plane structures to improve the robustness of positioning and mapping. The three-point static plane fitting method based on confident internal points is highly adaptable to situations where some image frames in visual positioning and mapping have too many external points, are difficult to identify, and are difficult to remove. Compared with the classic positioning and mapping algorithm, the feature matching error is smaller and the camera pose estimation accuracy is higher.

[0052] In this embodiment, the external points in the optical flow points are eliminated by the random sampling consistency method to obtain the internal point set, which is beneficial to improving the subsequent positioning and mapping accuracy, and the plane features are fitted for the sparse texture environment with planar structures, which is beneficial to improving the robustness of positioning and mapping.

[0053] Optionally, the dynamic plane update of the first plane information to obtain the second plane information includes: determining the consistency of the reprojection error based on the fitting plane equation, wherein the reprojection error consistency refers to the difference between the reprojection error of the point in the space outside the known plane and the reprojection error after the point in the space is projected onto the known plane; if the reprojection error consistency is less than a specified threshold, the point in the space is added to the known plane, and if the reprojection error consistency is greater than or equal to the specified threshold, the point in the space is separated from the known plane.

[0054] Specifically, in this embodiment, dynamically updating the first plane information to obtain the second plane information includes: determining the consistency of the reprojection error based on the fitting plane equation, wherein the consistency of the reprojection error refers to the difference between the reprojection error of a point in the space outside the known plane and the reprojection error after the point in the space is projected onto the known plane; if the consistency of the reprojection error is less than a specified threshold, adding the point in the space to the known plane, and if the consistency of the reprojection error is greater than or equal to the specified threshold, separating the point in the space from the known plane.

[0055] Based on the first plane information, key frame extraction and triangulation are performed, and point-to-surface reprojection consistency is solved and judged. Figure 2 As shown, the extracted key frames will be used for subsequent sliding window optimization. Triangulation can solve the depth information of points in space, and the consistency of point-surface reprojection errors will determine the dynamic update method and update strategy of the plane.

[0056] Keyframe extraction and triangulation include keyframe identification and triangulation. Keyframe identification refers to selecting image frames containing key information from multiple frames of images, with the aim of reducing information redundancy, reducing the loss of computer resources, and ensuring the smooth operation of the system. Triangulation refers to observing the angle between two image frames to determine the distance of the same point. In monocular SLAM, the depth information of pixels cannot be obtained from a single image, and triangulation is required to estimate the depth of map points.

[0057] The dynamic plane update consists of three parts: point-to-plane reprojection error consistency solution, point-to-plane reprojection error consistency judgment, and dynamic plane update strategy. The point-to-plane reprojection error consistency solution and judgment specifically refers to the difference between the reprojection error of the inner point outside the known plane and the reprojection error calculated after the inner point is projected onto the known plane. The difference is defined as the reprojection error consistency, which is used to determine whether the inner point is inside the plane. Once the reprojection consistency error is less than the specified threshold, we consider that the point is approximately within the plane, and we can perform dynamic plane expansion and update. Similarly, for a point in the plane, if the reprojection consistency error is greater than the specified threshold, a dynamic plane reduction and update is performed. The point-to-plane reprojection error consistency judgment method has good real-time performance in an environment with a small number of structured planes, and has the advantages of fast dynamic plane expansion and good local BA effect.

[0058] The strategy implemented in the plane expansion and update of the present invention is based on the consistency of point-plane reprojection errors as a judgment condition for dynamic plane expansion or dynamic plane reduction. The basic strategy is: if the difference between the reprojection error of a point in a space outside the fitted plane after being projected onto a known plane and the original reprojection error is less than a specified threshold, then the spatial point is added to the known plane; conversely, if the difference between the reprojection error of a point in a space inside the fitted plane after being projected onto a known plane and the original reprojection error is greater than a specified threshold, then the spatial point is separated from the known plane. Based on the above strategy, the present invention can achieve robust dynamic plane expansion and reduction.

[0059] Considering that the VIO system based on has a large triangulation error due to insufficient parallax in the early visual SFM, the present invention constructs the following reprojection error consistency judgment:

[0060]

[0061]

[0062] ΔE=||E Pre -E Pre-Plane ||

[0063] Where E Pre is the reprojection error of the point set in space, E Pre-Plane is the reprojection error of the point set in space projected onto the plane, and ΔE is the deviation between the original reprojection error and the reprojection error projected onto the plane.

[0064] In summary, the point-plane reprojection error consistency judgment method has good real-time performance in an environment with a small number of structured planes, and has the advantages of fast dynamic plane expansion and good local BA effect.

[0065] In this embodiment, the point-plane reprojection error consistency is used as a judgment condition for dynamic plane expansion or dynamic plane reduction, which is beneficial to improving the dynamic plane expansion speed.

[0066] Optionally, the visual rapid positioning method based on point-surface feature fusion also includes: screening out image frames containing key information from multiple frames of image and posture information data, and optimizing the key frames according to a sliding window strategy to retain the key frames closest to the current moment.

[0067] Specifically, in this embodiment, the visual rapid positioning method based on point-surface feature fusion also includes: selecting image frames containing key information from multiple frames of images, and optimizing the key frames according to the sliding window strategy to retain the key frames closest to the current moment. The sliding window optimization strategy refers to retaining the key frames closest to the current moment during the back-end BA optimization, and not retaining earlier key frames, thereby reducing the optimization pressure.

[0068] The dynamic plane expansion strategy is: if Then the landmark point is considered to meet the plane extension condition and is added to the known plane. Where η and δ are parameters set according to the consistency of the reprojection error.

[0069] The dynamic plane reduction strategy is: if Then the inner point in the known plane is considered to satisfy the plane reduction condition, and the point is removed from the known plane. Where α and β are parameters set according to the consistency of the reprojection error.

[0070] In this embodiment, image frames containing key information are screened out from multiple image frames, and the key frames are optimized according to a sliding window strategy to retain the key frames closest to the current moment, thereby reducing the optimization pressure.

[0071] Optionally, the PNP solution based on the interior point reprojection error, the updated plane reprojection error and the IMU constraint to determine the optimal posture includes: performing PNP tracking based on the combination of visual inertial navigation for each image frame output after updating the plane reprojection error, and solving the optimal posture according to the error optimization.

[0072] Specifically, in this embodiment, the PNP solution based on the inlier reprojection error, the updated plane reprojection error and the IMU constraint to determine the optimal pose includes: performing PNP tracking based on the combination of visual inertial navigation for each image frame output after the plane reprojection error is updated, and solving the optimal pose based on error optimization. The PNP solution based on the three-party constraint refers to solving the camera pose based on the three prior conditions of the inlier reprojection error, the plane reprojection error and the IMU constraint obtained by the previous solution to obtain the optimal pose at the current moment. The pose information and global map update refers to outputting the pose information based on the above-mentioned three-party constraint PNP results, and updating the plane landmark information to the global map at the same time.

[0073] A local map is constructed for the fitted plane, and it is assumed that there are P plane models in the local map. In addition, for each image frame I i , perform PNP tracking based on visual inertial navigation and solve the camera's posture. When performing PNP solution, the posture estimation error is mainly composed of two parts: the camera posture estimation error and the IMU posture estimation error. The camera posture estimation error is mainly the visual reprojection error, including the three-dimensional internal point reprojection error and the plane reprojection error. The IMU error specifically refers to the noise propagation error term.

[0074] The construction error optimization problem is as follows:

[0075]

[0076] Among them, E b is the sum of the reprojection errors, E Point To construct the reprojection error based on the out-of-plane interior points, E Plane is the plane reprojection error based on the fitted plane, E IMU is the IMU constraint, i.e., the IMU propagation error. In addition, N is the number of inliers outside the plane matched to the current image frame, N c is the number of planes fitted to the current image frame, Ω ik is the covariance matrix of the interior points outside the plane, Ω jk is the covariance matrix of the face feature, and p,q are the pose quaternions.

[0077] In this embodiment, by performing PNP tracking based on the combination of visual inertial navigation for each image frame output after updating the plane reprojection error, and solving the optimal posture according to the error optimization, it is possible to achieve visual rapid positioning based on point and surface feature fusion.

[0078] Optionally, the visual rapid positioning method based on point-plane feature fusion also includes: updating a global map according to the second plane information and the optimal posture.

[0079] Specifically, in this embodiment, the visual rapid positioning method based on point-surface feature fusion also includes: updating the global map according to the second plane information and the optimal posture. After the optimal posture is solved, the camera posture information and plane information are added and updated to the global map, thereby completing the entire BA optimization process. After a data sequence is processed, it will be determined whether there are still image frames and IMU data inputs. If there is no input, the entire algorithm process is terminated. If there is input, it returns to re-preprocess the data.

[0080] In this embodiment, the global map is updated according to the second plane information and the optimal posture, the BA optimization process is completed, and visual rapid positioning based on point-surface feature fusion is achieved.

[0081] Another embodiment of the present invention provides a visual rapid positioning device based on point-plane feature fusion, including: a preprocessing module, used to acquire image and posture information data, and preprocess the image and posture information data to obtain preprocessed data; a first plane information module, used to identify and extract an inner point set on the preprocessed data to obtain an inner point set, and perform plane fitting on the inner point set to obtain first plane information; a second plane information module, used to dynamically update the first plane information to obtain second plane information; a plane reprojection error module, used to update the plane reprojection error according to the second plane information; a posture solution module, used to perform PNP solution based on the inner point reprojection error, the updated plane reprojection error and IMU constraints to determine the optimal posture.

[0082] Another embodiment of the present invention provides a visual rapid positioning system based on point-surface feature fusion, including a computer-readable storage medium storing a computer program and a processor. When the computer program is read and executed by the processor, the visual rapid positioning method based on point-surface feature fusion as described above is implemented.

[0083] Another embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is read and executed by a processor, the visual rapid positioning method based on point-surface feature fusion as described above is implemented.

[0084] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.

Claims

1. A visual rapid positioning method based on point-surface feature fusion, characterized in that: include: Acquire image and posture information data, and preprocess the image and posture information data to obtain preprocessed data; Performing interior point set identification and extraction on the preprocessed data to obtain an interior point set, and performing plane fitting on the interior point set to obtain first plane information; Performing dynamic plane updating on the first plane information to obtain second plane information; updating a plane reprojection error according to the second plane information; Perform PNP solution based on the interior point reprojection error, the updated plane reprojection error and the IMU constraint to determine the optimal pose; The performing of identifying and extracting an inlier set on the preprocessed data to obtain an inlier set, and performing plane fitting on the inlier set to obtain first plane information comprises: Based on the random sampling consistency method, the outer points in the optical flow points are removed to obtain the inner point set, and the plane parameters are solved according to the set relationship between three or more inner points to obtain the fitting plane equation to determine the first plane information; The dynamically updating the first plane information to obtain the second plane information includes: Determine the reprojection error consistency according to the fitted plane equation, wherein the reprojection error consistency refers to the difference between the reprojection error of the point in the space outside the known plane and the reprojection error of the point in the space after being projected onto the known plane; If the reprojection error consistency is less than a specified threshold, the point in the space is added to the known plane; if the reprojection error consistency is greater than or equal to the specified threshold, the point in the space is separated from the known plane.

2. The visual rapid positioning method based on point-surface feature fusion according to claim 1 is characterized in that: The preprocessing of the image and posture information data to obtain preprocessed data comprises: The optical flow features are extracted from the image frames input by the camera, and the optical flow is tracked by matching the feature points between adjacent frames. The pose information input by the IMU is integrated so that the frequency of the integrated data is consistent with the image frame input frequency.

3. The visual rapid positioning method based on point-surface feature fusion according to claim 1 is characterized in that: Also includes: Image frames containing key information are selected from multiple frames of the image and posture information data, and the key frames are optimized according to a sliding window strategy to retain the key frames closest to the current moment.

4. The visual rapid positioning method based on point-surface feature fusion according to claim 1 is characterized in that: The performing PNP solution based on the interior point reprojection error, the updated plane reprojection error and the IMU constraint to determine the optimal posture comprises: For each image frame output after updating the plane reprojection error, PNP tracking based on visual inertial navigation is performed, and the optimal posture is solved according to the error optimization.

5. The visual rapid positioning method based on point-surface feature fusion according to claim 4 is characterized in that: Also includes: Update the global map according to the second plane information and the optimal posture.

6. A visual rapid positioning device based on point-surface feature fusion, characterized in that: include: A preprocessing module, used to obtain image and posture information data, and preprocess the image and posture information data to obtain preprocessed data; A first plane information module, used for performing interior point set identification and extraction on the pre-processed data to obtain an interior point set, and performing plane fitting on the interior point set to obtain first plane information; A second plane information module, used for dynamically updating the first plane information to obtain second plane information; A plane reprojection error module, used for updating a plane reprojection error according to the second plane information; A posture solving module, used for performing PNP solving to determine the optimal posture based on the interior point reprojection error, the updated plane reprojection error and the IMU constraint; The performing of identifying and extracting an inlier set on the preprocessed data to obtain an inlier set, and performing plane fitting on the inlier set to obtain first plane information comprises: Based on the random sampling consistency method, the outer points in the optical flow points are removed to obtain the inner point set, and the plane parameters are solved according to the set relationship between three or more inner points to obtain the fitting plane equation to determine the first plane information; The dynamically updating the first plane information to obtain the second plane information includes: Determine the reprojection error consistency according to the fitted plane equation, wherein the reprojection error consistency refers to the difference between the reprojection error of the point in the space outside the known plane and the reprojection error of the point in the space after being projected onto the known plane; If the reprojection error consistency is less than a specified threshold, the point in the space is added to the known plane; if the reprojection error consistency is greater than or equal to the specified threshold, the point in the space is separated from the known plane.

7. A visual rapid positioning system based on point-surface feature fusion, characterized in that: It comprises a computer-readable storage medium storing a computer program and a processor. When the computer program is read and executed by the processor, the visual rapid positioning method based on point-surface feature fusion as described in any one of claims 1 to 5 is implemented.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is read and executed by a processor, the visual rapid positioning method based on point-surface feature fusion as described in any one of claims 1 to 5 is implemented.

Images