A dynamic visual positioning mapping method and system based on multi-space information

Abstract

The application belongs to the field of computer vision and SLAM, and particularly relates to a dynamic visual positioning mapping method based on multi-space information, which comprises the following steps: collecting left and right video streams through a binocular camera, and performing gray processing on picture frames of the left and right video streams; extracting features of each picture frame in the left and right video streams, wherein the features of each picture frame comprise key point features, line segment features and plane features; calculating relative poses between the picture frames, and identifying key picture frames according to the relative poses between the picture frames, time intervals and feature matching; inserting the key picture frames into a local map, and establishing a local map; and optimizing the local map by minimizing re-projection errors between the picture frames to obtain an optimized local map. The application can improve positioning accuracy and mapping robustness of a robot in a dynamic environment, and enhance the perception ability and positioning accuracy of the system in the dynamic environment.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of computer vision and SLAM, and particularly relates to a dynamic visual positioning mapping method and system based on multi-space information. BACKGROUND

[0002] With the rapid development of robot technology, its application in various fields is becoming more and more widespread. In these applications, simultaneous localization and mapping (SLAM) and path planning systems play a crucial role. Robots often face complex and variable environments when performing tasks, such as urban, building groups or forest areas. Traditional positioning methods, especially those relying on global positioning system (GPS) technology, are often disturbed by signal shielding and multipath effects in these environments, resulting in decreased positioning accuracy. Visual SLAM technology has become a key in this context, aiming to estimate the motion of robots or autonomous vehicles in real time and construct an environment map, especially in unknown environments.

[0003] In the field of computer vision and SLAM, traditional visual positioning mapping methods are mostly based on the assumption of static scenes, relying on image frames captured by binocular cameras for feature extraction and matching, which is difficult to effectively deal with object motion interference in dynamic environments. Existing technologies usually only use key points (such as ORB features) for matching, lacking the collaborative use of geometric features such as lines and planes, resulting in insufficient stability of feature matching in dynamic scenes, and failing to combine asynchronous event data streams from event cameras with video streams from binocular cameras, making it impossible to accurately separate dynamic areas through event stream density analysis. In addition, traditional methods lack a collaborative optimization mechanism for visual features and inertial measurement information in multi-sensor fusion (such as vision + IMU) scenarios, and key frame selection does not consider multiple dimensions, resulting in low positioning accuracy and mapping robustness, making it difficult to meet the application requirements in complex dynamic environments. SUMMARY

[0004] To solve the problems in the background art and improve the positioning accuracy and mapping robustness of robots in dynamic environments, enhance the system's perception ability and positioning accuracy in dynamic environments, one aspect of the present application provides a dynamic visual positioning mapping method based on multi-space information, comprising:

[0005] S1: acquiring left and right video streams through a binocular camera, and performing grayscale processing on the picture frames of the left and right video streams;

[0006] S2: extracting features of each frame of picture in the left and right video streams, wherein the features of each frame of picture include key point features, line segment features and plane features;

[0007] S3: calculate the relative pose between picture frames, identify key picture frames according to the relative pose between picture frames, the time interval and the feature matching, insert the key picture frames into the local map, and establish a local map;

[0008] S4: optimize the local map to obtain an optimized local map by minimizing the re-projection error between frames.

[0009] Preferably, the feature extraction of each frame of picture in the left and right video streams comprises:

[0010] S21: extract initial key point features of each frame of picture in the left and right video streams by ORB algorithm, and extract static key points from the initial key points of each frame of picture as the final key point features according to the asynchronous event data stream collected by the event camera;

[0011] S22: extract line segment features of each frame of picture in the left and right video streams by LSD algorithm;

[0012] S23: extract left and right picture pairs from the left and right video streams collected by the binocular camera frame by frame, and generate point cloud data of the left and right picture pairs by stereo matching; and extract plane features of the left and right picture pairs by RANSAC algorithm according to the point cloud data of the left and right picture pairs.

[0013] Preferably, the step S3 comprises:

[0014] S31: identify a key point, line segment and plane feature matching set according to the key point, line segment and plane features of the current picture frame and the previous picture frame;

[0015] S32: estimate the relative pose between the current picture frame and the previous picture frame by epipolar geometry according to the key point, line segment and plane feature matching set;

[0016] S33: if the current picture frame meets any of the following determination conditions, the current picture frame is determined as a key picture frame, and the determination conditions are as follows:

[0017] Condition 1: the relative pose change between the current picture frame and the previous picture frame exceeds a set threshold;

[0018] Condition 2: the time difference between the current picture frame and the previous picture frame exceeds a preset time threshold;

[0019] Condition 3: the number of feature matches between the current picture frame and the previous picture frame is lower than a threshold;

[0020] S34: quantize the key point features extracted from the key picture frame, generate a BOW bag-of-words vector, and construct a co-view relationship graph according to the point, line segment and plane features commonly observed between the key picture frames, add the current key picture frame to the key picture frame queue to be inserted into the local map, and update the local map.

[0021] Another aspect of the present application provides a dynamic visual positioning mapping system based on multi-space information, comprising a memory and a processor; the memory is used to store an application program; the processor is used to run the application program to execute the dynamic visual positioning mapping method based on multi-space information.

[0022] Still another aspect of the present application provides a computer storage medium, which stores a program, and the program is executed by a processor to implement the dynamic visual positioning mapping method based on multi-space information.

[0023] The present application has at least the following beneficial effects

[0024] The present application can accurately identify dynamic objects and remove them in the mapping process by the dynamic region separation method based on event stream density analysis, optical flow field consistency detection and event accumulation graph, ensuring accurate modeling of the static background; the collaborative extraction and matching of multi-space features such as key points, line segments and planes are realized by combining the ORB algorithm, the LSD algorithm and stereo matching technology, enhancing the richness and robustness of feature information; the adaptive weight calculation mechanism is introduced to dynamically adjust the re-projection error optimization weight based on the number of features of the key picture frame, improving the overall error optimization precision of the system in the pure visual and visual+IMU scenes; in the visual+IMU scene, the inertial measurement information is effectively fused through the joint optimization of IMU data pre-integration and visual features, reducing the positioning drift in the scene with intense motion or sparse features. The method significantly improves the adaptability and stability of the system in complex dynamic environments, overcomes the low positioning accuracy and poor mapping robustness of traditional methods in dynamic scenes, and realizes high-precision simultaneous localization and mapping. BRIEF DESCRIPTION OF DRAWINGS

[0025] Figure 1 The present application is a method flowchart. DETAILED DESCRIPTION

[0026] Following, the advantages and effects of the present application can be easily understood by those skilled in the art from the description. The present application can also be implemented or applied by different specific embodiments, and the details in the description can be modified or changed based on different views and applications without departing from the spirit of the present application. It should be noted that the drawings provided in the following embodiments only illustrate the basic concept of the present application in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0027] Referring to Figure 1 The present application provides a dynamic visual positioning mapping method based on multi-space information, comprising:

[0028] S1: acquiring left and right video streams by a binocular camera, and performing gray scale processing on picture frames of the left and right video streams;

[0029] In the embodiment, the robot is equipped with a binocular camera. When the robot moves, the binocular camera acquires left and right video streams in real time, and performs gray scale processing on each picture frame in the left and right video streams. The formula is:

[0030] Gray(I) = 0.299R(I) + 0.587G(I) + 0.114B(I)

[0031] wherein Gray(I) represents a gray scale image of picture frame I, R(I), G(I) and B(I) respectively represent pictures of picture frame I in RGB three channels.

[0032] In the embodiment, the RGB three-channel color image is converted into a single-channel gray scale image, the data amount is reduced, the calculation complexity of subsequent feature extraction and pose calculation is reduced, and the real-time processing requirement in a dynamic scene is adapted. At the same time, the brightness and edge structure information of the image are retained through the gray scale conversion formula, high-quality input is provided for subsequent ORB algorithm to extract key points, LSD algorithm to extract line segment features, and stereo matching to generate point cloud data, and the accuracy and stability of feature extraction are ensured.

[0033] S2: extracting features of each picture frame in the left and right video streams, wherein the features of each picture frame include key point features, line segment features and plane features;

[0034] The extraction of the features of each picture frame in the left and right video streams includes:

[0035] S21: extracting initial key point features of each picture frame in the left and right video streams by an ORB algorithm, and extracting static key points from the initial key points of each picture frame as final key point features according to an asynchronous event data stream acquired by an event camera;

[0036] Preferably, the step S21 comprises:

[0037] S211: traversing each frame of picture in the left and right video stream, for each pixel point (x, y) of the picture, if there are 12 continuous pixels in the 16 pixels around the pixel point (x, y) whose pixel values are all greater than I(x, y)+20 or all less than I(x, y)+20, the pixel point (x, y) is determined as a candidate corner point, and a candidate corner point set is constructed, wherein I(x, y) represents the pixel value of the pixel point (x, y);

[0038] In the embodiment, the 16 pixels around each pixel point (x, y) of the picture refer to 16 pixels divided by a cross-shaped neighborhood, i.e., the current pixel is taken as the center, and 4 pixels are extended in each of the 4 directions of up, down, left and right to form a cross-shaped region, and there are 16 pixels in total.

[0039] S212: calculating the Harris response value of each candidate corner point for the candidate corner point set of the picture, and retaining the top 1000 corner points with the Harris response value as initial key points:

[0040] S213: calculating the main direction angle of the initial key points by the gray centroid method;

[0041] In the embodiment, the calculation of the main direction angle of the initial key points by the gray centroid method comprises the following steps: first, defining the domain of the initial key points, calculating the first moment and the zero moment for each pixel in the neighborhood, calculating the centroid coordinates in the domain range according to the first moment and the zero moment of the pixel, and obtaining the main direction angle of the initial key points according to the calculated centroid coordinates. Preferably, the Gaussian weight is applied to the pixel gray value when calculating the moment, so as to enhance the influence of the central region.

[0042] S214: in the local coordinate system rotated according to the main direction angle of the initial key points, randomly selecting 256 pixel points from the neighborhood of the initial key points, comparing the pixel value difference between the 256 pixel points and the initial key points to generate the binary BRIEF descriptor of the initial key points, and taking the binary BRIEF descriptor of the initial key points as the feature of the initial key points;

[0043] In the embodiment, the rotation according to the main direction angle of the initial key points is represented as rotating the image coordinate system around the origin by-θ angle, θ representing the main direction angle of the initial key points, to obtain the rotated local coordinate system. In the rotated local coordinate system, a square neighborhood with a size of s×s is defined, and the binary BRIEF descriptor of the initial key points is generated by comparing the pixel value difference between the 256 randomly selected pixel points and the initial key points:

[0044]

[0045] wherein, b irepresents the i-th binary BRIEF descriptor, I i represents the i-th selected pixel, I key represents the pixel of the initial key point, and the binary BRIEF descriptor of the initial key point is taken as the feature of the initial key point;

[0046] S215: An asynchronous event data stream of a picture is collected by an event camera, and a time surface response value of each initial key point is calculated according to the nearest event time of each initial key point in the picture:

[0047]

[0048] wherein S(x, y) represents the time surface response value of the pixel (x, y); e represents a natural base; t represents a current time, t last (x, y) represents the nearest event time of the pixel (x, y); τ is a time decay coefficient;

[0049] S216: An initial key point with a time surface response value greater than or equal to a set threshold is marked as a dynamic key point, and otherwise, it is marked as a static key point.

[0050] In the embodiment, step S21 realizes accurate extraction of static key points in a dynamic scene through the cooperation of the ORB algorithm and the event camera data: candidate corner points are first screened through the traversal of pixel points, key corner points are reserved by calculating the Harris response value, the main direction angle is determined by combining the gray centroid method, and a BRIEF descriptor is generated, thereby constructing an initial key point set with direction invariance and feature discrimination; then, an asynchronous event data stream collected by an event camera is used to calculate the dynamic degree through a time surface response value, and a key point with a response value lower than a threshold is marked as static, thereby effectively eliminating key points on dynamic objects (such as moving pedestrians and vehicles). This process not only ensures the efficiency and scale invariance of key point extraction by using the ORB algorithm, but also realizes adaptive screening of static features through the dynamic perception ability of the event camera, thereby providing a stable point feature basis for subsequent inter-frame feature matching, significantly reducing the feature mismatch probability in a dynamic environment, and improving the reliability of pose estimation and map construction.

[0051] S22: Line segment features of each frame of pictures in left and right video streams are extracted through an LSD algorithm;

[0052] Preferably, the step S22 comprises:

[0053] S221: Horizontal gradients and vertical gradients of each pixel point in a picture are calculated through a Sobel operator;

[0054] S222: Gradient amplitudes and gradient directions of each pixel point in a picture are calculated according to the horizontal gradients and vertical gradients of each pixel point in the picture.

[0055] S223: According to the gradient direction of each pixel point in the picture, the pixel points with gradient amplitude greater than 0 are assigned to multiple intervals to form multiple direction groups;

[0056] S224: In each direction group, the pixel points are arranged in descending order of gradient amplitude, and the pixel point with the largest gradient amplitude is selected as the initial seed point;

[0057] S225: The seed point is added to the line segment, and the seed point is marked as used; the region angle is initialized as the gradient direction of the seed point; the 8 neighborhood pixels of the seed point are traversed, if there is a field pixel in the unused state, and the absolute difference between the gradient direction of the field pixel and the region angle is within the preset error range, then the field pixel is added to the line segment and marked as used;

[0058] S226: The region angle is updated according to the horizontal gradient and the vertical gradient of the pixel points in the line segment;

[0059] S227: Repeat the search for the pixel points newly added to the line segment until there is no neighborhood pixel meeting the condition, and the line segment is obtained;

[0060] S228: From the pixel points in the direction group that are not marked, the pixel point with the largest gradient amplitude is selected as the seed point again; repeat the above steps until all the pixel points in the direction group are in the used state, output all the line segments found, and calculate the weighted centroid and the end point of the line segment as the line segment feature of the picture.

[0061] In the step S22 of the embodiment, the line segment features are extracted by the improved LSD algorithm, which can accurately identify different parallel line segments and line segments on the same horizontal line, extract more abundant and accurate line segment features, and delete the line segments with pixel points lower than the set threshold as noise. In addition, the embodiment calculates the pixel gradient by means of the Sobel operator to identify the edge region, groups according to the gradient direction and selects the seed points based on the gradient amplitude ranking, constructs the continuous line segment by the neighborhood expansion, and finally represents the line segment features by the weighted barycenter and the end points. The process effectively captures the linear structure (such as the wall edge and the object contour) in the scene, and provides the structured geometric constraint for the dynamic visual positioning and mapping: on the one hand, the gradient amplitude ranking and the direction grouping mechanism can filter out the noise interference, ensure the high accuracy and robustness of the extracted line segment features, and reduce the false extraction of the dynamic object edge; on the other hand, the line segment features, the key points and the plane features are complementary, the matching accuracy is improved by the double constraints of the end point coordinates and the direction vector in the feature matching, additional geometric constraints (such as the line segment epipolar constraint) are provided for the inter-frame relative pose calculation, and the line segment features are used as an independent error term in the local map optimization to participate in the re-projection error calculation, thereby enhancing the stability of the map structure. Especially in the structured scene, the system can significantly improve the representation ability of the environment structure and reduce the feature matching ambiguity under the dynamic interference.

[0062] S23: Extracting left and right image pairs from the left and right video streams collected by the binocular camera frame by frame, and generating point cloud data of the left and right image pairs by stereo matching; extracting plane features of the left and right image pairs by using the RANSAC algorithm according to the point cloud data of the left and right image pairs;

[0063] Preferably, the extraction of the plane features of the left and right image pairs by using the RANSAC algorithm comprises:

[0064] S231: Randomly sampling 3 points from the point cloud data of the left and right image pairs, and fitting a plane based on the three sampled points;

[0065] S232: Calculating the distance from each point in the point cloud to the plane, and determining that the point belongs to the inlier of the plane if the distance is less than a set threshold;

[0066] S233: Repeating the steps S31-S33 until a preset iteration number is reached, and taking the plane with the most inliers as the plane feature of the image pair.

[0067] In this embodiment, step S2 provides multi-dimensional scene representation capability for dynamic visual positioning mapping by cooperatively extracting key points, line segments and plane features: through the ORB algorithm combined with the asynchronous event data stream of the event camera, static key points can be accurately selected from the initial key points, effectively eliminating the interference of dynamic objects, ensuring the stability of the feature points; the line segment features extracted by the LSD algorithm can capture the linear structure (such as edges and contours) in the scene, making up for the insufficient description capability of single key points in structured scenes; the plane features (such as walls and floors) extracted by stereo matching and RANSAC algorithm strengthen the geometric structure information of the scene, and the three form a cooperative feature system of points, lines and planes. This feature extraction mechanism not only improves the robustness of feature matching in complex dynamic environments, but also provides multiple constraints (such as epipolar geometry and plane projection relationship) for subsequent inter-frame pose calculation, lays a data foundation for key frame selection and local map optimization, enables the system to more accurately identify the environment structure and suppress dynamic interference, thereby improving the accuracy and stability of positioning and mapping.

[0068] S3: calculating the relative poses between the picture frames, and identifying key picture frames according to the relative poses between the picture frames, the time intervals and the feature matching, and inserting the key picture frames into the local map to establish a local map;

[0069] Preferably, the step S3 comprises:

[0070] S31: identifying a key point, line segment and plane feature matching set according to the feature matching of the key points, line segments and plane features of the current picture frame and the previous picture frame;

[0071] The matching mode calculates the Hamming distance through the BRIEF descriptor of the key points, for example, the key point feature set of the current frame F i is defined as K i , the key point feature set of the previous frame F i-1 is defined as K i-1 , and the Hamming distance is calculated for the key point feature sets of the current frame F i and the previous frame F i-1 , and the matching pairs with a distance less than a threshold value are retained to obtain the key point feature matching set.

[0072] Line segment feature matching: the matching mode is performed by comparing the end point coordinates and direction vectors of the line segments. The Euclidean distance of the end point coordinates and the included angle of the direction vectors are calculated, and the end point distance threshold is set to 10 pixels and the direction included angle threshold is set to 10°.

[0073] Plane feature matching: the matching mode is performed by calculating the normal vector of each plane and the distance of the plane to the origin, and the normal vector included angle threshold is set to 15° and the distance difference threshold is set to 0.2 meters.

[0074] S32: estimating the relative pose between the current picture frame and the previous picture frame according to the key point, line segment and plane feature matching set by using epipolar geometry estimation;

[0075] In the embodiment, the essential matrix is calculated by using an eight-point method or the like for the key point, line segment and plane feature matching set, and the relative pose between the current picture frame and the previous picture frame is obtained by singular value decomposition.

[0076] S33: if the current picture frame satisfies any of the following determination conditions, the current picture frame is determined as a key picture frame, and the determination conditions are as follows:

[0077] Condition 1: the relative pose change between the current picture frame and the previous picture frame exceeds a set threshold value;

[0078] Condition 2: the time difference between the current picture frame and the previous picture frame exceeds a preset time threshold value;

[0079] Condition 3: the number of feature matches between the current picture frame and the previous picture frame is lower than a threshold value;

[0080] S34: quantizing the key point features extracted from the key picture frame to generate a BOW bag-of-words vector; and constructing a co-view relationship graph according to the point, line segment and plane features commonly observed between the key picture frames, adding the current key picture frame to a key picture frame queue to be inserted into the local map, and updating the local map.

[0081] In the embodiment, the construction of the co-view relationship graph includes:

[0082] viewing the features commonly observed between the key picture frame and the existing key picture frames, and determining the weight of the co-view relationship according to the number of the commonly observed features, for example, setting the key point weight as 1, the line segment weight as 2, and the plane weight as 5, and the co-view relationship weight between the key picture frame and the existing key picture frames as 1xA1+2xA2+5xA3; A1, A2 and A3 are respectively the number of the point, line and plane features commonly observed between the key picture frame and the existing key picture frames.

[0083] After the above operations, the update of the local map is completed, and the local map contains the pose information of the key picture frame, the BOW bag-of-words vector and the co-view relationship between the key picture frames and the like.

[0084] In the embodiment, step S3 realizes efficient screening and structured storage of environment information in a dynamic scene by constructing a local map through calculating the relative pose between picture frames and identifying key picture frames: first, based on the point-line-surface feature matching set of the current frame and the previous frame, the relative pose is solved by using epipolar geometry estimation to provide accurate geometric constraints for the inter-frame motion relationship; second, the key frame is determined by the three conditions of relative pose change, time interval and feature matching number, which not only avoids the calculation burden caused by the insertion of redundant frames, but also prevents the omission of key information, ensuring the real-time and effectiveness of map updating; finally, the key frame key point feature is quantized to generate a BOW bag-of-words vector, and a local map is constructed by combining the co-view relationship graph, which not only realizes the structured storage of key frame pose information and feature association, but also strengthens the geometric association between frames in the map through the design of co-view weight. This process enables the local map to dynamically adapt to scene changes, providing a compact and effective data basis for subsequent map optimization based on reprojection error, and laying a feature matching and pose association underlying framework for similarity search in loop closure detection and global map construction, significantly improving the positioning and mapping efficiency and robustness of the system in a dynamic environment.

[0085] S4: optimizing the local map by minimizing the reprojection error between frames to obtain an optimized local map;

[0086] In a pure visual scene, the optimizing the local map by minimizing the reprojection error between frames comprises:

[0087] S401: calculating the adaptive weight of the key picture frame according to the feature of the key picture frame:

[0088]

[0089] In the formula, ω adp represents the adaptive weight, ω ba is the basic weight set as 0.33, F n represents the number of key point features extracted by the key picture frame, P n represents the number of line segment features extracted by the key picture frame, N is the key point feature number threshold set as 300, k is a self-set parameter set as 0.005, and exp represents the exponential function.

[0090] S402: optimizing the pose of the key picture frame by minimizing the reprojection error between frames to obtain an optimized local map, wherein the minimizing the reprojection error between frames comprises:

[0091]

[0092] In the formula, ψ * represents the minimized reprojection error between frames, and ψ represents a set of keypoint features in a key picture frame, represents a set of line features in a key picture frame;e ij represents a re-projection error matrix of keypoint feature j in key picture frame i; represents a matrix transpose; represents an inverse matrix of the covariance weight matrix of keypoint feature j in key picture frame i; represents a set of line features in a key picture frame;e ik represents a re-projection error matrix of line segment feature k in key picture frame i; represents an inverse matrix of the covariance weight matrix of line segment feature k in key picture frame i;P p represents a set of plane features in a key picture frame;e ip represents a re-projection error matrix of plane feature p in key picture frame i, represents an inverse matrix of the covariance weight matrix of plane feature p in key picture frame i.

[0093] In this embodiment, steps S401-S402 realize dynamic and accurate optimization of the local map in a pure visual scene through adaptive weight calculation and multi-feature joint optimization: S401 dynamically calculates adaptive weights through an exponential function based on the number of static key points and the number of line segment features extracted from the key picture frame, so that key frames with rich features obtain higher weights in optimization, effectively suppressing the error contribution of frames with sparse or dynamic interference features; S402 couples the re-projection errors of point, line and plane features with adaptive weights, constructs a multi-dimensional error optimization model, and iteratively optimizes the key frame pose by minimizing the weighted error. This process not only dynamically adjusts the optimization weight based on the number of key points, avoiding the misfit of traditional fixed weight to dynamic features, but also improves the geometric consistency of pose estimation through multi-feature collaborative constraint, so that the local map can adaptively filter dynamic interference in a pure visual scene, strengthen the constraint effect of static structural features, and finally realize the dual improvement of map accuracy and robustness. Especially in scenes with uneven feature distribution or dynamic objects, the cumulative re-projection error can be significantly reduced to ensure the modeling accuracy and scene adaptability of the local map.

[0094] In the visual+IMU scene, the local map is optimized by minimizing the re-projection error between frames, including:

[0095] S411: Calculate the adaptive weight of the key picture frame according to the features of the key picture frame:

[0096]

[0097] where ω ba is the basic weight, F n represents the number of extracted static key point features, P nN is the number of point features, and k is a self-set parameter.

[0098] S412: Pre-integrating the IMU data of the current key picture frame and the previous key picture frame to obtain a relative pose increment of the current key picture frame and the previous key picture frame;

[0099] S413: Calculating a rotation residual, a velocity residual and a position residual between the current key picture frame and the previous key picture frame according to the relative pose increment of the current key picture frame and the previous key picture frame; wherein the rotation residual, the velocity residual and the position residual between the current key picture frame and the previous key picture frame include:

[0100]

[0101] wherein, r i,i+1 represents the IMU pre-integration residual from the i-th frame to the i+1-th frame, including a residual vector of the rotation, the velocity and the position, represents a rotation residual vector from the i-th frame to the i+1-th frame, represents a velocity residual vector from the i-th frame to the i+1-th frame, represents a position residual vector from the i-th frame to the i+1-th frame, ΔR i,i+1 , Δv i,i+1 and Δp i,i+1 respectively represent the rotation, the velocity and the position matrix in the relative pose of the current frame i+1 and the previous key picture frame i; R i represents the rotation matrix of the previous key picture frame i, R i+1 represents the rotation matrix of the i+1-th frame, v i+1 represents the velocity matrix of the i+1-th frame, v i represents the velocity matrix of the i-th frame, log() represents the logarithmic mapping from Lie group SO(3) to Lie algebra so(3), Δt i,i+1 represents the time from the i-th frame to the i+1-th frame, Δv i,i+1 represents the velocity matrix from the i-th frame to the i+1-th frame, p j represents the position matrix of the j-th frame, v represents the linear velocity at the i-th frame, p i represents the position coordinates of the i-th frame, Δt represents the change time, and g is the gravitational acceleration.

[0102] S414: Optimizing the local map according to the rotation residual, the velocity residual and the position residual of the current key picture frame and the previous key picture frame to minimize the re-projection error between the frames, to obtain an optimized local map, wherein the minimizing the re-projection error between the frames includes:

[0103]

[0104] wherein, ξ * denotes the minimized reprojection error between frames, r i,i+1 denotes the IMU pre-integration residual between the i-th frame (current keyframe) and the i+1-th frame (previous keyframe), including the collective residual vector of rotation, velocity and position, denotes the set of keyframe, denotes the set of keypoint features in the keyframe, e ij denotes the reprojection error matrix of the keypoint feature j in the keyframe i; denotes the matrix transpose; denotes the inverse matrix of the covariance weight matrix of the keypoint feature j in the keyframe i; denotes the set of line features in the keyframe;e ik denotes the reprojection error matrix of the line feature k in the keyframe i; denotes the inverse matrix of the covariance weight matrix of the line feature k in the keyframe i;P p denotes the set of plane features in the keyframe;e ip denotes the reprojection error matrix of the plane feature p in the keyframe i, denotes the inverse matrix of the covariance weight matrix of the plane feature p in the keyframe i.

[0105] In the embodiment, steps S411-S414 achieve high-precision calibration of the local map in a dynamic scene through deep fusion of visual and IMU data and adaptive optimization strategy: S411 calculates adaptive weights based on the number of static keypoint and line features of the keyframe, so that the frames with rich features dominate in the optimization, effectively weakening the error influence of dynamic interference frames; S412 obtains the relative pose increment of adjacent keyframes through pre-integration of IMU data, converts the inertial measurement information into an optimizable constraint term, and makes up for the tracking defects of the pure visual method in severe motion; S413 further decomposes the pre-integration result into rotation, velocity and position residuals, accurately quantifying the difference between IMU measurement and visual pose estimation; S414 couples the above residuals with the reprojection errors of point, line and plane features, constructs a joint optimization model with multiple source constraints, and synchronously corrects the keyframe pose and map structure by minimizing the comprehensive error. This process not only utilizes the high-frequency inertial measurement characteristics of the IMU to suppress the cumulative drift of visual positioning, but also dynamically balances the reliability of visual features through the adaptive weight mechanism, so that the system can still maintain positioning accuracy in sparse feature or dynamic interference scenes, especially when the robot moves quickly or the environment light changes suddenly, which can significantly improve the stability and scene adaptability of the local map, laying a precise local coordinate foundation for global multi-map optimization.

[0106] After the local map is optimized by step S4 in the embodiment, loop closure detection and real-time localization in SLAM (step S5) and global multi-map optimization (step S6) can also be performed, and the optimization process is as follows:

[0107] S5: Loop closure detection and real-time localization are performed on the local map optimized in step S4, and the local map is reconstructed;

[0108] Preferably, the loop closure detection and real-time localization on the optimized local map comprise:

[0109] S51: For the current key picture frame, the Hamming distance between its BoW vector and the BoW vectors of all historical key picture frames is calculated, and the historical key picture frames with a similarity higher than a threshold value are selected as loop closure candidate frames;

[0110] S52: For the loop closure candidate frames, a basis matrix is estimated using the RANSAC algorithm according to the key point features of the current key picture frame and the loop closure candidate frames, the number of inliers satisfying the epipolar constraint is counted, and the mean value of the re-projection error of the inliers is calculated;

[0111] S53: The loop closure candidate frames whose number of inliers exceeds a set threshold value, whose mean value of the re-projection error of the inliers is less than a set threshold value, whose time interval from the current key picture frame is greater than a set threshold value, and whose number of co-visible key picture frames from the current key picture frame is greater than a set threshold value are selected as loop closure frames;

[0112] S54: The relative pose between the current key picture frame and the loop closure frame is calculated according to the inliers of the current key picture frame and the loop closure frame;

[0113] S55: All key picture frames are regarded as graph nodes, and edge constraints include inter-frame constraints, inertial constraints and loop closure constraints, and an optimization objective is set: wherein e visual , e imu and e loop correspond to visual re-projection error, IMU residual error and loop closure constraint error, respectively.

[0114] In the embodiment, the IMU residual refers to the difference between the pose increment obtained by IMU pre-integration and the pose increment obtained according to the current key picture frame pose estimation. In actual application, IMU measurement has noise and drift, and the pre-integration process also introduces certain error, so it is necessary to measure the consistency between IMU data and visual positioning results by calculating the residual, so as to correct the key picture frame pose in global graph optimization. The loop constraint error is used to measure the difference between the relative pose constraint between two key picture frames determined in loop closure detection and the relative pose obtained according to the current key picture frame pose estimation. When a loop is detected, it is considered that the two frames should satisfy a certain relative pose relationship, and the actual pose estimation may not be consistent with the constraint due to cumulative error and other reasons. This difference is the loop constraint error. By minimizing the loop constraint error, the cumulative drift of the map can be corrected, so that the global map can accurately connect at the loop. The visual re-projection error is a key indicator for measuring the consistency between three-dimensional map points and two-dimensional image observations in SLAM (simultaneous localization and mapping). The core idea is to project the three-dimensional map points to the image plane through the current estimated camera pose, and compare them with the actually observed feature point positions. The pixel-level deviation between the two is the re-projection error.

[0115] S56: using Levenberg-Marquardt algorithm or g2o / GraphSLAM optimization library, minimizing the optimization objective, updating the pose estimation of the key picture frame;

[0116] S57: synchronizing the optimized key picture frame pose to the local map, updating the three-dimensional coordinates of the key points in the map;

[0117] S58: re-triangulating the key points according to the optimized key picture frame pose, updating the three-dimensional coordinates;

[0118] S59: calculating the change amount of the optimized key picture frame pose, if the change amount of the optimized key picture frame pose exceeds the threshold, re-finding the matching key points between the key picture frame and the adjacent key picture frame for the key points of the key picture frame, updating the corresponding relationship between them, and deleting the key points whose re-projection error is greater than the threshold.

[0119] In this embodiment, step S5 realizes effective suppression of global map cumulative error and optimization of map structure by loop closure detection and real-time positioning on the optimized local map: first, similarity search is performed on the BoW vector of the current key frame and historical key frames to screen out loop closure candidate frames, the basis matrix is estimated by the RANSAC algorithm, and the loop closure reliability is verified by multiple conditions such as the number of inliers and the re-projection error to avoid false loop closure caused by dynamic object interference; then the key frame is regarded as a graph node, an optimization target containing inter-frame constraints, inertial constraints and loop closure constraints is constructed, and the Levenberg-Marquardt algorithm or an optimization library is used to minimize the comprehensive error to update the key frame pose and the three-dimensional coordinates of the map points; finally, the map structure is further optimized and the feature points with error exceeding the limit are deleted through the pose change threshold judgment and key point re-triangulation. This process not only corrects the positioning drift in long-distance motion through loop closure constraints, but also improves the geometric consistency of the global map through multi-source constraint joint optimization, so that the system can continuously maintain high-precision positioning and mapping capability in complex dynamic environments, especially for map reconstruction and error elimination in large-scale scenes.

[0120] S6: Global multi-map optimization is performed on all reconstructed local maps to obtain a constructed final map.

[0121] Preferably, the global multi-map optimization on all reconstructed local maps comprises:

[0122] S61: All local maps are traversed to detect common key image frames or shared features between the local maps;

[0123] S62: Each local map is regarded as a graph node, and if there are common key image frames or shared features (including point, line and surface features, and the matching process is as described above) between the two maps, a connection edge is added between the nodes, and the weight of the edge represents the number of feature matches;

[0124] S63: Similarity search is performed on the BoW vector or binary BRIEF descriptor between the key image frames of different local maps to screen out cross-map frame pairs that may correspond to the same physical area;

[0125] S64: For the candidate frame pairs, the basis matrix or relative pose is estimated by the RANSAC algorithm, and the number of co-visible features and the re-projection error constraint condition are verified;

[0126] S65: Only when the frame pair passes the geometric verification and the number of shared features exceeds the threshold, it is determined as a cross-map loop closure edge and added to the global graph;

[0127] S66: Iterative optimization is performed using a global graph optimization library to update the poses of all key image frames and the relative poses between the maps, and to minimize the global cumulative error;

[0128] S67: According to the global optimization result, the pose of the key picture frame in each local map is updated to ensure that the poses of the overlapping areas of adjacent maps are consistent; if the overlapping area poses of two local maps are higher than a threshold, the repeated key picture frames and feature points are merged and only one is kept.

[0129] In the step S6 of the embodiment, the consistency of the global map in the complex scene is realized and the cumulative error is eliminated by global multi-map optimization on the reconstructed local map: the common key frame or shared point-line-surface feature is detected by traversing each local map, the local map is taken as a graph node and the connection edge weight is set according to the number of feature matching, the similar frame pairs across maps are retrieved by BoW vector or BRIEF descriptor, the geometric consistency of the frame pairs is verified by RANSAC algorithm and the effective closed-loop edge is screened, and finally the global graph optimization library is used to iteratively optimize all key frame poses and relative poses between maps, so that the poses of the overlapping areas of each local map are unified. The process not only strengthens the structural constraint of the global scene through multi-map feature association, but also corrects the cumulative drift when the local map is independently optimized through cross-map closed-loop detection. The merging operation of the repeated feature points and key frames in the overlapping area further improves the compactness and accuracy of the map, and finally a global map covering a large range of scenes and having geometric consistency is constructed, which significantly enhances the long-distance positioning and mapping capability of the system in a complex dynamic environment.

[0130] Another aspect of the present application provides a dynamic visual positioning and mapping system based on multi-space information, which comprises a memory and a processor; the memory is used to store an application program; the processor is used to run the application program to execute the dynamic visual positioning and mapping method based on multi-space information.

[0131] Still another aspect of the present application provides a computer storage medium, which stores a program, and the program is executed by a processor to realize the dynamic visual positioning and mapping method based on multi-space information.

[0132] 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 executed, can include the processes of the above-mentioned embodiment methods. Any reference to memory, storage, database or other medium used in the embodiments provided in the present application 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 various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), 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.

[0133] In summary, the present application can accurately identify dynamic objects and remove them in the mapping process by the dynamic region separation method based on event stream density analysis, optical flow field consistency detection and event accumulation map, ensuring accurate modeling of the static background. The present application can realize collaborative extraction and matching of key points, line segments, planes and other multi-space features by combining ORB algorithm, LSD algorithm and stereo matching technology, enhancing the richness and robustness of feature information. By introducing an adaptive weight calculation mechanism, the present application dynamically adjusts the re-projection error optimization weight based on the number of features of key picture frames, improving the overall error optimization precision of the system in the pure vision and vision+IMU scenes. In the vision+IMU scene, the present application effectively fuses inertial measurement information through joint optimization of IMU data pre-integration and visual features, reducing the positioning drift in scenes with intense motion or sparse features. In addition, the present application can combine loop detection and global multi-map optimization mechanism to effectively suppress cumulative errors, ensuring the consistency and accuracy of the global map. The present application significantly improves the adaptability and stability of the system in complex dynamic environments, overcomes the low positioning accuracy and poor mapping robustness of traditional methods in dynamic scenes, and realizes high-precision simultaneous localization and mapping.

[0134] Finally, it is to be explained that the above embodiments are only used to illustrate the technical solutions of the present application but not to limit the present application. Although the present application is described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solutions of the present application can be modified or equivalently replaced without departing from the purpose and scope of the technical solutions, and all should be covered in the scope of the claims of the present application.

Claims

1. A method for dynamic visual positioning mapping based on multi-space information, characterized in that, The method comprises the following steps: S1: acquiring left and right video streams by a binocular camera, and performing gray processing on picture frames of the left and right video streams; S2: extracting features of each picture frame in the left and right video streams, wherein the features of each picture frame comprise key point features, line segment features and plane features; S3: calculating relative poses between picture frames, and identifying key picture frames according to the relative poses between the picture frames, time intervals and feature matching, inserting the key picture frames into a local map, and establishing a local map; S4: optimizing the local map by minimizing the re-projection error between frames to obtain an optimized local map; In a vision+IMU scene, the optimization of the local map by minimizing the re-projection error between frames comprises: S411: calculating adaptive weights of the key picture frames according to the features of the key picture frames; In the formula, is a basic weight, represents the number of extracted static key point features, represents the number of extracted line segment features, is a point feature number threshold, is a self-set parameter; S412: pre-integrating IMU data of a current key picture frame and a previous key picture frame to obtain a relative pose increment between the current key picture frame and the previous key picture frame; S413: calculating rotation residual, velocity residual and position residual between the current key picture frame and the previous key picture frame according to the relative pose increment between the current key picture frame and the previous key picture frame; S414: optimizing the local map by minimizing the re-projection error between frames according to the rotation residual, the velocity residual and the position residual between the current key picture frame and the previous key picture frame, to obtain an optimized local map; In a vision+IMU scene, the minimization of the re-projection error between frames comprises: wherein, represents the minimized inter-frame reprojection error, represents the IMU pre-integration residual from the i-th frame to the i+1-th frame, including the collective residual vector of rotation, velocity and position three parts, represents a set of key picture frames, represents a set of key point features in the key picture frames, represents a key picture frame key point features in the key picture frame; represents the matrix transpose; represents a covariance weight matrix of the key point features in the key picture frame; represents the inverse matrix of the covariance weight matrix of the key point features represents a set of line features in the key picture frames; represents a reprojection error matrix of the line segment features in the key picture frame ; represents the inverse matrix of the covariance weight matrix of the line segment features in the key picture frame ; represents a set of face features in the key picture frames; represents a reprojection error matrix of the plane features in the key picture frame ; represents the inverse matrix of the covariance weight matrix of the plane features p in the key picture frame.

2. The method of claim 1, wherein, The extraction of the features of each picture frame in the left and right video streams comprises: S21: extracting initial key point features of each picture frame in the left and right video streams by an ORB algorithm, and extracting static key points from the initial key points of each picture frame as final key point features according to an asynchronous event data stream collected by an event camera; S22: extracting line segment features of each picture frame in the left and right video streams by an LSD algorithm; S23: extracting left and right picture pairs from the left and right video streams collected by the binocular camera frame by frame, and generating point cloud data of the left and right picture pairs by stereo matching; and extracting plane features of the left and right picture pairs by a RANSAC algorithm according to the point cloud data of the left and right picture pairs.

3. The method of claim 1, wherein, The step S3 comprises: S31: identifying a key point, line segment and plane feature matching set according to the key point, line segment and plane features of a current picture frame and a previous picture frame; S32: solving a relative pose between the current picture frame and the previous picture frame by epipolar geometry estimation according to the key point, line segment and plane feature matching set; S33: if the current picture frame meets any of the following determination conditions, the current picture frame is determined as a key picture frame, and the determination conditions are as follows: Condition 1: a relative pose change amount of the current picture frame and the previous picture frame exceeds a set threshold value; Condition 2: a time difference between the current picture frame and the previous picture frame exceeds a preset time threshold value; Condition 3: a feature matching number of the current picture frame and the previous picture frame is lower than a threshold value. S34: quantizing the key point features extracted from the key picture frame to generate a BOW bag-of-words vector; and constructing a co-view relationship graph according to the point, line segment and plane features commonly observed between the key picture frames, adding the current key picture frame to a queue of key picture frames to be inserted into the local map, and updating the local map.

4. The method of claim 3, wherein, In the pure vision scenario, the optimizing the local map by minimizing the reprojection error between frames comprises: S401: calculating an adaptive weight of the key picture frame according to the features of the key picture frame: In the formula, is a basic weight, represents the number of extracted static key point features, represents the number of extracted line segment features, is a point feature number threshold, is a self-set parameter; S402: optimizing the pose of the key picture frame by minimizing the reprojection error between frames to obtain an optimized local map, wherein the minimizing the reprojection error between frames comprises: In the formula, This represents minimizing the inter-frame reprojection error. This represents the inter-frame reprojection error. Represents the set of key image frames. This represents the set of key point features in a key image frame. Represents keyframe Key features The reprojection error matrix; Indicates matrix transpose; Represents keyframe Key features The inverse of the covariance weight matrix; Represents the set of features of the midline of key image frames; Represents keyframe Midline segment characteristics The reprojection error matrix; Represents keyframe Midline segment characteristics The inverse of the covariance weight matrix; Represents the set of surface features in a key image frame; Represents keyframe Midplane features The reprojection error matrix, Represents keyframe The inverse of the covariance weight matrix of the midplane feature p.

5. The method of claim 1, wherein, the rotation residual, the velocity residual and the position residual between the current key picture frame and the previous key picture frame comprise: wherein, denotes the IMU pre-integration residual from the i-th frame to the i+1-th frame, including the collective residual vector of the rotation, velocity and position three parts, denotes the rotation residual vector from the i-th frame to the i+1-th frame, denotes the velocity residual vector from the i-th frame to the i+1-th frame, denotes the position residual vector from the i-th frame to the i+1-th frame, and denote the rotation, velocity and position matrices in the relative pose of the current frame i+1 and the previous key picture frame i, respectively; denotes the rotation matrix of the previous key picture frame i, denotes the rotation matrix of the i+1-th frame, denotes the velocity matrix of the i+1-th frame, denotes the velocity matrix of the i-th frame, denotes the logarithmic mapping from Lie group SO(3) to Lie algebra so(3), denotes the time from the i-th frame to the i+1-th frame, denotes the velocity matrix from the i-th frame to the i+1-th frame, denotes the position matrix of the j-th frame, denotes the linear velocity at the i-th frame, denotes the position coordinates of the i-th frame, denotes the change time, and g is the gravity acceleration.​ 6. A multi-space information based dynamic visual positioning mapping system, characterized in that, The system comprises a memory and a processor; the memory is used to store an application program; the processor is used to run the application program to execute a kind of dynamic visual positioning mapping method based on multiple space information as claimed in any one of claims 1 to 5.

7. A computer storage medium, characterized in that The computer storage medium stores a program, and the program is executed by a processor to implement any one of the dynamic visual positioning mapping methods based on multiple space information in claims 1 to 5. The computer storage medium stores a program, and the program is executed by a processor to implement any one of the dynamic visual positioning mapping methods based on multiple space information in claims 1 to 5.

Images