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

Abstract

The invention belongs to the field of computer vision and SLAM (simultaneous localization and mapping), and particularly relates to a dynamic vision localization mapping method based on multi-spatial information, which comprises the following steps: collecting left and right eye video streams through a binocular camera, and carrying out gray processing on picture frames of the left and right eye video streams; features of each frame of picture in the left-eye and right-eye video streams are extracted, and the features of each frame of picture comprise key point features, line segment features and plane features; calculating relative poses among the picture frames, identifying key picture frames according to the relative poses among 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 the inter-frame re-projection error to obtain an optimized local map. According to the invention, the positioning precision and mapping robustness of the robot in a dynamic environment can be improved. And the sensing capability and the positioning accuracy of the system to the dynamic environment are enhanced.

Description

Technical Field

[0001] The present invention belongs to the field of computer vision and SLAM, and in particular relates to a dynamic visual positioning and mapping method and system based on multi-spatial information. Background Art

[0002] With the rapid development of robotics, its applications in various fields are becoming increasingly widespread. Simultaneous localization and mapping (SLAM) and path planning systems play a crucial role in these applications. Robots often face complex and changing environments when performing tasks, such as cities, building complexes, or forests. Traditional positioning methods, especially those relying on Global Positioning System (GPS) technology, are often affected by signal obstruction and multipath effects in these environments, resulting in reduced positioning accuracy. Visual SLAM technology has become crucial in this context. Its goal is to estimate the motion of robots or autonomous vehicles in real time and build environmental maps, playing an especially important role in unknown environments.

[0003] In the field of computer vision and SLAM, traditional visual positioning and mapping methods are mostly based on the assumption of static scenes, relying on image frames collected by binocular cameras for feature extraction and matching, which makes it difficult to effectively deal with the interference of object motion in dynamic environments. Existing technologies usually only use key points (such as ORB features) for matching, and lack the coordinated use of geometric features such as line segments and planes, resulting in insufficient stability of feature matching in dynamic scenes. In addition, the asynchronous event data stream of the event camera is not combined with the binocular camera video stream, and it is impossible to accurately separate dynamic areas through means such as event flow density analysis. In addition, in multi-sensor fusion (such as vision + IMU) scenarios, traditional methods lack a collaborative optimization mechanism for visual features and inertial measurement information, and key frame screening does not integrate multi-dimensional conditions, resulting in low positioning accuracy and mapping robustness, making it difficult to meet application requirements in complex dynamic environments. Summary of the Invention

[0004] In order to solve the problems existing in the background technology and improve the positioning accuracy and mapping robustness of the robot in a dynamic environment, and enhance the system's perception ability and positioning accuracy in a dynamic environment, one aspect of the present invention provides a dynamic visual positioning and mapping method based on multi-spatial information, comprising:

[0005] S1: Collect left and right video streams through the binocular camera and perform grayscale processing on the image frames of the left and right video streams;

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

[0007] S3: Calculate the relative poses between the image frames, identify key image frames based on the relative poses, time intervals, and feature matching between the image frames, insert the key image frames into the local map, and establish a local map;

[0008] S4: Optimize the local map by minimizing the reprojection error between frames to obtain an optimized local map.

[0009] Preferably, extracting features of each frame of the left and right video streams includes:

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

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

[0012] S23: extracting left and right eye image pairs frame by frame from the left and right eye video streams captured by the binocular camera, and generating point cloud data of the left and right eye image pairs through stereo matching; and extracting planar features of the left and right eye image pairs using the RANSAC algorithm based on the point cloud data of the left and right eye image pairs.

[0013] Preferably, step S3 includes:

[0014] S31: performing feature matching based on key points, line segments, and plane features of the current image frame and the previous image frame to identify a matching set of key points, line segments, and plane features;

[0015] S32: solving the relative pose between the current image frame and the previous image frame using epipolar geometry estimation based on the key point, line segment and plane feature matching set;

[0016] S33: If the current picture frame satisfies any of the following determination conditions, the current picture frame is determined to be a key picture frame. The determination conditions are as follows:

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

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

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

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

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

[0022] Another aspect of the present invention provides a computer storage medium having a program stored thereon, which, when executed by a processor, implements the dynamic visual positioning and mapping method based on multi-spatial information.

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

[0024] The present invention uses a dynamic region separation method based on event flow density analysis, optical flow field consistency detection, and event accumulation graphs to accurately identify dynamic objects and eliminate them during the mapping process, ensuring accurate modeling of static backgrounds. It combines the ORB algorithm, LSD algorithm, and stereo matching technology to achieve collaborative extraction and matching of multiple spatial features such as key points, line segments, and planes, thereby enhancing the richness and robustness of feature information. By introducing an adaptive weight calculation mechanism, the reprojection error optimization weight is dynamically adjusted based on the number of features in the key image frame, thereby improving the overall error optimization accuracy of the system in pure vision and vision + IMU scenarios. In vision + IMU scenarios, inertial measurement information is effectively integrated through the joint optimization of IMU data pre-integration and visual features, reducing positioning drift in scenes with intense motion or sparse features. This method significantly improves the adaptability and stability of the system in complex dynamic environments, overcomes the shortcomings of traditional methods in low positioning accuracy and poor mapping robustness in dynamic scenes, and achieves high-precision simultaneous positioning and map construction. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 Schematic diagram of the method of the present invention. DETAILED DESCRIPTION

[0026] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the features in the following embodiments and embodiments can be combined with each other without conflict.

[0027] See also Figure 1 The present invention provides a dynamic visual positioning and mapping method based on multi-spatial information, comprising:

[0028] S1: Collect left and right video streams through the binocular camera and perform grayscale processing on the image frames of the left and right video streams;

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

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

[0031] Among them, Gray(I) represents the grayscale image of picture frame I, R(I), G(I) and B(I) represent the images of picture frame I in the RGB three channels respectively.

[0032] In this embodiment, the RGB three-channel color image is converted into a single-channel grayscale image to reduce the amount of data, reduce the computational complexity of subsequent feature extraction and pose calculation, and adapt to the real-time processing requirements in dynamic scenes; at the same time, the grayscale conversion formula is used to retain the brightness and edge structure information of the image, providing high-quality input for the subsequent ORB algorithm to extract key points, the LSD algorithm to extract line segment features, and stereo matching to generate point cloud data, thereby ensuring the accuracy and stability of feature extraction.

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

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

[0035] S21: Extract the initial key point features of each frame in the left and right video streams using the ORB algorithm, and extract static key points from the initial key points of each frame as the final key point features based on the asynchronous event data stream collected by the event camera;

[0036] Preferably, the step S21 includes:

[0037] S211: Traverse each frame of the left and right video streams. For each pixel point (x, y) in the picture, if there are 12 consecutive pixels among the 16 surrounding pixels whose pixel values ​​are all greater than I(x, y) + 20 or less than I(x, y) + 20, then the pixel point (x, y) is determined to be a candidate corner point, and a candidate corner point set is constructed, where I(x, y) represents the pixel value of the pixel point (x, y);

[0038] In this embodiment, the 16 pixels around each pixel point (x, y) of the image refer to the 16 pixels obtained by dividing the neighborhood into a cross shape, that is, with the current pixel as the center, 4 pixels are extended in each of the four directions of up, down, left and right to form a cross-shaped area, totaling 16 pixels.

[0039] S212: For the candidate corner point set of the image, calculate the Harris response value of each candidate corner point, and retain the top 1000 corner points with the Harris response value as the initial key points:

[0040] S213: Calculate the main direction angle of the initial key point by grayscale centroid method;

[0041] In this embodiment, calculating the principal direction angle of the initial key point using the grayscale centroid method includes: first defining a neighborhood of the initial key point, calculating the first-order moment and the zero-order moment for each pixel within the neighborhood, calculating the centroid coordinates within the neighborhood based on the first-order and zero-order moments of the pixels, and obtaining the principal direction angle of the initial key point based on the calculated centroid coordinates. Preferably, Gaussian weighting can be applied to the pixel grayscale values ​​when calculating the moments to enhance the influence of the central area.

[0042] S214: In the local coordinate system rotated according to the main direction angle of the initial key point, randomly select 256 pixels from the neighborhood of the initial key point, compare the pixel value differences between the 256 pixels and the initial key point to generate a binary BRIEF descriptor of the initial key point, and use the binary BRIEF descriptor of the initial key point as the feature of the initial key point;

[0043] In this embodiment, the rotation according to the main direction angle of the initial key point is expressed as follows: with the initial key point as the origin, the image coordinate system is rotated around the origin by an angle of -θ, where θ represents the main direction angle of the initial key point, to obtain a rotated local coordinate system. In the rotated local coordinate system, a square neighborhood of size s×s is defined, and 256 randomly selected pixels are compared with the pixel value difference of the initial key point to generate a binary BRIEF descriptor of the initial key point:

[0044]

[0045] Among them, b iIndicates the i-th binary BRIEF descriptor, I i Indicates the i-th selected pixel, I key The pixel representing the initial key point and the binary BRIEF descriptor of the initial key point are used as the feature of the initial key point;

[0046] S215: The asynchronous event data stream of the image is collected by the event camera, and the time surface response value of each initial key point in the image is calculated based on the most recent event time of each initial key point:

[0047]

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

[0049] S216: Mark the initial key points whose time surface response values ​​are greater than or equal to the set threshold as dynamic key points, otherwise mark them as static key points.

[0050] In this embodiment, step S21 achieves accurate extraction of static key points in dynamic scenes through the synergy of the ORB algorithm and event camera data: first, candidate corner points are screened by traversing the pixels, the Harris response value is calculated to retain the key corner points, and the grayscale centroid method is used to determine the main direction angle and generate the BRIEF descriptor, thus constructing an initial key point set with direction invariance and feature discrimination; then, using the asynchronous event data stream collected by the event camera, the dynamic degree is calculated by the time surface response value, and the key points with response values ​​below the threshold are marked as static, effectively eliminating key points on dynamic objects (such as moving pedestrians and vehicles). This process not only uses the ORB algorithm to ensure the efficiency and scale invariance of key point extraction, but also realizes the adaptive screening of static features through the dynamic perception capability of the event camera, providing a stable point feature basis for subsequent inter-frame feature matching, significantly reducing the probability of feature mismatching in dynamic environments, and improving the reliability of pose estimation and map construction.

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

[0052] Preferably, the step S22 includes:

[0053] S221: Calculate the horizontal gradient and vertical gradient of each pixel in the image using the Sobel operator;

[0054] S222: Calculate the gradient magnitude and gradient direction of each pixel in the image based on the horizontal gradient and vertical gradient of each pixel in the image;

[0055] S223: assigning pixels with gradient magnitudes greater than 0 to multiple intervals according to the gradient direction of each pixel in the image to form multiple direction groups;

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

[0057] S225: Add the seed point to the line segment and mark the seed point as used; initialize the region angle to the gradient direction of the seed point; traverse the eight neighboring pixels of the seed point, and if there is a neighborhood pixel that is unused and the absolute difference between the gradient direction of the neighborhood pixel and the region angle is within a preset error range, then add the neighborhood pixel to the line segment and mark it as used;

[0058] S226: Update the area angle according to the horizontal gradient and vertical gradient of the pixel points in the line segment;

[0059] S227: Repeat the search for the pixel points added to the new line segment until there are no neighboring pixels that meet the conditions to obtain the line segment;

[0060] S228: Select the pixel with the largest gradient amplitude from the unmarked pixels in the direction group as the seed point again; repeat the above steps until all pixels in the direction group are used, output all the found line segments; and calculate the weighted centroid and endpoints of the line segments as the line segment features of the image.

[0061] In this embodiment, step S22 extracts line segment features through the improved LSD algorithm, which can accurately identify different parallel line segments and line segments on the same horizontal line, and can extract richer and more accurate line segment features. At the same time, line segments with pixel points below the set threshold can be deleted as noise. In addition, this embodiment uses the Sobel operator to calculate pixel gradients to identify edge areas, group them by gradient direction and select seed points based on gradient amplitude sorting, construct continuous line segments through neighborhood expansion, and finally use weighted centroids and endpoints as line segment feature representations. This process effectively captures linear structures in the scene (such as wall edges, object contours, etc.), providing structured geometry for dynamic visual positioning and mapping. What constraints: On the one hand, the gradient amplitude sorting and direction grouping mechanism can filter out noise interference, ensure that the extracted line segment features have high accuracy and robustness, and reduce the false extraction of dynamic object edges; on the other hand, the line segment features complement the key points and plane features. During feature matching, the dual constraints of endpoint coordinates and direction vectors are used to improve the matching accuracy, provide additional geometric constraints for the relative pose calculation between frames (such as line segment epipolar constraints), and participate in the reprojection error calculation as an independent error term in local map optimization, thereby enhancing the stability of the map structure. Especially in structured scenes, it can significantly improve the system's ability to represent the environmental structure and reduce the ambiguity of feature matching under dynamic interference.

[0062] S23: extracting left and right eye image pairs frame by frame from the left and right eye video streams captured by the binocular camera, and generating point cloud data of the left and right eye image pairs through stereo matching; extracting planar features of the left and right eye image pairs using the RANSAC algorithm based on the point cloud data of the left and right eye image pairs;

[0063] Preferably, the extracting planar features of the left and right eye image pairs using the RANSAC algorithm includes:

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

[0065] S232: Calculate the distance between each point in the point cloud and the plane. If the distance is less than a set threshold, the point is determined to be an interior point of the plane.

[0066] S233: Reread steps S31-S33 until a preset number of iterations is reached, and take 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 capabilities for dynamic visual positioning and mapping by collaboratively extracting key points, line segments, and plane features: the ORB algorithm combined with the asynchronous event data stream of the event camera can accurately screen static key points from the initial key points, effectively eliminate the interference of dynamic objects, and ensure 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 lack of description ability of a single key point in a structured scene; the plane features (such as walls and ground) extracted by stereo matching and RANSAC algorithms enhance the geometric structure information of the scene. The combination of the three forms a feature system of point, line, and plane collaboration. This feature extraction mechanism not only improves the robustness of feature matching in complex dynamic environments, but also provides multivariate constraints (such as epipolar geometry and plane projection relationships) for subsequent inter-frame pose calculations. At the same time, it lays a data foundation for key frame screening and local map optimization, enabling the system to more accurately identify environmental structures and suppress dynamic interference, thereby improving the accuracy and stability of positioning and mapping.

[0068] S3: Calculate the relative poses between the image frames, identify key image frames based on the relative poses, time intervals, and feature matching between the image frames, insert the key image frames into the local map, and establish a local map;

[0069] Preferably, step S3 includes:

[0070] S31: performing feature matching based on key points, line segments, and plane features of the current image frame and the previous image frame to identify a matching set of key points, line segments, and plane features;

[0071] The matching method calculates the Hamming distance by the BRIEF descriptor of the key points, for example, the current frame F i The key point feature set is defined as K i ; Previous frame F i-1 The key point feature set is defined as K i-1 ; For the current frame F i and the previous frame F i-1 Calculate the Hamming distance, retain the matching pairs with a distance less than the threshold, and obtain the key point feature matching set;

[0072] Line segment feature matching is performed by comparing the endpoint coordinates and direction vectors of the line segment. The Euclidean distance between the endpoint coordinates and the angle between the direction vectors are calculated, with the endpoint distance threshold set to 10 pixels and the direction angle threshold set to 10°.

[0073] Plane feature matching,the matching method is to calculate the normal vector of each plane and the distance,from the plane to the origin, setting the normal vector angle threshold to 15°,and the distance difference threshold to 0.2 meters.

[0074] S32: solving the relative pose between the current image frame and the previous image frame using epipolar geometry estimation based on the key point, line segment and plane feature matching set;

[0075] In this embodiment, for the key point, line segment and plane feature matching set, the eight-point method and other algorithms are used to calculate the essential matrix, and then the relative position between the current image frame and the previous image frame is obtained through singular value decomposition.

[0076] S33: If the current picture frame satisfies any of the following determination conditions, the current picture frame is determined to be a key picture frame. The determination conditions are as follows:

[0077] Condition 1: The relative pose change between the current image frame and the previous image frame exceeds the set threshold;

[0078] Condition 2: The time difference between the current image frame and the previous image frame exceeds the preset time threshold;

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

[0080] S34: Quantify the key point features extracted from the key picture frame to generate a BOW bag-of-words vector; and construct a common view relationship graph based on the commonly observed points, line segments, and plane features between the key picture frames, add the current key picture frame to the queue of key picture frames to be inserted into the local map, and update the local map.

[0081] In this embodiment, constructing a co-viewing relationship graph includes:

[0082] Check the features commonly observed between the key picture frame and the existing key picture frames, and determine the weight of the common view relationship based on the number of commonly observed features. For example, set the key point weight to 1, the line segment weight to 2, and the plane weight to 5. The common view relationship weight between the key picture frame and the existing key picture frame is 1×A1+2×A2+5×A3; A1, A2, and A3 are the number of point, line, and surface features commonly observed between the key picture frame and the existing key picture frame.

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

[0084] In this embodiment, step S3 constructs a local map by calculating the relative pose between picture frames and identifying key picture frames, thereby achieving efficient screening and structured storage of environmental information in dynamic scenes: first, based on the point, line, and surface feature matching set of the current frame and the previous frame, the relative pose is solved using epipolar geometry estimation to provide accurate geometric constraints for the inter-frame motion relationship; second, the key frame is determined by the triple conditions of relative pose change, time interval, and number of feature matches, which not only avoids the computational load caused by redundant frame insertion, but also prevents the omission of key information, ensuring the real-time and effectiveness of map updates; finally, the key point features of the key frame are quantized to generate a BOW bag-of-words vector, and the local map is constructed in combination with the common view relationship graph, which not only achieves the structured storage of key frame pose information and feature association, but also strengthens the geometric association between frames in the map through common view weight design. This process enables the local map to dynamically adapt to scene changes, providing a compact and effective data foundation for subsequent map optimization based on reprojection errors. It also lays the foundation for the underlying framework of feature matching and pose association for similarity retrieval and global map construction in closed-loop detection, significantly improving the system's positioning and mapping efficiency and robustness in dynamic environments.

[0085] S4: Optimizing the local map by minimizing the inter-frame reprojection error to obtain an optimized local map;

[0086] In a purely visual scenario, the optimization of the local map by minimizing the inter-frame reprojection error includes:

[0087] S401: Calculate the adaptive weight of the key picture frame according to the characteristics of the key picture frame:

[0088]

[0089] Where, ω adp represents the adaptive weight, ω ba The base weight is set to 0.33, F n Indicates the number of key point features extracted from the key picture frame, P n represents the number of line segment features extracted from the key picture frame, N is the number of key point features with a threshold set to 300, k is a self-set parameter of 0.005, and exp represents the exponential function.

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

[0091]

[0092] Where, ψ * represents minimizing the reprojection error between frames, ψ represents the reprojection error between frames, Represents a set of key picture frames, represents the key point feature set in the key picture frame, e ij represents the reprojection error matrix of key point feature j in key picture frame i; Represents matrix transpose; represents the inverse matrix of the covariance weight matrix of key point feature j in key picture frame i; represents the midline feature set of the key picture frame; e ik represents the reprojection error matrix of line segment feature k in key image frame i; P represents the inverse matrix of the covariance weight matrix of the line segment feature k in the key picture frame i; p represents the face feature set in the key picture frame; e ip represents the reprojection error matrix of the planar feature p in the key picture frame i, represents the inverse matrix of the covariance weight matrix of the plane feature p in the key picture frame i.

[0093] In this embodiment, steps S401-S402 achieve dynamic and accurate optimization of local maps in purely visual scenarios through adaptive weight calculation and multi-feature joint optimization. S401 dynamically calculates adaptive weights using an exponential function based on the number of static keypoints and line segment features extracted from key image frames. This gives feature-rich keyframes higher weights in the optimization, effectively suppressing the error contribution of frames with sparse features or dynamic interference. S402 couples the reprojection errors of point, line, and surface features with the adaptive weights to construct a multi-dimensional error optimization model, iteratively optimizing the keyframe poses by minimizing the weighted errors. This process not only dynamically adjusts the optimization weights based on the number of keypoints, avoiding the misadaptation of traditional fixed weights to dynamic features, but also improves the geometric consistency of pose estimation through multi-feature collaborative constraints. This enables the local map to adaptively filter dynamic interference in purely visual scenarios and strengthen the constraints of static structural features, ultimately achieving a dual improvement in map accuracy and robustness. This significantly reduces the accumulation of reprojection errors, especially in scenes with uneven feature distribution or the presence of dynamic objects, ensuring the modeling accuracy and scene adaptability of the local map.

[0094] In the vision + IMU scenario, the optimization of the local map by minimizing the inter-frame reprojection error includes:

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

[0096]

[0097] Where, ω ba is the basic weight, F n Indicates the number of static key point features extracted, P nRepresents the number of extracted line segment features, N is the threshold for the number of point features, and k is a custom parameter.

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

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

[0100]

[0101] Where r i,i+1 Represents the IMU pre-integration residual from the i-th frame to the i+1 frame, including the collective residual vector of the three parts of rotation, velocity and position, Represents the rotation residual vector from frame i to frame i+1, Represents the velocity residual vector from frame i to frame i+1, Represents the position residual vector from frame i to frame i+1, ΔR i,i+1 , Δv i,i+1 and Δp i,i+1 Represents the rotation, velocity and position matrices of the relative pose between 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+1th frame, v i+1 represents the velocity matrix of the i+1th 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 Indicates the time from frame i to frame i+1, Δv i,i+1 Represents the velocity matrix from frame i to frame i+1, p j represents the position matrix of the jth frame, v represents the linear velocity at the i-th frame, p i represents the position coordinate of the i-th frame, Δt represents the change time, and g is the gravitational acceleration.

[0102] S414: Optimizing the local map based on the rotation residual, velocity residual, and position residual between the current key picture frame and the previous key picture frame to minimize the inter-frame reprojection error to obtain an optimized local map, wherein minimizing the inter-frame reprojection error includes:

[0103]

[0104] Where, ξ * Represents minimizing the reprojection error between frames, r i,i+1 Represents the IMU pre-integration residual from the i-th frame (current key picture frame) to the i+1 frame (previous key picture frame), including the aggregate residual vector of the three parts of rotation, velocity and position, Represents a set of key picture frames, represents the key point feature set in the key picture frame, e ij represents the reprojection error matrix of key point feature j in key picture frame i; Represents matrix transpose; represents the inverse matrix of the covariance weight matrix of key point feature j in key picture frame i; represents the midline feature set of the key picture frame; e ik represents the reprojection error matrix of line segment feature k in key image frame i; P represents the inverse matrix of the covariance weight matrix of the line segment feature k in the key picture frame i; p represents the face feature set in the key picture frame; e ip represents the reprojection error matrix of the planar feature p in the key picture frame i, represents the inverse matrix of the covariance weight matrix of the plane feature p in the key picture frame i.

[0105] In this embodiment, steps S411-S414 achieve high-precision calibration of local maps in dynamic scenes through deep fusion of visual and IMU data and adaptive optimization strategies: S411 calculates adaptive weights based on the number of static key points and line segment features of the key frames, so that feature-rich frames dominate the optimization and effectively weaken the error influence of dynamic interference frames; S412 obtains the relative pose increments of adjacent key frames by pre-integrating the IMU data, converts the inertial measurement information into optimizable constraints, and compensates for the tracking defects of pure vision methods during intense motion; S413 further decomposes the pre-integration results into rotation, velocity and position residuals, accurately quantifying the difference between IMU measurements and visual pose estimation; S414 couples the above residuals with the reprojection errors of point, line and surface features to construct a joint optimization model with multi-source constraints, and synchronously corrects the key frame 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 an adaptive weight mechanism, so that the system can still maintain positioning accuracy in scenarios with sparse features or dynamic interference. Especially when the robot moves rapidly or the ambient lighting changes suddenly, it can significantly improve the stability and scene adaptability of the local map, laying a precise local coordinate foundation for global multi-map optimization.

[0106] In this embodiment, after the local map is optimized in step S4, closed-loop detection and real-time positioning in SLAM (step S5) and global multi-map optimization (step S6) can also be performed. The optimization process is as follows:

[0107] S5: performing closed-loop detection and real-time positioning on the local map optimized in step S4, and reconstructing the local map;

[0108] Preferably, performing closed-loop detection and real-time positioning on the optimized local map includes:

[0109] S51: For the current key picture frame, calculate the Hamming distance between its BoW vector and the BoW vectors of all historical key picture frames, and select historical key picture frames with similarity higher than a threshold as closed-loop candidate frames;

[0110] S52: For the closed-loop candidate frame, the RANSAC algorithm is used to estimate the basic matrix based on the key point features of the current key picture frame and the closed-loop candidate frame, and the number of inliers that meet the epipolar constraint is counted; and the mean reprojection error of the inliers is calculated;

[0111] S53: Filter out, from the closed-loop candidate frames, closed-loop candidate frames whose number of inliers exceeds a set threshold, whose mean reprojection error of the inliers is less than a set threshold, whose time interval with the current key picture frame is greater than a set threshold, and whose number of co-view key picture frames with the current key picture frame is greater than a set threshold as closed-loop frames;

[0112] S54: Calculating the relative pose between the previous key picture frame and the closed-loop frame based on the inner points of the current key picture frame and the closed-loop frame;

[0113] S55: All key image frames are considered as graph nodes. Edge constraints include: inter-frame constraints, inertia constraints, and closed-loop constraints. The optimization goal is set: Among them, e visual 、e imu and e loop They correspond to visual reprojection error, IMU residual, and closed-loop constraint error respectively;

[0114] In this embodiment, the IMU residual refers to the difference between the pose increment obtained by IMU pre-integration and the pose increment estimated based on the pose of the current key picture frame. In practical applications, IMU measurements are subject to noise and drift, and the pre-integration process also introduces certain errors. Therefore, it is necessary to measure the consistency between the IMU data and the visual positioning results by calculating the residual, so as to correct the pose of the key picture frame in the global map optimization. The closed-loop constraint error is used to measure the difference between the relative pose constraint between the two key picture frames determined in the closed-loop detection and the relative pose estimated based on the pose of the current key picture frame. When a closed loop is detected, it is assumed that the two frames should satisfy a specific relative pose relationship, but the actual pose estimate may be inconsistent with the constraint due to reasons such as cumulative errors. This difference is the closed-loop constraint error. By minimizing the closed-loop constraint error, the accumulated drift of the map can be corrected so that the global map can be accurately connected at the closed loop. The visual reprojection error is a key indicator in SLAM (Simultaneous Localization and Mapping) to measure the consistency between three-dimensional map points and two-dimensional image observations. The core idea is to project the 3D map points onto the image plane through the currently estimated camera pose, and compare them with the actual observed feature point positions. The pixel-level deviation between the two is the reprojection error.

[0115] S56: Use the Levenberg-Marquardt algorithm or the g2o / GraphSLAM optimization library to minimize the optimization objective and update the pose estimate of the key image frame;

[0116] S57: Synchronize the optimized key picture frame pose to the local map and update the three-dimensional coordinates of the key points in the map;

[0117] S58: Re-triangulate the key points according to the optimized key picture frame pose and update the three-dimensional coordinates;

[0118] S59: Calculate the pose change of the optimized key picture frame. If the pose change of the optimized key picture frame exceeds a threshold, search for matching key points in adjacent key picture frames for the key points of the key picture frame, update the correspondence between them, and delete the key points whose reprojection error is greater than the threshold.

[0119] In this embodiment, step S5 effectively suppresses global map cumulative errors and optimizes map structure consistency by performing closed-loop detection and real-time positioning on the optimized local map. First, a similarity search is performed based on the BoW vector of the current keyframe and historical keyframes to screen closed-loop candidate frames. The fundamental matrix is ​​estimated using the RANSAC algorithm, and the closed-loop reliability is verified by combining multiple criteria such as the number of inliers and reprojection error to avoid false loop closures caused by dynamic object interference. The keyframes are then treated as graph nodes, and an optimization objective is constructed that includes inter-frame constraints, inertia constraints, and closed-loop constraints. The combined error is minimized using the Levenberg-Marquardt algorithm or an optimization library, and the keyframe poses and map point 3D coordinates are simultaneously updated. Finally, through pose change thresholding and keypoint retriangulation, the map structure is further optimized and feature points with excessive error are removed. This process not only corrects positioning drift during long-distance motion through closed-loop constraints but also improves the geometric consistency of the global map through the joint optimization of multi-source constraints. This enables the system to maintain high-precision positioning and mapping capabilities in complex and dynamic environments, making it particularly suitable for map reconstruction and error elimination in large-scale scenarios.

[0120] S6: Perform global multi-map optimization on all reconstructed local maps to obtain the constructed final map.

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

[0122] S61: traverse all local maps and detect common key picture frames or shared features between local maps;

[0123] S62: Treat each local map as a graph node. If the two maps have common key image frames or shared features (including point, line, and surface features, the matching process is as described above), then add a connecting edge between the nodes. The weight of the edge represents the number of feature matches.

[0124] S63: Use BoW vectors or binary BRIEF descriptors to perform similarity retrieval between key image frames in different local maps to filter out cross-map frame pairs that may correspond to the same physical area;

[0125] S64: For the candidate frame pairs, estimate the basic matrix or relative pose using the RANSAC algorithm, and verify the number of common view features and the reprojection error constraints;

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

[0127] S66: Use the global graph optimization library for iterative optimization to update the poses of all key image frames and the relative poses between maps to minimize the global cumulative error;

[0128] S67: Based on the global optimization results, update the pose of the key picture frame in each local map to ensure that the pose of the overlapping area of ​​adjacent maps is consistent; if the pose overlap of the overlapping area of ​​two local maps is higher than a threshold, merge the repeated key picture frames and feature points to retain the unique identifier.

[0129] In this embodiment, step S6 achieves consistent construction of a global map and elimination of accumulated errors in complex scenarios by performing global multi-map optimization on the reconstructed local maps. The steps include traversing each local map to detect common keyframes or shared point, line, and surface features, using the local maps as graph nodes and setting edge weights based on the number of feature matches. Similar frame pairs across maps are retrieved using BoW vectors or BRIEF descriptors. The RANSAC algorithm is used to verify the geometric consistency of the frame pairs and filter valid closed-loop edges. Finally, the global graph optimization library is used to iteratively optimize the poses of all keyframes and the relative poses between maps, unifying the poses of the overlapping regions of each local map. This process not only strengthens the structural constraints of the global scene through multi-map feature association, but also corrects the accumulated drift during independent optimization of local maps through cross-map closed-loop detection. Repeated merging of feature points and keyframes in overlapping regions further improves the compactness and accuracy of the map, ultimately constructing a geometrically consistent global map covering a wide range of scenes. This significantly enhances the system's long-distance positioning and mapping capabilities in complex dynamic environments.

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

[0131] Another aspect of the present invention provides a computer storage medium having a program stored thereon, which, when executed by a processor, implements the dynamic visual positioning and mapping method based on multi-spatial information.

[0132] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this 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. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM), 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).

[0133] In summary, the present invention uses a dynamic region separation method based on event flow density analysis, optical flow field consistency detection, and event accumulation graph to accurately identify dynamic objects and eliminate them during the mapping process, ensuring accurate modeling of static backgrounds. It combines the ORB algorithm, LSD algorithm, and stereo matching technology to achieve collaborative extraction and matching of multiple spatial features such as key points, line segments, and planes, enhancing the richness and robustness of feature information. By introducing an adaptive weight calculation mechanism, the reprojection error optimization weight is dynamically adjusted based on the number of features in the key image frame, improving the overall error optimization accuracy of the system in pure vision and vision + IMU scenarios. In vision + IMU scenarios, the inertial measurement information is effectively integrated through the joint optimization of IMU data pre-integration and visual features, reducing positioning drift in scenes with intense motion or sparse features. Furthermore, the present invention can also combine closed-loop detection with a global multi-map optimization mechanism to effectively suppress cumulative errors and ensure the consistency and accuracy of the global map. This method significantly improves the adaptability and stability of the system in complex dynamic environments, overcomes the shortcomings of traditional methods in low positioning accuracy and poor mapping robustness in dynamic scenes, and achieves high-precision simultaneous positioning and mapping.

[0134] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A dynamic visual positioning and mapping method based on multi-spatial information, characterized in that: include: S1: Collect left and right video streams through the binocular camera and perform grayscale processing on the image frames of the left and right video streams; S2: Extract the features of each frame in the left and right video streams, where the features of each frame include: key point features, line segment features, and plane features; S3: Calculate the relative poses between the image frames, identify key image frames based on the relative poses, time intervals, and feature matching between the image frames, insert the key image frames into the local map, and establish a local map; S4: Optimize the local map by minimizing the reprojection error between frames to obtain an optimized local map.

2. The method for dynamic visual positioning and mapping based on multi-spatial information according to claim 1, characterized in that: The feature extraction of each frame of the left and right video streams includes: S21: Extract the initial key point features of each frame in the left and right video streams using the ORB algorithm, and extract static key points from the initial key points of each frame as the final key point features based on the asynchronous event data stream collected by the event camera; S22: extract line segment features of each frame in the left and right video streams using the LSD algorithm; S23: extracting left and right eye image pairs frame by frame from the left and right eye video streams captured by the binocular camera, and generating point cloud data of the left and right eye image pairs through stereo matching; and extracting planar features of the left and right eye image pairs using the RANSAC algorithm based on the point cloud data of the left and right eye image pairs.

3. The method for dynamic visual positioning and mapping based on multi-spatial information according to claim 1, characterized in that: The step S3 comprises: S31: performing feature matching based on key points, line segments, and plane features of the current image frame and the previous image frame to identify a matching set of key points, line segments, and plane features; S32: solving the relative pose between the current image frame and the previous image frame using epipolar geometry estimation based on the key point, line segment and plane feature matching set; S33: If the current picture frame satisfies any of the following determination conditions, the current picture frame is determined to be a key picture frame. The determination conditions are as follows: Condition 1: The relative pose change between the current image frame and the previous image frame exceeds the set threshold; Condition 2: The time difference between the current image frame and the previous image frame exceeds the preset time threshold; Condition 3: The number of feature matches between the current image frame and the previous image frame is lower than the threshold; S34: Quantify the key point features extracted from the key picture frame to generate a BOW bag-of-words vector; and A common view relationship graph is constructed based on the commonly observed points, line segments, and plane features between the key picture frames, the current key picture frame is added to the queue of key picture frames to be inserted into the local map, and the local map is updated.

4. The method for dynamic visual positioning and mapping based on multi-spatial information according to claim 3, characterized in that: In a purely visual scenario, the optimization of the local map by minimizing the inter-frame reprojection error includes: S401: Calculate the adaptive weight of the key picture frame according to the characteristics of the key picture frame: Where, ω ba is the basic weight, F n Indicates the number of static key point features extracted, P n Represents the number of extracted line segment features, N is the threshold for the number of point features, and k is a custom parameter. S402: Optimizing the position of the key picture frame by minimizing the reprojection error between frames to obtain an optimized local map, wherein minimizing the reprojection error between frames includes: Where, ψ * represents minimizing the reprojection error between frames, ψ represents the reprojection error between frames, Represents a set of key picture frames, represents the key point feature set in the key picture frame, e ij represents the reprojection error matrix of key point feature j in key picture frame i; Represents matrix transpose; represents the inverse matrix of the covariance weight matrix of key point feature j in key picture frame i; represents the midline feature set of the key picture frame; e ik represents the reprojection error matrix of line segment feature k in key image frame i; P represents the inverse matrix of the covariance weight matrix of the line segment feature k in the key picture frame i; p represents the face feature set in the key picture frame; e ip represents the reprojection error matrix of the planar feature p in the key picture frame i, represents the inverse matrix of the covariance weight matrix of the plane feature p in the key picture frame i.

5. The method for dynamic visual positioning and mapping based on multi-spatial information according to claim 3, characterized in that: In the vision + IMU scenario, the optimization of the local map by minimizing the inter-frame reprojection error includes: S411: Calculate the adaptive weight of the key picture frame according to the characteristics of the key picture frame: Where, ω ba is the basic weight, F n Indicates the number of static key point features extracted, P n Represents the number of extracted line segment features, N is the threshold for the number of point features, and k is a custom parameter. S412: Pre-integrate the IMU data of the current key picture frame and the previous key picture frame to obtain a relative pose increment between the current key picture frame and the previous key picture frame; S413: Calculating the 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 based on the rotation residual, velocity residual, and position residual between the current key picture frame and the previous key picture frame to minimize the reprojection error between frames, thereby obtaining an optimized local map.

6. The method for dynamic visual positioning and mapping based on multi-spatial information according to claim 5, characterized in that: In the vision + IMU scenario, minimizing the inter-frame reprojection error includes: Where, ξ * represents minimizing the reprojection error between frames, r i,i+1 Represents the IMU pre-integration residual from the i-th frame (current key picture frame) to the i+1 frame (previous key picture frame), including the aggregate residual vector of the three parts of rotation, velocity and position, Represents a set of key picture frames, represents the key point feature set in the key picture frame, e ij represents the reprojection error matrix of key point feature j in key picture frame i; Represents matrix transpose; represents the inverse matrix of the covariance weight matrix of key point feature j in key picture frame i; represents the midline feature set of the key picture frame; e ik represents the reprojection error matrix of line segment feature k in key image frame i; P represents the inverse matrix of the covariance weight matrix of the line segment feature k in the key picture frame i; p represents the face feature set in the key picture frame; e ip represents the reprojection error matrix of the planar feature p in the key picture frame i, represents the inverse matrix of the covariance weight matrix of the plane feature p in the key picture frame i.

7. The method for dynamic visual positioning and mapping based on multi-spatial information according to claim 5, characterized in that: The rotation residual, velocity residual and position residual between the current key picture frame and the previous key picture frame include: Where r i,i+1 Represents the IMU pre-integration residual from the i-th frame to the i+1 frame, including the collective residual vector of the three parts of rotation, velocity and position, Represents the rotation residual vector from frame i to frame i+1, Represents the velocity residual vector from frame i to frame i+1, Represents the position residual vector from frame i to frame i+1, ΔR i,i+1 , Δv i,i+1 and Δp i,i+1 Represents the rotation, velocity and position matrices of the relative pose between 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+1th frame, v i+1 represents the velocity matrix of the i+1th 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 Indicates the time from frame i to frame i+1, Δv i,i+1 Represents the velocity matrix from frame i to frame i+1, p j represents the position matrix of the jth frame, v represents the linear velocity at the i-th frame, p i represents the position coordinate of the i-th frame, Δt represents the change time, and g is the gravitational acceleration.

8. A dynamic visual positioning and mapping system based on multi-spatial information, characterized in that: The system includes a memory and a processor; the memory is used to store an application; the processor is used to run the application and execute a dynamic visual positioning and mapping method based on multi-spatial information as described in any one of claims 1 to 7.

9. A computer storage medium, characterized in that The computer storage medium stores a program, and when the program is executed by the processor, the dynamic visual positioning and mapping method based on multi-spatial information according to any one of claims 1 to 7 is implemented.

Images