A fast semi-direct slam map construction algorithm

Abstract

The application relates to the technical field of computer vision, and discloses a fast semi-direct SLAM (Simultaneous Localization and Mapping) map construction algorithm, which comprises the following steps: acquiring and calculating image features of two adjacent video frames to obtain feature matching points; when the feature matching points reach a set value, the 3D coordinates of map points are calculated, and camera pose estimation is carried out; key points are extracted from the video frame image, and the key points are subjected to feature enhancement based on a subgraph; pixel back projection of the video frame image is carried out to a three-dimensional space to acquire point cloud data, and the point cloud data is subjected to preliminary processing; the point cloud data after the preliminary processing is fused and optimized to obtain a 3D reconstruction map. The application comprises two parts of initialization and feature enhancement and 3D mapping reconstruction; the initialization and feature enhancement aim to acquire the pose of a camera and feature information and the like, and prepare for 3D reconstruction; the 3D mapping reconstruction part converts point cloud information and the like into 3D space information by using the result of the first step, and the space information can be used for robot navigation and the like.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of computer vision, in particular to a fast semi-direct SLAM map construction algorithm. BACKGROUND

[0002] The existing map construction algorithm mainly includes a feature point-based method, a direct method, a hybrid method and a backend optimization algorithm. The feature point-based method includes an early SIFT algorithm method, an RGB-DSLAM system (using a SURF algorithm) and an ORB-SLAM system (adopting an ORB algorithm), which has a large amount of calculation, resulting in poor real-time performance and reducing the overall performance of the system. In a low-texture and feature-lacking environment, the feature point method is difficult to effectively apply because it is difficult to extract enough feature points to accurately estimate the camera pose and construct a map. The direct method has a LSD-SLAM, but it still needs feature points for loop detection in the camera pose optimization stage and has a gap in accuracy. The camera pose is directly estimated according to the brightness difference of the pixels, which is very sensitive to the internal parameters and exposure of the camera. When the camera moves quickly, the camera pose is easily lost, resulting in less accurate camera pose estimation than the feature point method. The hybrid method includes SVO, which is fast but has many limitations, is designed for the downward-looking camera of a drone and cannot adapt to complex robot motion. It has a high requirement for the environment. Moreover, it uses a monocular camera, cannot recover the real pose information and scene scale and cannot realize the loop function to improve the consistency of positioning and map construction. The backend optimization algorithm (such as the SC algorithm) needs to simultaneously solve the joint optimization of the switch variable, pose and loop when performing global optimization, which has a large amount of calculation and results in poor real-time performance.

[0003] The existing methods all have technical limitations, which ultimately lead to difficult point cloud fusion: in the map construction process, the point cloud often appears to be interleaved when superimposed, cannot be perfectly superimposed and affects the quality and success rate of map construction. SUMMARY

[0004] The present application provides a fast semi-direct SLAM map construction algorithm, which includes two parts of initialization and feature enhancement and 3D mapping reconstruction. The initialization and feature enhancement are aimed at obtaining the camera pose and feature information, etc., to prepare for 3D reconstruction. The 3D mapping reconstruction part converts the point cloud information, etc. into 3D space information using the results of the first step, and the space information can be used for robot navigation, etc.

[0005] The present application provides a fast semi-direct SLAM map construction algorithm, which includes:

[0006] A video collected by a camera is acquired, and the video is segmented into a plurality of continuous video frame images. The features of two adjacent video frame images are calculated to obtain feature matching points.

[0007] When the feature matching points reach a set value, the 3D coordinates of the map points are calculated, and camera pose estimation is performed;

[0008] Key points are extracted from the video frame images of motion blur and non-texture environment, and the key points are feature enhanced based on subgraphs;

[0009] The pixels of the video frame images are back projected to three-dimensional space through camera pose to obtain point cloud data, and the point cloud data is preliminarily processed;

[0010] The preliminarily processed point cloud data is fused and optimized to obtain a 3D reconstruction map.

[0011] Further, the video captured by the camera is obtained, and the video is segmented into a plurality of continuous video frame images, and the feature of the adjacent two video frame images is calculated to obtain the feature matching points, and the step comprises:

[0012] The video captured by the camera is obtained, and the video is segmented into a plurality of continuous video frame images;

[0013] The feature points in the video frame images are extracted by using the FAST algorithm, and the matching feature point pairs are found between the adjacent two video frame images by using the feature descriptor and the similarity measurement method.

[0014] Further, in the step of calculating the 3D coordinates of the map points when the feature matching points reach a set value, the 3D coordinates of the map points comprise:

[0015] The RANSAC algorithm is used to calculate the homography matrix or the fundamental matrix, the relative pose of the camera is recovered by decomposing the homography matrix or the fundamental matrix, and then the 3D coordinates of the map points are calculated by triangular division through the matching feature point pairs.

[0016] Further, in the step of calculating the 3D coordinates of the map points when the feature matching points reach a set value, the camera pose estimation comprises estimating the re-projection error and the photometric error of the feature points;

[0017] The re-projection error calculation comprises:

[0018] For a spatial point P 1i , the pixel coordinates on the 2D image are p 1i , and the re-projection error e of a single point is:

[0019]

[0020] The re-projection error E of all feature points is:

[0021]

[0022] where T represents the camera pose to be estimated, K represents the internal parameters of the camera, Z 1i represents P 1i the coordinates relative to the Z-axis after projection to the camera coordinate system, and N represents the total number of feature matches.

[0023] photometric error of a single pixel is defined as:

[0024]

[0025] photometric error of multiple pixels is defined as:

[0026]

[0027] where p 2j represents the 2D point coordinates in the previous frame, P 2j represents the spatial 3D coordinates of the point, Z 2j represents the spatial point P 2j the coordinates relative to the Z-axis after projection to the camera coordinate system, and I1() and I2() represent the photometric information of the corresponding pixels in the previous frame image and the current frame image, respectively, and W represents the total number of pixels.

[0028] By combining the re-projection error of the matched feature points and the photometric error of the pixel pairs, the camera pose estimate value T is solved:

[0029]

[0030] where T * represents the camera pose estimate value.

[0031] Further, the step of performing key point extraction on the video frame images in the motion blurred and textureless environment and performing feature enhancement on the key points based on subgraphs comprises:

[0032] In the video frame images in the motion blurred and textureless environment, for the feature L in the i-th subgraph, the calculation formula of the enhanced 3D point is as follows:

[0033]

[0034] where v j represents the 3D feature point in the j-th node, represents the coordinate transformation from the camera to the robot, H j represents the transformation expression of the j-th ranging, represents the inverse matrix of the distance transformation matrix, and v and m represent the first and last node indices of the subgraph, respectively.

[0035] Further, the step of acquiring point cloud data by inversely projecting pixels of video frame images to three-dimensional space through camera pose and preliminarily processing the point cloud data, comprises:

[0036] inversely projecting each 2D image pixel to three-dimensional space through camera pose to acquire 3D point cloud data;

[0037] taking a surface element model as a minimum processing unit, for each surface element, storing position information (X, Y, Z), vertex vector n, color information (R, G, B), surface element radius r, weight w and point collection time information t; wherein any 3D structure in space can be composed of a surface element model, and any surface element M S is controlled by a node in the deformation mapping;

[0038] constructing a deformation graph composed of nodes and edges, wherein each node g n stores position information g g , rotation matrix g R , translation matrix g t and time information I(M S , g) represents a surface deformation function; and when the node is optimized; M S represents a surface element of a 3D object; represents the element coordinates before surface deformation; represents the coordinates of the surface element after deformation, and the calculation formula is as follows:

[0039]

[0040] The surface element normal vector becomes:

[0041]

[0042] represents the normal vector of the surface element before deformation, represents the normal vector of the surface element after deformation, U represents the transformation relationship between the surface elements before and after deformation, w n (M S ) represents the weight of the influence of the node on the surface element model, and the sum of the weights of the nodes that have influence on the current surface element is:

[0043]

[0044] wherein d max represents the peak value of the Euclidean distance of the surface element from all nodes in the neighborhood;

[0045] After the above processing, the 3D point cloud data after removing deformation is obtained.

[0046] Further, the step of fusing and optimizing the point cloud data after preliminary processing to obtain a 3D reconstruction map comprises:

[0047] obtaining two point cloud data after preliminary processing, and extracting corresponding points to establish constraints, constraint quantity Q P is:

[0048]

[0049] wherein, represents the position of the target point cloud, T S represents the pose of the current frame camera, T d represents the camera pose of the detected loop frame, represents the point obtained by projecting the model from the previous frame pose, represents the position of the frame point cloud, and represents the corresponding timestamp of and

[0050] The pose of each node is optimized using the Forbenius norm to ensure that the pose matrix is an orthogonal matrix, and the square error function is:

[0051]

[0052] wherein, l represents a node, represents the result after pose transformation of node l, represents the result without pose transformation of node l; C represents a transformation constant; F represents the Forbenius norm;

[0053] The nodes are normalized to ensure that the pose parameters of adjacent two nodes are uninterrupted, and the corresponding square error function is:

[0054]

[0055] wherein, represents the position, translation, and attitude related variables of the node; N(g') represents the adjacent node set of the node;

[0056] The deformation coordinates of and are solved and and the adjacent two frame point clouds are aligned and optimized as the target point cloud, and the cost function is:

[0057]

[0058] The coordinates of the target point cloud are optimized, and the square error function is:​

[0059]

[0060] Finally, based on the above four cost function formulas E rot 、E reg 、E con 、E pin , the final total optimization function is constructed to solve the deformation map, and the total optimization function is:

[0061] E def =w rot E rot +w reg E reg +w con E con +w pin E pin

[0062] Wherein, the empirical value of each weight is: w rot =1,w reg =10,w con =w pin =100;

[0063] After the deformation map of the point cloud is calculated, the data of the first point cloud is deformed and transformed into the same coordinate system as the second point cloud data, the fusion and optimization of the point cloud are realized through the feature points between the two data, and finally the 3D reconstruction map is obtained.

[0064] The application also provides a fast semi-direct SLAM map construction device, comprising:

[0065] An acquisition module is configured to acquire a video collected by a camera, split the video into a plurality of continuous video frame images, and calculate features of two adjacent video frame images to obtain feature matching points.

[0066] An estimation module is configured to calculate 3D coordinates of map points and estimate a camera pose when the feature matching points reach a set value.

[0067] An extraction module is configured to extract key points from the video frame images in a motion blur and textureless environment, and enhance features of the key points based on subgraphs.

[0068] A projection module is configured to project pixels of the video frame images to a three-dimensional space through a camera pose to acquire point cloud data, and perform preliminary processing on the point cloud data.

[0069] A fusion module is configured to fuse and optimize the point cloud data after preliminary processing to obtain a 3D reconstruction map.

[0070] The application further provides a computer device comprising a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of the method when executing the computer program.

[0071] The application further provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the steps of the method.

[0072] The application has the following beneficial effects:

[0073] 1. Strong feature extraction capability: The feature enhancement module based on subgraph is designed, which can stably extract image feature points in the environment with motion blur and no texture and other insufficient features. It has strong adaptability to various complex situations that may be encountered in actual application scenarios, solves the problem of difficult feature extraction in such scenes for traditional methods, and provides high-quality feature information for subsequent pose estimation and map construction. At the same time, through the special subgraph generation method and processing mechanism, the feature point extraction effect is effectively enhanced, and compared with other methods, the key features in the image can be obtained more quickly and accurately, thereby improving the running efficiency of the whole system.

[0074] 2. Accurate camera pose estimation: The apparent shape weighted fusion method is proposed for camera pose estimation, which comprehensively considers the re-projection error and photometric error of the matched feature point pairs. This fusion method makes full use of different types of information sources, makes up for the shortcomings of single error information in some cases, and can still achieve relatively accurate camera pose estimation in difficult scenes such as feature point loss. At the same time, accurate camera pose estimation is the key to the stable operation of the visual SLAM system, and the method of the application can cope with various complex actual situations, reduce the accumulation of system errors and performance degradation caused by inaccurate pose estimation, and improve the robustness and reliability of the whole system.

[0075] 3. Efficient global optimization: The incremental dynamic covariance scaling algorithm is used for global optimization, which improves the problem of large calculation amount and poor real-time performance of traditional global optimization algorithms. By redefining the objective function and optimization solving process, the calculation complexity is simplified and the optimization efficiency is improved. At the same time, the efficient global optimization algorithm can correct the accumulated error in the system in time, so that the SLAM system can maintain good positioning accuracy and map construction quality during long-time operation, which is of great significance for long-time and large-scale scene exploration and navigation tasks in practical applications.

[0076] 4. Good real-time performance: During the design and implementation of the algorithm, attention is paid to the optimization of computational efficiency, so that the entire FSD-SLAM system can have good real-time performance while ensuring high accuracy. This is very critical for robot navigation, autonomous driving and other application scenarios that require real-time response, and can meet the real-time requirements of actual systems.

[0077] 5. Easy to extend and apply: The method of the present application has universality and scalability, and can be applied to different types of sensors and application scenarios. It can be easily fused with other sensors (such as lidar, inertial measurement unit, etc.), further improving the performance and adaptability of the system, and providing convenience for its wide application in actual engineering. BRIEF DESCRIPTION OF DRAWINGS

[0078] Figure 1 The method flowchart of an embodiment of the present application.

[0079] Figure 2 The device structure diagram of an embodiment of the present application.

[0080] Figure 3 The internal structure diagram of a computer device of an embodiment of the present application.

[0081] The implementation of the object of the present application, functional features and advantages will be further described with reference to the accompanying drawings. DETAILED DESCRIPTION

[0082] It should be understood that the specific embodiments described herein are only used to explain the present application and do not limit the present application.

[0083] The present application is divided into two parts: initialization and feature enhancement and 3D mapping reconstruction. Among them, the initialization and feature enhancement is the first part of the algorithm, aiming to obtain the pose and features of the camera and other information, and to prepare for the next step of 3D reconstruction. The 3D mapping reconstruction part uses the results of the first step to convert the point cloud information into 3D space information.

[0084] As shown in Figure 1 The present application provides a fast semi-direct SLAM map construction algorithm, which comprises:

[0085] The first part: initialization and feature enhancement, the initialization and feature enhancement is the first stage of the FSD-SLAM algorithm, including system initialization (S1-2) and feature enhancement (S3).

[0086] S1, acquiring a video collected by a camera, and dividing the video into a plurality of continuous video frame images, calculating the features of two adjacent video frame images to obtain feature matching points;

[0087] Step S1 specifically comprises:

[0088] S11, acquire a video collected by a camera and divide the video into a plurality of continuous video frame images;

[0089] S12, extract feature points in the video frame images by using a FAST algorithm, and find matched feature point pairs between two adjacent video frame images by a feature descriptor and a similarity measurement method.

[0090] Feature extraction: extract feature points in the image by using a FAST (Features from Accelerated Segment Test) algorithm.

[0091] Feature matching: find matched feature point pairs between adjacent frames by a feature descriptor and a similarity measurement method. For example, an ORB (Oriented FAST and Rotated BRIEF) feature descriptor, and a similarity measurement method such as an Euclidean distance or a Hamming distance.

[0092] S2, when the number of the matched feature points reaches a set value, calculate 3D coordinates of the map points and perform camera pose estimation.

[0093] The calculation of the 3D coordinates of the map points includes:

[0094] The RANSAC algorithm is used to calculate a homography matrix or a fundamental matrix, and the relative pose of the camera is recovered by decomposing the homography matrix or the fundamental matrix. Then, the 3D coordinates of the map points are calculated by triangulation based on the matched feature point pairs.

[0095] When the number of the matched feature points reaches a sufficient degree for subsequent processing (when the number of the matched feature points reaches a set value, it is considered that the feature points are sufficient. For example, when the number of the matched feature points reaches 50 or 100 (the specific value needs to be determined according to the actual situation and algorithm requirements), the condition for subsequent processing is met.

[0096] The RANSAC algorithm is used to calculate a homography matrix or a fundamental matrix. The relative pose of the camera is recovered by decomposing the homography matrix or the fundamental matrix. Then, the 3D coordinates of the map points are calculated by triangulation based on the matched feature point pairs, complete scene structure and camera pose information are provided for the initialization of the system, and the initialization process of the system is completed. After the system is initialized, the video frames are put into a fixed timeline Q queue. The feature enhancement module based on the subgraph acts on the head of the Q queue, outputs the RGB frame with the enhanced FAST feature points, and then updates the output result back to the timeline Q queue for use by the subsequent process.

[0097] Camera pose estimation includes estimating re-projection error and photometric error of feature points, and the re-projection error calculation includes:

[0098] For a spatial point P 1i , the pixel coordinate on a 2D image is p 1i , and the re-projection error e of a single point is:

[0099]

[0100] The re-projection error E of all feature points is:

[0101]

[0102] Where T represents the camera pose to be estimated, K represents the internal parameters of the camera, Z 1i represents the coordinate of P 1i projected into the camera coordinate system relative to the Z axis, and N represents the total number of feature matches.

[0103] The photometric error of a single pixel point is defined as:

[0104]

[0105] The photometric error of multiple pixel points is defined as:

[0106]

[0107] Where p 2j represents the 2D point coordinate in the previous frame, P 2j represents the spatial 3D coordinate of the point, Z 2j represents the coordinate of P 2j projected into the camera coordinate system relative to the Z axis, I1() and I2() represent the photometric information of the corresponding pixels in the previous frame image and the current frame image respectively, and W represents the total number of pixels.

[0108] By combining the re-projection error of the matched feature points and the photometric error of the pixel pairs, the camera pose estimation value T

[0109]

[0110] is solved, where T * represents the camera pose estimation value.

[0111] S3, key point extraction is performed on the video frame image of motion blur and textureless environment, and feature enhancement is performed on the key points based on subgraphs.

[0112] In the video frame image of motion blur and non-texture environment, the calculation formula of the enhanced 3D point of the feature L in the i-th subgraph is

[0113]

[0114] Where, v j represents the 3D feature point in the j-th node, represents the coordinate transformation from the camera to the robot, H j represents the transformation expression of the j-th ranging, represents the inverse matrix of the distance transformation matrix, and v and m represent the first and last node indices of the subgraph, respectively.

[0115] Second part: 3D mapping reconstruction, the process mainly includes point cloud data acquisition and preliminary processing and graph optimization (S4), point cloud fusion (S5) two parts.

[0116] S4, the pixel of the video frame image is back projected to the three-dimensional space through the camera pose to obtain point cloud data, and the point cloud data is preliminarily processed;

[0117] After obtaining the high-precision camera pose through the optimization processing of the previous step, the 2D image pixel is back projected to the three-dimensional space to obtain 3D point cloud data, so as to perform point cloud fusion to obtain a high-precision dense 3D map. Finally, the point cloud data after removing deformation is obtained, and the next step can use constraints to further optimize the pose parameters of the data.

[0118] Step S4 is specifically as follows:

[0119] The 2D image pixel is back projected to the three-dimensional space through the camera pose to obtain 3D point cloud data;

[0120] Taking a surface element model as the minimum processing unit, for each surface element, the position information (X, Y, Z), the vertex vector n, the color information (R, G, B), the surface element radius r, the weight w and the point collection time information t are stored; any 3D structure in space can be composed of a surface element model, and any surface element M S is controlled by the nodes in the deformation mapping. When new position parameters are obtained through node optimization, it will affect the surface element, including the change of the normal vector and the change of the surface element position.

[0121] A deformation graph composed of nodes and edges is constructed for global optimization, wherein each node g n stores position information g g , a rotation matrix g R , a translation matrix g t and time information I(M​S g) represents the surface deformation function; and when the node is optimized; M S represents the surface element of the 3D object; represents the element coordinate before surface deformation; represents the coordinate of the surface element after deformation, and the calculation formula is as follows:

[0122]

[0123] The formula means that the position of the affected surface element is changed after the node is optimized.

[0124] The surface element normal vector becomes:

[0125]

[0126] represents the normal vector of the surface element before deformation, represents the normal vector of the surface element after deformation, U represents the transformation relationship between the surface elements before and after deformation, and w n (M S ) represents the weight of the node on the surface element model, and the sum of the weights of the nodes that have an impact on the current surface is:

[0127]

[0128] wherein d max represents the peak value of the Euclidean distance between the surface element and all nodes in the neighborhood;

[0129] After the above processing, the 3D point cloud data without deformation is obtained.

[0130] S5, the point cloud data after preliminary processing is fused and optimized to obtain a 3D reconstruction map.

[0131] The node optimization changes the position and normal vector of the surface element, obtains two point cloud data after preliminary processing (without determining which two point clouds to fuse. When point cloud data A and point cloud data B are given, they need to be fused into point cloud data C. Only the corresponding points in A and B are extracted and the following constraints are established, the two point cloud data can be fused), and the corresponding points are extracted (ICP (Iterative Closest Point, Iterative Closest Point) algorithm can be used to find the corresponding points of the two point cloud data. ICP is one of the most basic and classic methods in point cloud registration. The corresponding relationship between the nearest points between the source point cloud and the target point cloud is calculated by iteration, and a rotation and translation matrix is constructed to minimize the distance between the points) to establish constraints, and the constraint quantity Q P is:

[0132]

[0133] wherein, represents the position of the target point cloud, T S represents the pose of the current frame camera, T d represents the camera pose of the detected loop frame, represents the point projected from the model by the pose of the previous frame, represents the position of the frame point cloud, and represents the position of the target point cloud corresponding to and the corresponding timestamp;

[0134] The pose of each node is optimized using the Forbenius norm, ensuring that the pose matrix is an orthogonal matrix, and the square error function is:

[0135]

[0136] wherein, l represents a node, represents the result after the pose transformation of the node l, represents the result without pose transformation of the node l; C represents the transformation constant; F represents the Forbenius norm;

[0137] The nodes are normalized, and the pose parameters of the adjacent two nodes are uninterrupted, and the corresponding square error function is:

[0138]

[0139] wherein, represents the position, translation, and attitude-related variables of the node; N(g') represents the adjacent node set of the node;

[0140] The deformed coordinates of the above-solved and are combined and and the adjacent two frame point clouds are aligned and optimized as the target point cloud, and the cost function is:

[0141]

[0142] The coordinates of the target point cloud are optimized, and the square error function is:

[0143]

[0144] Finally, based on the above four cost function formulas E rot , E reg , E con , E pin , the final total optimization function is constructed to solve the deformation map, and the total optimization function is:

[0145] E def = w rot E rot + w reg E reg + w con E con + w pin E pin

[0146] Wherein, the empirical value of each weight is: w rot = 1, w reg = 10, w con = w pin = 100;

[0147] After the deformation diagram of the point cloud is calculated, the data of the point cloud A is deformed and transformed to the same coordinate system as the point cloud data B, the fusion and optimization of the point cloud are realized through the feature points between the two data, and finally the 3D reconstruction diagram is obtained.

[0148] The feature enhancement module of the application is based on a subgraph design feature enhancement module, improves the feature extraction efficiency, and enables the system to stably extract feature points in a complex environment. The method of the application is used for camera pose estimation, can realize accurate pose estimation in the case of missing feature points, and is used for global optimization, simplifies the optimization target, avoids a large amount of calculation, and improves the positioning accuracy. The method is proposed for point cloud fusion, and improves the success rate of map construction.

[0149] In the application, the advantages are as follows:

[0150] (1) Design of feature enhancement module

[0151] A feature enhancement module based on a subgraph is designed, and the FAST key points are enhanced. The module generates a subgraph according to the reliability of robot ranging, so as to effectively enhance the feature points.

[0152] In the environment of motion blur and insufficient features such as no texture, the image feature points can be stably extracted, the feature extraction efficiency is greatly improved, a good foundation is laid for the subsequent link, and the problem of difficult feature extraction of traditional methods in such complex environment is effectively improved.

[0153] (2) Innovation of camera pose estimation method

[0154] The method of apparent shape weighted fusion is proposed for camera pose estimation, not only considering the traditional re-projection error, but also combining the photometric error of the matching feature point pair, and the camera pose estimation value is comprehensively solved through a specific optimization objective function.

[0155] The camera pose estimation can be realized accurately in difficult scenes such as feature point loss. Compared with the traditional pose estimation method which only relies on single error information, the fusion method fully utilizes the advantages of different information sources, improves the accuracy and stability of pose estimation, and improves the performance of the entire SLAM system in complex environments.

[0156] (3) Improvement of global optimization algorithm

[0157] An incremental dynamic covariance scaling algorithm is proposed for global optimization. In view of the problems of large calculation amount and poor real-time performance of the traditional global optimization algorithm, the algorithm redefines the objective function by considering the adjacent nodes and the edges between them, and solves the switching variable by making the partial derivative of each variable to be 0 when the optimizer converges, so as to determine the optimal value range of the switching variable, thereby simplifying the optimization process and improving the optimization efficiency.

[0158] The traditional global optimization algorithm effectively solves the problem of high calculation complexity when processing the accumulated error of the SLAM system, improves the positioning accuracy, makes the entire system maintain good performance during long-time operation, reduces the positioning deviation caused by accumulated error and other problems, and improves the robustness and reliability of the system.

[0159] As shown in Figure 2 The application also provides a fast semi-direct SLAM map construction device, which comprises:

[0160] An acquisition module 1 is configured to acquire a video collected by a camera, divide the video into a plurality of continuous video frame images, and calculate features of adjacent two video frame images to obtain feature matching points.

[0161] An estimation module 2 is configured to calculate 3D coordinates of map points and estimate a camera pose when the feature matching points reach a set value.

[0162] An extraction module 3 is configured to extract key points from the video frame images in a motion blur and a textureless environment, and perform feature enhancement on the key points based on subgraphs.

[0163] A projection module 4 is configured to project pixels of the video frame images to a three-dimensional space through a camera pose to obtain point cloud data, and perform preliminary processing on the point cloud data.

[0164] A fusion module 5 is configured to fuse and optimize the point cloud data after preliminary processing to obtain a 3D reconstruction map.

[0165] In one embodiment, the acquisition module 1 comprises:

[0166] An acquisition unit is configured to acquire a video collected by a camera, and divide the video into a plurality of continuous video frame images.

[0167] The extraction unit is configured to extract feature points in the video frame images using a FAST algorithm, and find matched pairs of feature points between two adjacent video frame images by using a feature descriptor and a similarity measurement method.

[0168] In one embodiment, in the estimation module 2, the calculating 3D coordinates of the map points comprises:

[0169] The RANSAC algorithm is used to calculate the homography matrix or the fundamental matrix, and the relative pose of the camera is recovered by decomposing the homography matrix or the fundamental matrix, and then the 3D coordinates of the map points are calculated by triangulation based on the matched pairs of feature points.

[0170] In one embodiment, in the estimation module 2, the camera pose estimation comprises estimating the re-projection error and the photometric error of the feature points.

[0171] The re-projection error calculation comprises:

[0172] For a spatial point P 1i , the pixel coordinates of which on a 2D image are p 1i , the re-projection error e of a single point is:

[0173]

[0174] The re-projection error E of all the feature points is:

[0175]

[0176] where T represents the camera pose to be estimated, K represents the internal parameters of the camera, Z 1i represents the coordinates of P 1i projected into the camera coordinate system relative to the Z axis, and N represents the total number of feature matches.

[0177] The photometric error of a single pixel point is defined as:

[0178]

[0179] The photometric error of multiple pixel points is defined as:

[0180]

[0181] where p 2j represents the 2D point coordinates in the previous frame, P 2j represents the spatial 3D coordinates of the point, Z 2j represents the coordinates of the spatial point P 2j projected into the camera coordinate system relative to the Z axis, I1() and I2() represent the photometric information of the corresponding pixels in the previous frame image and the current frame image respectively, and W represents the total number of pixels.

[0182] Solve the camera pose estimate value by combining the re-projection error of the matching feature points and the photometric error of the pixel pair:

[0183]

[0184] Where T * represents the camera pose estimate value.

[0185] In one embodiment, the extraction module 3 comprises:

[0186] In the video frame image of the motion blur and the non-texture environment, for the feature L in the i-th subgraph, the calculation formula of the enhanced 3D point is as follows:

[0187]

[0188] Where v j represents the 3D feature point in the j-th node, represents the coordinate transformation from the camera to the robot, H j represents the transformation expression of the j-th ranging, represents the inverse matrix of the distance transformation matrix, and v and m represent the first and last node indices of the subgraph, respectively.

[0189] In one embodiment, the projection module 4 comprises:

[0190] Reverse project each 2D image pixel into the three-dimensional space through the camera pose to obtain 3D point cloud data;

[0191] Take the surface element model as the minimum processing unit, for each surface element, store the position information (X, Y, Z), the vertex vector n, the color information (R, G, B), the surface element radius r, the weight w, and the point collection time information t; wherein any 3D structure in space can be composed of a surface element model, and any surface element M S is controlled by the nodes in the deformation mapping;

[0192] Construct a deformation graph composed of nodes and edges, wherein each node g n stores the position information g g , the rotation matrix g R , the translation matrix g t , and the time information I(M S , g) represents the surface deformation function; and when the node is optimized; M S represents the surface element of the 3D object. represents the element coordinates before surface deformation; The coordinates after the surface element deformation are represented by the following formula:

[0193]

[0194] The surface element normal vector becomes:

[0195]

[0196] The normal vector of the surface element before deformation is represented by: The normal vector of the surface element after deformation is represented by U, which represents the transformation relationship between the surface elements before and after deformation, and w n (M S ) represents the weight of the node pair on the surface element model, and the sum of the weights of the nodes that have an impact on the current surface is:

[0197]

[0198] where d max represents the peak value of the Euclidean distance between the surface element and all nodes in the neighborhood;

[0199] After the above processing, the 3D point cloud data without deformation is obtained.

[0200] In one embodiment, the fusion module 5 includes:

[0201] The two point cloud data after preliminary processing are obtained, and the corresponding points are extracted to establish constraints, and the constraint quantity Q P is:

[0202]

[0203] wherein, represents the position of the target point cloud, T S represents the pose of the current frame camera, T d represents the camera pose of the detected loop frame, represents the point obtained by projecting the model from the pose of the previous frame, represents the position of the frame point cloud, and represent the time stamps corresponding to and

[0204] The pose of each node is optimized using the Forbenius norm to ensure that the pose matrix is an orthogonal matrix, and the square error function is:

[0205]

[0206] wherein, l represents the node, represents the result of the node l after the pose transformation,​ represents that the node i has no result of pose transformation; C represents a transformation constant; F represents Forbenius norm;

[0207] The nodes are normalized, and the pose parameters of two adjacent nodes are uninterrupted, and the corresponding square error function is:

[0208]

[0209] wherein, represents a position, translation, and attitude related variable of the node; N(g') represents a set of adjacent nodes of the node;

[0210] The deformation coordinates of the above solutions and are combined and and the adjacent two frames of point clouds are aligned and optimized as the target point cloud, and the cost function is:

[0211]

[0212] The coordinates of the optimized target point cloud are optimized, and the square error function is:

[0213]

[0214] Finally, based on the above four cost function formulas E rot , E reg , E con , E pin , the final total optimization function is constructed to solve the deformation map, and the total optimization function is:

[0215] E def = w rot E rot + w reg E reg + w con E con + w pin E pin

[0216] wherein, the empirical values of the weights are: w rot = 1, w reg = 10, w con = w pin = 100;

[0217] After the deformation map of the point cloud is calculated, the data of the first point cloud is deformed and transformed into the same coordinate system as the second point cloud data, the fusion and optimization of the point cloud are realized through the feature points between the two data, and finally the 3D reconstruction map is obtained.

[0218] The modules and units are used to perform the steps of the fast semi-direct SLAM map construction algorithm, and the specific implementation manners are described in the method embodiments, which will not be described here.

[0219] As shown in Figure 3 The computer device can be a server, and the internal structure thereof can be as shown in Figure 3 The computer device includes a processor, a memory, a network interface and a database connected through a system bus. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store all data required by the process of the fast semi-direct SLAM map construction algorithm. The network interface of the computer device is used to communicate with an external terminal through a network connection. The computer program is executed by the processor to implement the fast semi-direct SLAM map construction algorithm.

[0220] Those skilled in the art can understand that Figure 3 The structure shown in the figure is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied.

[0221] An embodiment of the present application further provides a computer readable storage medium, which stores a computer program. The computer program is executed by a processor to implement any one of the fast semi-direct SLAM map construction algorithms.

[0222] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer readable storage medium, and when the computer program is executed, the processes of the above-mentioned embodiment methods can be included. Any reference to memory, storage, databases, or other media in this application and in examples provided herein, unless specifically stated otherwise, can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not limitation, RAM is available in many forms such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (SSRSDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0223] It should be noted that in this document, the terms "comprising", "including", or any other variant thereof are intended to cover a non-exclusive inclusion, such that a process, device, article, or method that comprises a list of elements does not only include those elements, but can also include other elements not expressly listed or inherent to such process, device, article, or method. Without more limitations, the element defined by the statement "comprising a" does not exclude the presence of additional identical elements in the process, device, article, or method that includes the element.

[0224] The above description is only the preferred embodiment of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation, or direct or indirect application in other related technical fields, based on the content of the specification and drawings of the present application, are also included in the patent protection scope of the present application.

Claims

1. A fast semi-direct SLAM map building algorithm, characterized in that, The method comprises the following steps: acquiring a video collected by a camera, and dividing the video into a plurality of continuous video frame images, and calculating features of two adjacent video frame images to obtain feature matching points; when the feature matching points reach a set value, calculating 3D coordinates of map points, and estimating camera pose; extracting key points from the video frame images in a motion blur and a non-texture environment, and enhancing features of the key points based on subgraphs; Specifically, the method comprises the following steps: In the video frame image in a motion-blurred and textureless environment, for feature L in the i-th sub-image, its enhanced 3D points The calculation formula is: where v j represents a 3D feature point in the jth node, represents a coordinate transformation from the camera to the robot, H j represents a transformation expression of the jth ranging, represents an inverse matrix of the distance transformation matrix, v and m represent the first and last node indices of the subgraph, respectively; projecting pixels of the video frame images to a three-dimensional space through camera pose to acquire point cloud data, and performing preliminary processing on the point cloud data; specifically, the method comprises the following steps: projecting each 2D image pixel to a three-dimensional space in a reverse direction through camera pose to acquire 3D point cloud data; With the surface element model as the minimum processing unit, for each surface element, the position information (X, Y, Z), the vertex vector n, the color information (R, G, B), the surface element radius r, the weight w and the point collection time information t are stored; wherein any 3D structure in space can be composed of a surface element model, and any surface element M S is controlled by a node in the deformation mapping; A morphing graph is constructed consisting of nodes and edges, where each node g n stores position information g g , a rotation matrix g R , a translation matrix g t , and time information I(M S , g) represents a surface morphing function; and when the nodes are optimized; M S represents a surface element of a 3D object; represents the element coordinates before surface morphing; represents the coordinates of the surface element after morphing, which is calculated as follows: the surface element normal vector becomes: a normal vector of a pre-deformed face element, a normal vector of a post-deformed face element, U represents a transformation relationship between the pre-deformed and post-deformed face elements, w n (M S ) represents a weight of a node pair on a face element model, and the sum of weights of nodes having an influence on a current face is: where d max denotes the peak value of the Euclidean distance of the surface element to all nodes within the neighborhood; After the above processing, 3D point cloud data with deformation removed is obtained; fusing and optimizing the point cloud data after the preliminary processing to obtain a 3D reconstruction map; specifically, the method comprises the following steps: obtain two of the point cloud data after preliminary processing, and extract corresponding points thereof to establish a constraint, a constraint quantity Q P is: wherein, represents the position of the target point cloud, T S represents the pose of the camera of the current frame, T d represents the camera pose of the frame in which the loop is detected, represents the points obtained by projecting the model from the pose of the previous frame, represents the position of the point cloud of the frame, and represents the position of the point cloud of the frame, and the corresponding timestamp; optimizing the pose of each node by using a Forbenius norm to ensure that the pose matrix is an orthogonal matrix, and the square error function is: wherein, l denotes a node, represents a result after pose transformation of the node l, represents a result without pose transformation of the node l; C denotes a transformation constant; F denotes a Forbenius norm; normalizing the nodes to ensure that the pose parameters of two adjacent nodes are uninterrupted, and the corresponding square error function is: wherein, N(g') denotes the set of neighboring nodes of node g; and N(g') denotes the set of neighboring nodes of node g; and Solve the above and deformation coordinates and Align and optimize the two adjacent frames of point clouds to the target point cloud, and the cost function is: optimizing the coordinates of the target point cloud, and the square error function is: Finally, based on the above four cost function formulas E rot , E reg , E con , E pin , the final total optimization function is constructed to solve the deformation map, and the total optimization function is: E def = w rot E rot + w reg E reg + w con E con + w pin E pin wherein the empirical values of the weights are: w rot = 1, w reg = 10, w con = w pin = 100; After the deformation map of the point cloud is calculated, the data of the first point cloud is deformed and transformed into the same coordinate system as the second point cloud data, the fusion and optimization of the point cloud are realized through the feature points between the two data, and finally the 3D reconstruction map is obtained.

2. The fast semi-direct SLAM map building algorithm according to claim 1, characterized in that, The step of acquiring a video collected by a camera, and dividing the video into a plurality of continuous video frame images, and calculating features of two adjacent video frame images to obtain feature matching points comprises the following steps: acquiring a video collected by a camera, and dividing the video into a plurality of continuous video frame images; extracting feature points in the video frame images by using a FAST algorithm, finding matched feature point pairs between two adjacent video frame images by using a feature descriptor and a similarity measurement method.

3. The fast semi-direct SLAM map building algorithm according to claim 1, wherein, In the step of calculating 3D coordinates of map points and estimating camera pose when the feature matching points reach a set value, the calculation of the 3D coordinates of the map points comprises the following steps: calculating a homography matrix or a fundamental matrix by using a RANSAC algorithm, restoring the relative pose of the camera by decomposing the homography matrix or the fundamental matrix, and then performing triangular partitioning on the matched feature point pairs to calculate the 3D coordinates of the map points.

4. The fast semi-direct SLAM map building algorithm of claim 1, wherein, In the step of calculating 3D coordinates of map points and estimating camera pose when the feature matching points reach a set value, the camera pose estimation comprises estimating the re-projection error and the photometric error of the feature points; the re-projection error calculation comprises the following steps: For a spatial point P 1i with pixel coordinates p on the 2D image 1i The re-projection error e for a single point is: the re-projection error E of all feature points is: where T represents the camera pose to be estimated, K represents the internal parameters of the camera, Z 1i represents P 1i the coordinates relative to the Z axis after projection to the camera coordinate system, and N represents the total number of feature matches; Photometric error of individual pixels is defined as: Photometric error of multiple pixels defined as: where p 2j represents the 2D point coordinate in the previous frame, P 2j represents the spatial 3D coordinate of the point, Z 2j represents the spatial point P 2j the coordinate relative to the Z axis after projection to the camera coordinate system, I1() and I2() represent the luminance information of the corresponding pixels in the previous frame image and the current frame image, respectively, and W represents the total number of pixels. by combining the re-projection error of the matched feature points and the photometric error of the pixel pairs, the camera pose estimation value is solved: where T * denotes the camera pose estimate.

5. A fast semi-direct SLAM map building device based on the fast semi-direct SLAM map building algorithm of any one of claims 1-4, characterized in that, The method comprises the following steps: an acquisition module is configured to acquire a video collected by a camera, and divide the video into a plurality of continuous video frame images, and calculate features of two adjacent video frame images to obtain feature matching points; an estimation module is configured to calculate 3D coordinates of map points and estimate camera pose when the feature matching points reach a set value. An extraction module is configured to perform key point extraction on the video frame images of motion blur and non-texture environment, and perform feature enhancement on the key points based on subgraphs; A projection module is configured to project pixels of the video frame images to a three-dimensional space to obtain point cloud data by camera pose, and perform preliminary processing on the point cloud data; A fusion module is configured to fuse and optimize the point cloud data after preliminary processing to obtain a 3D reconstruction map. 6.A computer device, comprising a memory and a processor, wherein the memory stores a computer program, and the computer device is configured to perform the method according to any one of claims 1-5 when the computer program is executed by the processor. The processor executes the computer program to implement the steps of the method in any one of claims 1 to 4.

7. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the steps of the method in any one of claims 1 to 4.

Images