An RGB-D visual synchronous localization mapping method based on windowed matching estimation

Abstract

This invention discloses an RGB-D vision-synchronous localization and mapping method based on windowed matching estimation, belonging to the field of robot computer vision localization and mapping. First, by windowing the extracted feature points, matching efficiency is improved while ensuring good feature point matching. Then, the analytical ICP algorithm is used to solve the camera pose, avoiding the computational resource waste caused by iterative solutions. The windowed pose solution with the smallest error is selected for bundle adjustment optimization to reduce pose errors caused by local information loss or mismatches. Finally, the robot pose result is introduced into the ORB-SLAM framework to output real-time robot pose and environment map. The windowed matching estimation method proposed in this invention can maintain stable feature tracking and accurate pose solution even in environments with monotonous textures and low illumination.

Description

Technical Field

[0001] This invention relates to the field of robot computer vision localization and mapping, and more specifically, to an RGB-D visual synchronous localization and mapping method based on windowed matching estimation. Background Technology

[0002] With the development of science and technology, mobile robots, such as unmanned vehicles and drones, are increasingly being applied to logistics, surveying, exploration, reconnaissance, and search and rescue. Cameras, as inexpensive, lightweight, and information-rich sensor components, play an indispensable role in mobile robots. Therefore, to enable mobile robots to achieve accurate localization and environmental reconstruction in unknown environments, an accurate and efficient visual simultaneous localization and mapping (VLS) method is needed. This method utilizes image data captured by cameras for localization and mapping. By efficiently and accurately extracting and matching image features, mobile robots can use these features to locate themselves and simultaneously reconstruct a 3D model of the environment. In this way, mobile robots can accurately determine their position in unknown environments and generate visualized maps, providing support for subsequent tasks and decision-making.

[0003] Visual SLAM algorithms are divided into direct methods and feature-based methods based on their different ways of utilizing image information. Direct methods have the advantages of high efficiency and insensitivity to regions with missing features. However, direct methods are sensitive to camera parameters and exposure, and are prone to feature loss when the camera is moving rapidly. Feature-based methods have the advantages of strong robustness to illumination, high repeatability, and high distinguishability, and have been more widely used in practice.

[0004] However, the EPnP method for solving camera pose based on feature points is inherently random, leading to unstable localization accuracy. Furthermore, it is prone to tracking errors and failures during tracking due to dim ambient lighting or monotonous environmental textures. Tracking errors result in incorrect poses. Tracking failures require re-initializing the localization and building a new sub-map, losing the localization function and compromising the accuracy of the resulting global map. Currently, there are no simultaneous localization and mapping methods that utilize windowed matching estimation to improve feature point matching accuracy. Summary of the Invention

[0005] To address the aforementioned problems, the present invention aims to provide a real-time synchronous localization and mapping (SMR) technology for RGB-D camera-based mobile robots, offering high feature point matching accuracy, high pose estimation accuracy, and high mapping accuracy even without prior maps. This invention employs a windowed pose matching estimation method to suppress the probability of random matching generated by the pose solving EPnP method, reducing the tracking loss rate and thus improving the accuracy of localization and global mapping.

[0006] To achieve the above technical objectives, this invention provides an RGB-D visual synchronous localization and mapping method based on windowed matching estimation, applied to a mobile robot equipped with an RGB-D camera, to reduce the probability of tracking loss and improve the accuracy of localization and global mapping, including the following steps:

[0007] Using an RGB-D camera, new input image frames are acquired and Fast corner detection is performed. The detected corners are then subjected to orientation detection, and their orientations are rotated to the horizontal. The processed corners are then used to generate binary descriptors and feature point matching is performed.

[0008] Based on the feature point windowing matching results, the camera pose is obtained by parsing the ICP method;

[0009] Based on the ORB-SLAM algorithm, the mobile robot is simultaneously localized and globally mapped according to the camera pose and key points obtained through feature point matching.

[0010] Preferably, during the feature point matching process, a matching filter is performed in the window, retaining only the pair of matching results with the smallest Hamming distance within the window for camera pose solving.

[0011] Preferably, during the window matching process, corner points are extracted from the acquired RGB images of the previous and current frames, the corner points are divided according to the window, and the direction vector is obtained based on the centroid and position of the corner points.

[0012] Align the direction vector with the horizontal direction, then extract the binary descriptor, and obtain the initial matching result by filtering the binary descriptor;

[0013] A 31×31 pixel window is set with the initially matched feature points as the center. Matching point pairs other than those with the smallest Hamming distance are removed from the window to obtain the final matching point pairs, which are used as the feature point windowing matching results.

[0014] Preferably, during the process of dividing corner points into windows, the gray-scale centroid method is used to detect the orientation consistency of feature points.

[0015] Preferably, during the process of obtaining the direction vector, a quality-weighted sum is calculated for the pixel values ​​within the radius r of corner point O: Where, m 10 The weighted sum in the x-direction of the image, m 01 This is the weighted sum in the y-direction of the image;

[0016] The centroid C of the image is obtained using a matrix:

[0017] Connect the corner point O and the centroid C to obtain the direction vector.

[0018] Preferably, during the process of acquiring camera pose, an error function that is only related to Euclidean transformation R and t is established based on the matching point pairs of the feature point windowing matching results, and the pose is solved using the SVD decomposition method.

[0019] Based on the pose solution, the minimum reprojection error of the window between the two frames is obtained by forming a pair of verification point sets by establishing a matching relationship between the feature points in the corresponding windows of the two frames.

[0020] Obtain the set of verification points that meet the requirements for minimum reprojection error of the window, and use them as key points. Based on the pose solution results, perform joint landmark point BA optimization to obtain the final landmark point position and camera pose. Global mapping is performed using the landmark point position.

[0021] Preferably, in the process of obtaining the error function, the error function is expressed as:

[0022]

[0023] Where, p i p′ represents the camera coordinates of the i-th feature point in the previous frame. i Indicates the time interval p in the next frame. i Matched feature point p′ i The camera coordinates are: p and p′ represent the centroids of the feature points in the two frames respectively; R and t represent the rotation and translation matrices between the camera coordinates of the two frames respectively.

[0024] Preferably, during the pose calculation process, based on the error function, the centroid coordinates are defined as q. i ,q i ′:q i =p i -p,q i ′=p i '-p', obtain the solution function for the rotation matrix R, where the solution function is expressed as:

[0025]

[0026] Based on the centroid-free coordinates, define a 3×3 matrix W = q i q i T Perform SVD decomposition on W: W = UΣV T In the formula, Σ represents a diagonal matrix composed of singular values, and U and V represent orthogonal matrices, then R = UV. T ,t=p-Rp′.

[0027] Preferably, during the BA optimization process, the least squares objective function for constructing the BA optimization is:

[0028]

[0029] Among them, z ij This indicates that in state i, p j Observations at key points, h(ξ) i ,p j ) indicates that in state i, p j Expected observations for keypoint calculation.

[0030] Preferably, during the synchronous localization and global mapping of the mobile robot, the acquired camera pose and key points are used as input results and input into the local mapping thread of ORB-SLAM. In the local mapping thread of ORB-SLAM, local BA optimization is performed by combining local historical key points and camera pose to obtain the locally optimal continuous camera pose and corresponding observation landmarks.

[0031] The input results are fed into the ORB-SLAM map atlas thread. The ORB-SLAM map atlas thread generates a BOW2 bag-of-words model for the key points. The BOW2 bag-of-words model is used to detect whether the mobile robot has ever passed through a certain place and caused a loop. If the mobile robot has been there and there is an inactive map of that place in the map atlas, the inactive map and the active map are fused together using the historical pose of the loop and the current pose.

[0032] The input results are fed into the ORB-SLAM loop closure detection thread. The ORB-SLAM loop closure detection thread uses key points to generate a BOW2 bag-of-words model. The bag-of-words model is used to detect whether the mobile robot has passed through a certain place and a loop has occurred. If a loop has occurred, a loop closure BA optimization is constructed, which starts from the historical image frame where the loop occurred and ends with the current input frame, constituting the mobile robot pose and its key points.

[0033] The present invention discloses the following technical effects:

[0034] Compared to traditional RGB-D visual synchronous localization and mapping algorithms, the present invention can maintain stable front-end feature tracking even in environments with monotonous textures and low lighting. Based on windowed matching, the present invention uses an analytical ICP solution method instead of an iterative ICP solution method to improve computational efficiency. Attached Figure Description

[0035] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0036] Figure 1 This is a flowchart of the camera pose calculation process described in this invention;

[0037] Figure 2 This is a system architecture diagram of the RGB-D visual synchronous localization and mapping method based on windowed matching estimation described in this invention.

[0038] Figure 3 This is a schematic diagram of the windowed matching estimation described in this invention;

[0039] Figure 4 This is a schematic diagram of the BA optimization described in this invention;

[0040] Figure 5 This is a diagram showing the mapping result described in this invention. Detailed Implementation

[0041] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0042] like Figure 1-5 As shown, this invention discloses an RGB-D visual synchronous localization and mapping method based on windowed matching estimation. It is mainly a visual RGB-D synchronous localization and mapping method that can output the pose and map of a mobile robot in real time by using windowed matching to reduce feature point mismatches, using analytical ICP pose solving to improve computational efficiency, and using error verification to determine the pose solving quality.

[0043] The implementation of this invention mainly includes:

[0044] First, parameter configuration:

[0045] 1) Obtain RGB-D camera parameters through camera calibration: Set RGB-D camera parameter information including image resolution (Width, Height); camera focal length (F). x F y Optical center position C x C yDepth Map Factor (depth unit); Camera distortion parameters K1, K2, P1, P2; Extrinsic parameter transformation matrix T between RGB and depth lenses; Video stream frame rate (fps).

[0046] 2) The configuration algorithm parameter information includes: number of feature points per frame (NumFeatures); radius of the corner neighborhood (r); scale factor of the feature extraction pyramid; and number of pyramid levels (NumLevels).

[0047] Second, feature point extraction is performed on the input image:

[0048] 1) Generate a 31×31 window from the input image, and calculate the centroid and orientation angle of the corner points in the window;

[0049] 2) Rotate the pixels within the neighborhood radius r of the corner point until the corner direction angle is 0;

[0050] 3) Generate rotated corner descriptors for feature point matching between adjacent frames;

[0051] 4) Retain the set of feature points with the closest Hamming distance within the window and remove the remaining feature points within the window;

[0052] 5) Use the preserved feature points to solve the analytical ICP pose between frames that is only related to the Euclidean transform R and t;

[0053] 6) Use a windowed feature point set to perform error verification of R and t;

[0054] 7) If the error check is successful, perform joint key point BA optimization.

[0055] Third, the output pose and landmark results from step two are input into the local mapping thread for local mapping and local pose optimization.

[0056] Fourth, the output pose and landmark results from step two are input into the map atlas thread to trigger the fusion of active and inactive maps.

[0057] Fifth, by combining pose, landmarks, and map confidence, we can detect whether loop closure has occurred. If loop closure occurs, we can optimize the positioning error and further improve the consistency of the map.

[0058] This invention relates to the field of robot computer vision localization and mapping, and presents an RGB-D visual simultaneous localization and mapping method based on windowed matching estimation. The goal of this algorithm is to enable mobile robots to autonomously localize and map using an RGB-D camera in low-light environments or scenes with monotonous textures, without prior maps. Examples demonstrate that the windowed matching estimation method proposed in this invention maintains better front-end tracking and more accurate back-end localization and mapping performance than the traditional ORB-SLAM algorithm in low-light and low-texture environments.

[0059] Step 1: Obtain RGB-D camera parameters through camera calibration: Set RGB-D camera parameter information including image resolution (Width, Height); camera focal length (F). x F y Optical center position C x C y ; Depth Map Factor; Camera distortion parameters K1, K2, P1, P2; Extrinsic parameter transformation matrix T between RGB lens and depth lens; Video stream frame rate fps.

[0060] Step 2: Configure algorithm parameters including the number of feature points per frame (NumFeatures); the radius of the corner neighborhood (r); the feature extraction pyramid scale (ScaleFactor); and the number of pyramid levels (NumLevels).

[0061] Step 3: Feature point windowing matching:

[0062] Step 3.1: Extract corner points from the acquired RGB images of the previous and current frames, and divide the corner points into windows. To enhance the rotation invariance of feature points, the gray-scale centroid method is used to detect the orientation consistency of feature points.

[0063] Calculate the quality-weighted sum of pixel values ​​within the radius r of corner point O:

[0064]

[0065] m 10 The weighted sum in the x-direction of the image, m 01 The centroid C of the image can be obtained by using the matrix as a weighted sum along the y-direction.

[0066] Connecting corner point O and centroid C yields the direction vector. The direction angle θ of the feature point can then be obtained as: θ = arctan(m 01 / m 10 ).

[0067] Step 3.2: Set the direction vector Align with the horizontal direction, then extract binary descriptors, and obtain initial matching results by filtering the binary descriptors.

[0068] Step 3.3: Set a 31×31 pixel window centered on the initially matched feature points, and remove the matching point pairs within the window except for those with the smallest Hamming distance to obtain the final matching point pairs.

[0069] Step 4: Analyze ICP to solve for camera pose

[0070] Step 4.1: Establish an error function based on the matching point pairs that is only related to the Euclidean transformation R and t:

[0071]

[0072] Step 4.2: Solve the pose using the SVD decomposition method based on the least squares error model established in Step 4.1. Define the centroid-free coordinates as q. i ,q i ′:q i =p i -p,q i ′=p i If '-p', then the function for solving the rotation matrix R is:

[0073]

[0074] Expanding the above equation, we get:

[0075]

[0076] Define a 3×3 matrix W = q based on the centroid coordinates. i q i T Perform SVD decomposition on W: W = UΣV T In the formula, Σ represents a diagonal matrix composed of singular values, and U and V represent orthogonal matrices, then R = UV. T ,t=p-Rp′.

[0077] Step 4.3: Establish a pair of verification point sets S, S′: S = {s1, ... s2} in the corresponding windows of the two frames. m}, S′={s1′,...s′ m Based on the already solved R, t, the point set S is transformed into S″: S″ = RS′ + t. Then, the reprojection error of the matching points in the corresponding window is: e j =||s j ′-s j "|| 2 The minimum reprojection error of the window between two frames is: e = min{e1, e2, ... e} m}, determine whether e meets the requirements.

[0078] Step 4.4: If the output e of step 3.3 does not meet the requirements, the new camera observation is repeated from step 2.1. If e meets the requirements, the feature points in the current frame that have established a matching relationship with the previous frame are named keypoints. The final camera pose and landmark positions are obtained through joint BA optimization of keypoints and pose. The least squares error objective function constructed by BA optimization is:

[0079]

[0080] Where z ij This indicates that in state i, p j Observations at key points, h(ξ) i ,p j ) indicates that in state i, p j Expected observations for keypoint calculation.

[0081] Step 5: After outputting the keypoints and camera pose corresponding to the latest frame image in Step 4, the result is input into the local mapping thread of ORB-SLAM. In the ORB-SLAM local mapping thread, local BA optimization is performed by combining local historical keypoints and camera pose to obtain the locally optimal continuous camera pose and corresponding observation landmarks.

[0082] Step 6: Input the key points and camera poses output in Step 4 into the ORB-SLAM map atlas thread. The ORB-SLAM map atlas thread generates a BOW2 bag-of-words model for the key points. The bag-of-words model is used to detect whether the mobile robot has ever passed through a certain place and caused a loop. If the mobile robot has been there and there is an inactive map of that place in the map atlas (the map saved due to the interruption of historical mapping is named an inactive map), then the inactive map and the active map (the map that the layout mapping thread is running) are fused together using the historical pose of the loop and the current pose.

[0083] Step 7: Input the keypoints and camera poses output from Step 4 into the ORB-SLAM loop closure detection thread. The ORB-SLAM loop closure detection thread uses the keypoints to generate a BOW2 bag-of-words model. The bag-of-words model is used to detect whether the mobile robot has traversed a certain location and experienced a loop closure. If a loop closure occurs, a loop closure BA optimization is constructed, starting from the historical image frame where the loop closure occurred and ending with the current input frame, constituting the mobile robot pose and its keypoints for the pose loop. Loop closure BA optimization can utilize loop closure information to achieve trajectory closure optimization, correcting localization errors and further improving map consistency.

[0084] This invention also addresses the aforementioned technical logic by designing an RGB-D visual synchronous localization and mapping system based on windowed matching estimation, applied to a mobile robot equipped with an RGB-D camera. This system aims to reduce the probability of tracking loss and improve the accuracy of localization and global mapping, comprising the following steps:

[0085] The feature point matching module is used to acquire new input image frames through an RGB-D camera, perform Fast corner detection, detect the direction of the detected corners, rotate the corner directions to horizontal, generate binary descriptors using the processed corners, and perform feature point matching.

[0086] The pose generation module is used to obtain the camera pose by parsing the ICP method based on the feature point windowing matching results.

[0087] The localization and mapping module is used to perform synchronous localization and global mapping of the mobile robot based on the ORB-SLAM algorithm, according to the camera pose and key points obtained by feature point matching.

[0088] The feature point matching module of the synchronous positioning and mapping system mentioned in this invention performs matching filtering in a window during the feature point matching process, retaining only the pair of matching results with the smallest Hamming distance in the window for camera pose solving.

[0089] The feature point matching module of the synchronous positioning and mapping system mentioned in this invention extracts corner points from the acquired RGB images of the previous and current frames during the window matching process, divides the corner points according to the window, and obtains the direction vector based on the centroid and position of the corner points.

[0090] Align the direction vector with the horizontal direction, then extract the binary descriptor, and obtain the initial matching result by filtering the binary descriptor;

[0091] A 31×31 pixel window is set with the initially matched feature points as the center. Matching point pairs other than those with the smallest Hamming distance are removed from the window to obtain the final matching point pairs, which are used as the feature point windowing matching results.

[0092] The feature point matching module of the synchronous positioning and mapping system mentioned in this invention performs orientation consistency detection on feature points by using the gray-scale centroid method during the process of dividing corner points into windows.

[0093] The feature point matching module of the synchronous localization mapping system mentioned in this invention calculates a quality-weighted sum of pixel values ​​within the radius r of corner point O during the acquisition of the direction vector: Where, m 10 The weighted sum in the x-direction of the image, m 01 This is the weighted sum in the y-direction of the image;

[0094] The centroid C of the image is obtained using a matrix:

[0095] Connect the corner point O and the centroid C to obtain the direction vector.

[0096] The pose generation module of the synchronous positioning and mapping system mentioned in this invention establishes an error function that is only related to the Euclidean transformation R and t based on the matching point pairs of the feature point windowing matching results during the process of acquiring the camera pose, and uses the SVD decomposition method to solve the pose.

[0097] Based on the pose solution, the minimum reprojection error of the window between the two frames is obtained by forming a pair of verification point sets by establishing a matching relationship between the feature points in the corresponding windows of the two frames.

[0098] Obtain the set of verification points that meet the requirements for minimum reprojection error of the window, and use them as key points. Based on the pose solution results, perform joint landmark point BA optimization to obtain the final landmark point position and camera pose. Global mapping is performed using the landmark point position.

[0099] In the pose generation module of the synchronous positioning and mapping system mentioned in this invention, the error function is expressed as follows during the acquisition process:

[0100]

[0101] Where, p i p′ represents the camera coordinates of the i-th feature point in the previous frame. i Indicates the time interval p in the next frame. i Matched feature point p′ i The camera coordinates are given. p and p′ represent the centroids of feature points in the preceding and following frames, respectively. The calculation method is as follows:

[0102]

[0103] R and t represent the rotation and translation matrices between the camera coordinate systems of the two frames, respectively.

[0104] The pose generation module of the synchronous localization mapping system mentioned in this invention, during the pose solving process, uses an error function and defines the centroid-free coordinates as q. i ,q i ′:q i =p i -p,q i ′=p i '-p', obtain the solution function for the rotation matrix R, where the solution function is expressed as:

[0105]

[0106] Based on the centroid-free coordinates, define a 3×3 matrix W = q i q i T Perform SVD decomposition on W: W = UΣV T In the formula, Σ represents a diagonal matrix composed of singular values, and U and V represent orthogonal matrices, then R = UV. T ,t=p-Rp′.

[0107] The pose generation module of the synchronous localization mapping system mentioned in this invention, during the BA optimization process, constructs an error least squares objective function as follows:

[0108]

[0109] Among them, z ij This indicates that in state i, p j Observations at key points, h(ξ) i ,p j ) indicates that in state i, p j Expected observations for keypoint calculation.

[0110] The localization and mapping module of the synchronous localization and mapping system mentioned in this invention takes the acquired camera pose and key points as input during the synchronous localization and global mapping of the mobile robot, and inputs them into the local mapping thread of ORB-SLAM. In the local mapping thread of ORB-SLAM, local BA optimization is performed by combining local historical key points and camera pose to obtain the locally optimal continuous camera pose and corresponding observation landmarks.

[0111] The input results are fed into the ORB-SLAM map atlas thread. The ORB-SLAM map atlas thread generates a BOW2 bag-of-words model for the key points. The BOW2 bag-of-words model is used to detect whether the mobile robot has ever passed through a certain place and caused a loop. If the mobile robot has been there and there is an inactive map of that place in the map atlas, the inactive map and the active map are fused together using the historical pose of the loop and the current pose.

[0112] The input results are fed into the ORB-SLAM loop closure detection thread. The ORB-SLAM loop closure detection thread uses key points to generate a BOW2 bag-of-words model. The bag-of-words model is used to detect whether the mobile robot has passed through a certain place and a loop has occurred. If a loop has occurred, a loop closure BA optimization is constructed, which starts from the historical image frame where the loop occurred and ends with the current input frame, constituting the mobile robot pose and its key points.

[0113] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0114] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0115] In the description of this invention, it should be understood that the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0116] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A method of RGB-D visual simultaneous localization and mapping based on windowed matching estimation, characterized in that, The method is applied to a mobile robot provided with an RGB-D camera, and is used for reducing the probability of tracking loss and improving the accuracy of positioning and global mapping, and comprises the following steps: extracting corner points from acquired previous frame and current frame RGB images through the RGB-D camera, dividing the corner points according to windows, and obtaining a direction vector according to a corner point centroid and a corner point position; aligning the direction vector with a horizontal direction, and then extracting a binary descriptor, and obtaining an initial matching result through screening of the binary descriptor; setting a 31*31 pixel window with the initial matching feature point as a center, eliminating matching point pairs in the window except for a minimum Hamming distance, obtaining a final matching point pair as a feature point window matching result; obtaining a camera pose through an ICP method according to the feature point window matching result; in a process of obtaining the camera pose, establishing an error function related to only a Euclidean transformation R and t according to the matching point pair of the feature point window matching result based on an ORB-SLAM algorithm, and solving the pose by using an SVD decomposition method; obtaining a window minimum re-projection error between the previous frame and the current frame through a pair of verification points composed of feature points having a matching relationship in corresponding windows of the previous frame and the current frame according to a pose solving result; obtaining a verification point set of the window minimum re-projection error meeting a requirement as a key point, and performing joint landmark point BA optimization according to the pose solving result to obtain a final landmark point position and the camera pose, wherein global mapping is performed through the landmark point position.

2. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 1, wherein: in a process of performing feature point matching, matching screening is performed in a window, and only a matching result of a pair of points having a minimum Hamming distance in the window is reserved for camera pose solving.

3. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 2, wherein: in a process of dividing corner points according to windows, a feature point is detected for direction consistency by using a gray centroid method.

4. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 3, wherein: In the process of acquiring the direction vector, the pixel value within the corner point O corner point radius r is calculated for a quality weighted sum: wherein m 10 is the weighted sum in the x direction of the image, m 01 is the weighted sum in the y direction of the image; The centroid C of the image is found by matrix operations: a direction vector is obtained by connecting a corner point O and a centroid C.

5. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 4, wherein: in a process of obtaining an error function, the error function is expressed as: Where, p i p′ represents the camera coordinates of the i-th feature point in the previous frame. i Indicates the time interval p in the next frame. i Matched feature point p′ i The camera coordinates are: p and p′ represent the centroids of the feature points in the two frames respectively; R and t represent the rotation and translation matrices between the camera coordinates of the two frames respectively.

6. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 5, wherein: In the process of pose solving, based on the error function, by defining the decentered coordinates as q i , q i ′ : q i = p i -p, q i ′= p i ′-p′, a solving function of the rotation matrix R is obtained, wherein the solving function is expressed as: According to the decentered coordinates, a 3x3 matrix W=q i q i T SVD decomposition is performed on W: W=U∑V T where ∑ represents a diagonal matrix composed of singular values, U and V represent orthogonal matrices, and R=UV T t=p-Rp'.

7. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 6, wherein: in a process of performing BA optimization, a least square error objective function constructed by BA optimization is: where z ij represents the observation of p j key points in state i, h(ξ i , p j ) represents the expected observation of p j key points in state i.

8. The RGB-D visual simultaneous localization and mapping method based on window matching estimation according to claim 7, wherein: In the process of synchronous localization and global mapping of the mobile robot, the camera pose and the key points obtained are input as input results into a local mapping thread of ORB-SLAM, and joint local historical key points and camera poses are locally optimized in the local mapping thread of ORB-SLAM to obtain locally optimal camera continuous poses and corresponding observation landmarks; The input results are input into a map integration thread of ORB-SLAM, a BOW2 bag-of-words model is generated for the key points through the map integration thread of ORB-SLAM, and whether the mobile robot has ever occurred loop in a certain place is detected using the BOW2 bag-of-words model; if the mobile robot has ever come and there is an inactive map about the place in the map integration, the inactive map and an active map are fused using a historical pose and a current pose in which the loop occurs; The input results are input into a loop detection thread of ORB-SLAM, a BOW2 bag-of-words model is generated for the key points through the loop detection thread of ORB-SLAM, and whether the mobile robot has ever occurred loop in a certain place is detected using the bag-of-words model; if the loop occurs, a loop BA optimization of the mobile robot pose and the key points is constructed with a historical image frame in which the loop occurs as a starting point and a current input frame as an end.

Images