CN112802096A - Device and method for realizing real-time positioning and mapping - Google Patents

Abstract

Provided are an implementation device and method for real-time positioning and mapping. The implementation device includes: an image acquisition unit configured to acquire a color image and a depth image of the surrounding environment; an initial pose estimation unit configured to estimate an initial pose based on the color image and the depth image; a map construction unit, is configured to construct a three-dimensional map based on the depth image and the color image; and a pose determination unit is configured to determine a final pose based on the initial pose and the three-dimensional map.

Description

Device and method for real-time positioning and mapping

technical field

The present application relates to the field of real-time positioning and mapping (SLAM), and more particularly, to an apparatus and method for implementing SLAM based on a reconstructed map.

Background technique

In the method for constructing a three-dimensional map in the prior art, sensing devices such as cameras, laser sensors and inertial measuring instruments installed on the equipment are usually used to obtain information of the surrounding environment in real time, thereby constructing a three-dimensional map of the surrounding environment. Map, and output the position and attitude information of the device in the world coordinate system in real time. This technology is called real-time positioning and mapping (SLAM, Simultaneous Localization And Mapping).

Almost all SLAM systems are based on two frameworks: one is nonlinear optimization and the other is statistical filtering. According to the different hardware sensing devices, SLAM systems can also be divided into laser sensors, binocular/polyocular, monocular + inertial measurement equipment, and monocular + depth camera.

Laser sensing technology is a commonly used technology in early SLAM systems. It has high accuracy and can obtain relatively dense map points. However, due to the large size, weight and high cost of laser equipment, it is not suitable for the current lightweight SLAM. The requirements of the system (such as drones, AR glasses, etc.), but still have value in some equipment that does not require high loads (such as sweeping robots, warehouse robots). Another technology is the binocular/multi-eye SLAM system, which can estimate the approximate depth information of the feature points in the scene according to the disparity between the two cameras, and then further refine the estimated value obtained by the front end in the back-end optimization process. change. The more classic binocular SLAM technologies include OKVIS and ORB-SLAM.

In addition, the current vision-based SLAM technology has been gradually applied to consumer products. However, when a monocular camera is used in a SLAM system, there are often some problems, such as the need for a relatively fine initialization process, difficulty in estimating the scale of the motion trajectory, and so on. Therefore, some changes for monocular cameras have been studied in depth. Currently, the more classic ones are monocular camera plus inertial sensor, monocular camera plus depth camera, and some fine initialization algorithms. Inertial sensors are sensitive to device motion, so they can make up for the lack of monocular cameras, and can accurately estimate the device's pose when the device moves quickly or rotates a lot. The monocular camera plus the depth camera can roughly measure the three-dimensional spatial position of the scene, so the scale of the motion trajectory can be obtained. Monocular cameras are widely used in many consumer products (such as drones, AR glasses, automatic food delivery robots, etc.) due to their lightness.

However, nonlinear optimization is included in most existing SLAM technologies, but real-time performance and accuracy are difficult to balance in nonlinear optimization. Many existing SLAM systems perform nonlinear optimization in order to ensure Real-time, only the measurement information of a short period of time in the past can be used, and it is difficult to obtain better positioning accuracy. Additionally, some SLAM systems may need to frequently work in familiar environments, such as warehouse robots and food delivery robots. How to use the previous map information to further reduce the computational load of the system over time is relatively scarce. Many vision-based SLAM technology front-ends use feature point extraction and feature point matching to initially track the position of the camera, but in some scenes with relatively poor textures (such as glass rooms, rooms with many white walls), it is possible to extract and match. Fewer feature points will reduce the tracking accuracy, resulting in a larger drift error.

Therefore, there is a need for a SLAM technology that can ensure real-time performance, reduce the amount of computation, and also ensure map accuracy.

The above information is presented as background information only to assist with an understanding of the present disclosure. No determination has been made, and no claim is made, as to whether any of the above information is applicable as prior art with regard to the present disclosure.

SUMMARY OF THE INVENTION

According to an exemplary embodiment of the present invention, an apparatus for realizing real-time positioning and mapping is provided, including: an image acquisition unit configured to acquire color images and depth images of the surrounding environment; an initial pose estimation unit configured to Based on the color image and the depth image, an initial pose is estimated; a map construction unit is configured to construct a three-dimensional map based on the depth image and the color image; and a pose determination unit is configured based on the initial pose and the three-dimensional map, Determine the final pose.

The map construction unit may include: a map reconstruction module configured to reconstruct an initial three-dimensional map based on a first depth image and a first color image of the surrounding environment; a map update module configured to reconstruct a second three-dimensional map based on the surrounding environment A depth image and a second color image update the three-dimensional map.

The initial pose estimation unit may include: a feature extraction module configured to extract point features in the color image; a feature matching module configured to perform point matching according to the point features; an estimation module configured to use the matched point features , estimating the initial pose. When the number of matched point features is less than the first threshold, the feature extraction module may further extract line segment features in the color image, the feature matching module may perform line segment matching according to the line segment features, and the estimation module may use the matched point features and Line segment features to estimate the initial pose.

The feature matching module can perform line segment matching according to the geometric structure information around the line segment feature.

The implementing apparatus may further include: a full-image tracking module configured to determine a key frame having a common viewpoint with the current frame among the previous key frames. The initial pose estimation unit may use the determined keyframe to estimate an initial pose based on the color image and the depth image.

The implementation device may further include: a point-line optimization combination module configured to establish a three-dimensional collinearity constraint based on the matching result of the line segment features. The pose determination unit may also determine the final pose according to the three-dimensional collinearity constraint. The three-dimensional collinearity constraint may represent that a point on a line segment of the first frame is on a corresponding line segment of the second frame.

The pose determination unit may use a preset frame set to determine the final pose based on the initial pose and the three-dimensional map. The preset frame set may include: a plurality of tracking subsets obtained by dividing the tracking set related to the current frame.

The pose determination unit may set a time domain window of a predetermined size for the current frame, and based on the initial pose and the three-dimensional map, determine the final pose by using key frames within the time domain window.

According to another exemplary embodiment of the present invention, a method for realizing real-time positioning and mapping is provided, the method includes: acquiring a color image and a depth image of a surrounding environment; based on the color image and the depth image, estimating an initial pose; build a three-dimensional map based on the depth image and the color image; and determine a final pose based on the initial pose and the three-dimensional map.

The step of constructing a three-dimensional map may include: reconstructing an initial three-dimensional map based on a first depth image and a first color image of the surrounding environment; updating the three-dimensional map based on a second depth image and a second color image of the surrounding environment map.

The step of estimating the initial pose may include: extracting point features in the color image; performing point matching according to the point features; and estimating the initial pose using the matched point features. When the number of matched point features is less than the first threshold, the step of estimating the initial pose may further include: extracting line segment features in the color image, performing line segment matching according to the line segment features, and using the matched point features and line segment features to estimate the initial pose.

The step of performing line segment matching according to the line segment feature may include: performing line segment matching according to geometric structure information around the line segment feature.

The method may further include: determining a key frame having a common viewpoint with the current frame among the previous key frames, wherein the step of estimating the initial pose may include: using the determined key frame based on the color image and the depth image. Estimate the initial pose.

The method may further include: establishing a three-dimensional collinearity constraint based on the matching result of the line segment features. The step of determining the final pose may include determining the final pose also based on the three-dimensional collinearity constraint. The three-dimensional collinearity constraint may represent that a point on a line segment of the first frame is on a corresponding line segment of the second frame.

The step of determining the final pose may include: using a preset frame set to determine the final pose based on the initial pose and the three-dimensional map. The preset frame set may include: a plurality of tracking subsets obtained by dividing the tracking set related to the current frame.

The step of determining the final pose may include: setting a time domain window of a predetermined size for the current frame, and based on the initial pose and the three-dimensional map, using key frames within the time domain window to determine the final pose.

According to an exemplary embodiment of the present invention, there is provided a computer-readable storage medium storing instructions, wherein the instructions, when executed by at least one computing device, cause the at least one computing device to perform the aforementioned real-time positioning and mapping implementation method.

According to an exemplary embodiment of the present invention, there is provided a system comprising at least one computing device and at least one storage device storing instructions, wherein the instructions, when executed by the at least one computing device, cause the at least one computing device to The computing device executes the aforementioned implementation method of real-time positioning and mapping.

beneficial effect

By applying the SLAM scheme according to the exemplary embodiment of the present invention, at least the following effects can be achieved:

Since the SLAM system reconstructs a map with spatial accuracy through 3D semantics when it first enters a specific environment, and fuses and improves the map by calculating the spatial and temporal confidence every time it enters the specific environment, it can ensure that the map In the back-end optimization process, since the coordinates of the 3D map points can be fixed, only the pose (ie, position and attitude) of the SLAM system equipment is optimized, thus making the entire SLAM system lighter;

When the entire SLAM system enters the scene area with poor texture, it can also extract and match the line segment features of the scene, and add the three-dimensional space point-line collinearity constraint sub-item in the back-end optimization to make the entire SLAM system more robust;

By performing feature tracking on the global map, more effective constraints can be obtained and the positioning accuracy of the SLAM system can be improved; through the feature re-identification feature, on the basis of the original time domain space 2d-2d matching or 3d-2d matching, more features are added. In the spatial matching, these visual measurements are added to the energy functions of the local and global polar beam optimization, which further reduces the cumulative error;

When the global beam adjustment method is implemented, the original long feature tracking set can be decomposed into several small feature tracking subsets, which improves the computational efficiency;

When implementing the global beam adjustment method, when a new keyframe arrives, the 3D map points and poses of all the keyframes involved can no longer be optimized, but only the preset time domain window associated with the new keyframe can be optimized Several keyframes within the system further improve the efficiency of back-end optimization.

Other aspects, advantages and salient features of the invention will become apparent to those skilled in the art from the following detailed description, which, taken in conjunction with the accompanying drawings, discloses various embodiments of the invention.

Description of drawings

The above and other aspects, features and advantages of specific embodiments of the present disclosure will become more apparent from the following description taken in conjunction with the accompanying drawings, wherein:

FIG. 1 is a block diagram illustrating a map reconstruction based SLAM system according to an exemplary embodiment of the present invention.

FIG. 2 is a schematic diagram illustrating a processing flow of a SLAM system based on map reconstruction according to an exemplary embodiment of the present invention.

3 is a block diagram illustrating an initial pose estimation unit in a map reconstruction-based SLAM system according to an exemplary embodiment of the present invention.

4 is a block diagram illustrating a map construction unit in a map reconstruction-based SLAM system according to an exemplary embodiment of the present invention.

FIG. 5 shows a schematic diagram of the generation of the line segment feature descriptor vector.

Figure 6 shows a schematic diagram of a feature re-identification operation.

FIG. 7 shows a schematic diagram of key frame screening of the full-image tracking module.

FIG. 8 shows a schematic diagram of a full-image tracking flow of the full-image tracking module.

Figure 9 shows a schematic diagram of the division of the feature tracking set

Figure 10 shows a schematic diagram of the global beam adjustment based on the time domain window.

FIG. 11 shows a schematic diagram of the closed loop error cancellation process.

FIG. 12 is a flowchart illustrating a map reconstruction-based SLAM method according to an exemplary embodiment of the present invention.

Throughout the drawings, it should be noted that the same reference numerals are used to refer to the same or similar elements, features and structures.

Detailed ways

In order for those skilled in the art to better understand the present invention, the exemplary embodiments of the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

FIG. 1 is a block diagram illustrating a map reconstruction based SLAM system 100 according to an exemplary embodiment of the present invention.

1 , a SLAM system 100 based on map reconstruction (hereinafter referred to as “SLAM system 100 ”) includes an image acquisition unit 110 , an initial pose estimation unit 120 , a map construction unit 130 , a pose determination unit 140 and a storage unit 150 .

The image acquisition unit 110 may acquire the color image and the depth image of the surrounding environment where the SLAM system 100 is currently located, as the color image and the depth image of the current frame to be processed. Also, the “previous frame” mentioned below refers to a frame that has been processed by the SLAM system 100 according to an exemplary embodiment of the present invention and related information is stored in the storage unit 150 . The image acquisition unit 110 may include a monocular camera and a depth camera to obtain the color image and the depth image, respectively, however, it should be understood that the present application is not limited to this, and any other device capable of obtaining the depth image and the color image may also be used. The image acquisition unit 110 is implemented by a camera or a combination of cameras (eg, a binocular camera). The color image may be an RGB image, or may be other types of color images (eg, grayscale images) capable of providing pixel grayscale information.

The initial pose estimation unit 120 may estimate the initial pose of the SLAM system 100 based on the color image and the depth image. In an exemplary embodiment of the present invention, the initial pose may be a relative pose, which will be explained in detail later with reference to FIG. 3 .

FIG. 2 is a schematic diagram illustrating a processing flow of the map reconstruction-based SLAM system 100 according to an exemplary embodiment of the present invention.

2 , it can be seen that the main processing flow of the SLAM system 100 includes: pre-integration of inertial navigation data, feature tracking, new feature extraction, feature re-identification, global beam adjustment and local beam adjustment.

In the pre-integration process of inertial navigation data, the relative attitude can be estimated according to the collected inertial navigation data. The input of the process is the inertial navigation data, and the output is the relative attitude; in the feature tracking process, the initial extracted features can be tracked, The input of this process is the initial extraction feature, and the output is the feature tracked in the next frame; in the new feature extraction process, the feature can be extracted from the image acquired by the image acquisition unit 110, the input of this process is a color image, and the output is the feature ; In the feature re-identification process, the feature that has been extracted can be found, the input of this process is the original feature and the pose result of the previous frame, and the output is the feature after the feature re-identification process, and the specific implementation can be explained with reference to the following Figure 6; In the global beam adjustment processing, global nonlinear optimization can be performed, the input of the processing is the feature matching set, and the output is the pose; in the local beam adjustment processing, local nonlinear optimization can be performed, and the input of the processing is the feature matching. Set, the output is the pose.

FIG. 3 is a block diagram illustrating the initial pose estimation unit 120 in the map reconstruction-based SLAM system 100 according to an exemplary embodiment of the present invention.

3 , the initial pose estimation unit 120 may include: a feature extraction module 121, configured to extract features from the color image acquired by the image acquisition unit 110; a feature matching module 122, configured to perform feature matching according to the features; and an estimation module 123 configured to estimate the initial pose of the SLAM system 100 using the matched features.

In an exemplary embodiment of the present invention, after the feature extraction module 121 completes the feature extraction, the feature matching module 122 may obtain the three-dimensional coordinates of the extracted features in the camera coordinate system of the current frame according to the depth information of the depth image, and then according to The local information description of the feature finds its matching feature on another frame, and obtains the three-dimensional coordinates of the matching feature in the camera coordinate system of the other frame. In an exemplary embodiment of the present invention, the another frame may be a specific frame among the previous frames, which will be described in more detail later. These feature-related information obtained by the feature extraction module 121 and the feature matching module 122 may be stored in the storage unit 150 as the related information for the current frame to be used in subsequent processing.

For example only, the feature extraction module 121 may extract point features in the color image of the current frame, and the feature matching module 122 may perform point matching according to the point features. More specifically, the feature matching module 122 may look for point features that match the point features extracted by the feature extraction module 121 in a specific frame among the previous frames. Afterwards, the estimation module 123 may use the matched point features to estimate the initial pose of the SLAM system 100 . The method for feature matching using point features is known to those skilled in the art, so for the sake of brevity, it will not be described in detail here.

In addition, when the environment texture in which the SLAM system 100 is located is relatively lacking (eg, in a glass room, a room with relatively uniform colors, etc.), the number of matching point features that can be obtained is small, which will lead to errors in the SLAM system 100 larger or directly ineffective. Therefore, preferably, in an exemplary embodiment of the present invention, when the number of point features matched by the feature matching module 122 is less than the first preset threshold (for example, 20), the feature extraction module 121 may also Line segment features are further extracted from the color image of the current frame, and the feature matching module 122 can also perform line segment matching according to the line segment features, that is, search for line segment features matching the line segment features extracted by the feature extraction module 121 in the specific frame. At this point, the estimation module 123 may use both the matched point features and line segment features to estimate the initial pose of the SLAM system 100 .

In an exemplary embodiment of the present invention, in order to perform feature matching using the extracted line segment features, a feature descriptor may be set for the line segment. At this time, due to the lack of environmental textures, the utilization efficiency of local grayscale information around the line segment features is low. Therefore, in an exemplary embodiment of the present invention, the line segment features can be described according to the relative shape information between the line segments. Specifically, any line segment extracted from the color image can be used as a reference line segment, and then feature descriptors can be established according to the relative positions and relative angles of all remaining line segments in the color image with respect to this reference line segment. Use the midpoint coordinates of the line segment to indicate its position. For example, for any line segment _i , the vector form of its descriptor vi (k) can be expressed as:

v _i (k)=#{q≠ _pi :( _qpi )∈bin(k)} (1)

Among them, bin(k) represents the kth region among the multiple regions in the color image, k is the region label, pi is the midpoint coordinate of line segment _i , and q represents the midpoint of other line segments except line segment i coordinate.

FIG. 5 shows a schematic diagram of the generation of line segment feature descriptor vectors. In FIG. 5 , {v ₁ , v ₂ ,...,v _n } respectively indicate the first area, the second area,..., and the first area in the 4×4 block area shown in FIG. 5 of the color image Line segment feature descriptor vector in n regions.

The feature matching module 122 may perform line segment matching according to the geometric structure information around the line segment feature. Specifically, the feature matching module 122 may calculate the Euclidean distance between the line segment feature descriptor vectors of the current frame and the specific frame, so as to find the best line segment matching. Here, in order to ensure the correct rate of matching, in an exemplary embodiment of the present invention, it can be set that the best line segment matching must be the best matching of both line segments in the corresponding images.

In an exemplary embodiment of the present invention, since the depth image of the current frame may provide depth information, the estimation module 123 may use the matched point feature and/or line segment feature to compare the current frame with the The pose changes between the specific frames are estimated to obtain the initial pose of the SLAM system 100 . That is, the initial pose of the SLAM system 100 obtained for the current frame is the relative pose with respect to the specific frame. In addition, it should be understood that the present application is not limited thereto, and various other suitable methods may also be used to estimate the initial pose of the SLAM system 100 .

In addition, in the exemplary embodiment of the present invention, when the SLAM system 100 enters a specific environment for the first time, the estimation module 123 may directly set the initial pose of the SLAM system 100 to a zero vector at this time.

Furthermore, in an exemplary embodiment of the present invention, when the number of matched point features is very small, for example, when the number of point features matched with the point features extracted by the feature extraction module 121 in the specific frame is less than the second When the preset threshold (for example, 5), the relative pose estimated by point feature matching is no longer reliable, and at this time, the estimation module 123 may determine the initial pose of the SLAM system 100 to be the same as that in the specific frame. The relative pose of the SLAM system 100 remains the same.

Furthermore, in an exemplary embodiment of the present invention, the specific frame may be a previous frame or several frames in the vicinity of the current frame. However, if the constraints for the current frame are established using only the relationship to a few frames near the current frame, then when the SLAM system 100 runs faster or rotates more, fewer frames can be associated with the current frame, The accuracy of the SLAM system 100 may be affected. Therefore, preferably, the SLAM system 100 according to the exemplary embodiment of the present invention may further include a full-image tracking module (not shown). The full-image tracking module (not shown) may find a co-view key frame (ie, a co-view key frame in a previous frame (eg, a co-view feature point) with the current frame that has a co-view point (eg, a co-view feature point) among previous frames (eg, a previous key frame). The co-view point can be seen in both the current frame and these co-view key frames, and there is a projection point corresponding to the co-view point), and these co-view key frames can be used to further establish constraints for the current frame. In this case, the specific frame may also include these co-view key frames. That is to say, the feature matching module 122 can also look for features matching the features (including point features and line segment features) extracted by the feature extraction module 121 in the co-view key frame, so that the estimation module 123 can further use these matched features Estimate the initial pose. The full-image tracking module will be described in detail below with reference to FIG. 6 , FIG. 7 and FIG. 8 .

Figure 6 shows a schematic diagram of a feature re-identification operation. FIG. 7 shows a schematic diagram of key frame screening of the full-image tracking module. FIG. 8 shows a schematic diagram of a full-image tracking flow of the full-image tracking module.

Referring to Figure 6, the upper left image extracts a point feature, labeled 6993, which is tracked from the image of frame number 1035 to the image of frame number 1042, and then in the image of frame number 1043, this feature tracking is lost, because in this In this image, a ladder blocks this feature. But in the frame number 1723 image, this feature is re-identified. The angle between the frame where the feature is extracted (ie, the frame with frame number 1035) and the frame where this feature is re-identified (ie, the frame with frame number 1723) is 67.8 degrees, and the translation distance is about 3 meters. Frames of frame numbers 1035 to 1042 share a common view feature point 6993 with the frame of frame number 1723, therefore, frames of frame numbers 1035 to 1042 can also be used to establish constraints for the frame of frame number 1723. This can increase the number of features that generate constraints between frames and find broader and more stable constraints for the current frame (ie, find more frames that can establish constraints on the current frame), thereby improving the SLAM system 100 positioning accuracy. precision.

Optionally, since the initial pose estimation has been performed using the previous frame or a nearby frame of the current frame, it is not meaningful to use the common-view key frame that is closer to the current frame to perform the initial pose estimation. In an exemplary embodiment of the present invention, preferably, the initial pose estimation unit 120 may only select and use the co-view key frames whose time distance from the current frame exceeds a preset time threshold for initial pose estimation. This is further described later with reference to FIG. 7 .

When selecting a keyframe that matches the current frame, due to computational burden, keyframe screening is required. Here, a strategy combining time and space can be used to screen keyframes.

For temporal screening, only as an example, referring to FIG. 7 , the key frames that are closer to the current frame in time, such as K _t , K _t+1 , K _t+2 and other frames in the figure, are close to the current frame due to the , then there are many common viewpoints between them, so adding a few common viewpoints has little effect on the final accuracy, so a time threshold can be set to find matching features in key frames that are far away from the current frame. Improve computational efficiency. In addition, from a spatial point of view, a series of screening operations are also required, such as feature filtering, baseline larger feature matching, and feature classification for points on key frames. When the keyframes containing all the points sensitive to the spatial and temporal conditions (ie, the points filtered out by the spatial and temporal conditions) are processed, the keyframe filtering is completed.

The process of spatially filtering points can be as follows:

First, unstable points can be filtered out. The inverse depth of each map point is iteratively optimized in the full-map beam adjustment, so the change of the inverse depth of each map point over a period of time can be calculated. If the change is too large or its inverse depth is less than zero, it is considered that This point is unstable, and remove this point.

In addition, points at the edge of the image can be removed. Here, through the preliminary estimation of the camera pose (for example, by using the initial pose), usually the projected point of the map point will be projected around the corresponding feature point on the current frame, if the calculated difference between the projected point and the corresponding feature point If the spatial distance is too large, the map point can be filtered out.

Afterwards, a description distance (ie, a descriptor) between the projection point and the corresponding feature point can be calculated, and then when the description distance is smaller than a certain threshold, a feature re-identification operation can be performed.

Due to the point sensitive to the spatial and temporal conditions, the baseline of the corresponding point between the current frame and the corresponding point is often too wide, which is prone to false matching. Therefore, in an exemplary embodiment of the present invention, the process described below in conjunction with FIG. 8 can be used to determine Common view keyframes.

Just as an example, as shown in FIG. 8 , C ₁ , C ₂ , and C ₃ are all key frames, and C ₄ is the current frame. Among them, C ₁ , C ₂ , C ₃ and C ₄ are far away, and the traditional method does not consider the constraints between C ₁ , C ₂ , C ₃ and C ₄ . However, in the exemplary embodiment of the present invention, taking the feature point P ₁ (here, the feature point P ₁ is a map point) as an example, the feature point P ₁ can be seen in the key frame C ₁ , and the full image tracking module ( (not shown) can be determined by the following operations to determine whether the key frame C ₁ is the co-view key frame of the current frame C ₄ :

(1) Project the feature point P ₁ to the current frame C ₄ according to the initial relative pose relationship estimated by the initial pose estimation unit 120, and mark it as feature point p _i ;

(2) _Calculate the spatial coordinates of the feature point _qi and the feature point _pi near the feature point pi in the current frame _C4 and the local grayscale difference d( _pi , _qi );

其中，k＝1,2,3,…,m，m为找出的特征点的数量；(3) Find the set of all feature points in the current frame _C4 whose local grayscale difference with the feature point p _i is less than the set threshold

Among them, k=1,2,3,...,m, m is the number of found feature points;

的特征描述符

分别进行比较。这里，如果关键帧C₁和当前帧C₄的相对旋转ΔR和相对平移ΔT均小于给定阈值(例如，相对旋转阈值T_ΔR＝45度，相对平移阈值T_ΔT＝2米)，则直接比较特征点p_i的ORB描述符D(p_i)和特征点

的ORB描述符

的汉明距离，否则，可如公式(3)所示先将特征点

的描述重新投射(warp)到当前帧上，然后比较ORB描述符的汉明距离；(4) Based on the following formulas (2) and (3), the feature descriptor D( _{pi ) of the feature point p i and} the feature point

feature descriptor of

Compare them separately. Here, if the relative rotation ΔR and relative translation ΔT of key frame C ₁ and current frame C ₄ are both less than a given threshold (eg, relative rotation threshold T _ΔR = 45 degrees, relative translation threshold T _ΔT = 2 meters), then a direct comparison is made _ORB descriptor D( _pi ) of feature point pi and feature point

ORB descriptor

The Hamming distance of , otherwise, as shown in formula (3), the feature points can

The description is re-projected (warp) to the current frame, and then the Hamming distance of the ORB descriptor is compared;

中的ORB描述符之间的汉明距离最小的点确定为与特征点P₁匹配的特征点，并且此时可确定关键帧C₁与当前帧C₄具有共视特征点P₁。(5) Combine the above

The point with the smallest Hamming distance between the ORB descriptors in is determined as the feature point matching the feature point P ₁ , and at this time, it can be determined that the key frame C ₁ and the current frame C ₄ have a common view feature point P ₁ .

in,

In the above description, the Hamming distance is used to indicate the distance between the feature points, but this is just an example, and other various distance representation methods may also be used to determine the distance between the feature points.

In an exemplary embodiment of the present invention, when the initial pose estimation unit 120 performs pose estimation using a plurality of frames (eg, the above-mentioned co-view key frame is used in addition to the previous frame of the current frame), the obtained When there are multiple initial poses, the initial pose estimation unit 120 may determine the obtained statistical values (eg, average, median, etc.) of the multiple initial poses as the initial pose of the SLAM system 100 .

Next, referring back to FIG. 1 , the map construction unit 130 may construct a three-dimensional map based on the depth image and the color image acquired by the image acquisition unit 110 .

FIG. 4 shows the structure of the map construction unit 130 in the SLAM system 100 based on map reconstruction according to an exemplary embodiment of the present invention.

As shown in FIG. 4 , according to an exemplary embodiment of the present invention, the map construction unit 130 may include a map reconstruction module 131 , a map storage module 132 and a map update module 133 .

The map reconstruction module 131 may reconstruct an initial three-dimensional map based on the first depth image and the first color image of the specific environment when entering the specific environment for the first time, and then the map updating module 133 may re-enter the specific environment based on the a second depth image and a second color image of the specific environment to update the 3D map

More specifically, when the SLAM system 100 enters a specific environment for the first time, the map reconstruction module 131 can reconstruct a three-dimensional map based on the depth image and color image obtained when the SLAM system 100 enters the specific environment for the first time, and determine the 3D map on the reconstructed three-dimensional map. The three-dimensional coordinates of each map point (ie, three-dimensional coordinates in the world coordinate system) and its spatial accuracy.

For example only, the map reconstruction module 131 may reconstruct a three-dimensional map using three-dimensional semantic reconstruction based on depth information included in the depth image and grayscale information included in the color image. In the process of reconstructing the three-dimensional map using the three-dimensional semantic reconstruction by the map reconstruction module 131, in addition to determining the three-dimensional coordinates of each map point on the three-dimensional map, the spatial accuracy of each map point may also be determined. The spatial accuracy may indicate a reprojection error of the calculated map points, and the calculation of the spatial accuracy is described in detail below. In addition, it should be understood that the three-dimensional semantic reconstruction method enumerated above is only one of the ways to realize the above-mentioned three-dimensional map reconstruction processing, and the present application is not limited to this, and any other suitable three-dimensional map reconstruction method known in the art can also be used to reconstruct three-dimensional maps. map and determine the spatial accuracy of each map point.

After completing the three-dimensional map reconstruction, the map reconstruction module 131 may store the three-dimensional map with spatial accuracy in the map storage module 132 . Here, since the SLAM system 100 does not obtain the spatial and temporal confidence when entering a specific environment for the first time, the spatial and temporal confidence of the map point on the three-dimensional map at this time can be set as null and stored.

After that, when the SLAM system 100 re-enters the specific environment, it is not necessary to rebuild the three-dimensional map again, but the map update module 133 calculates each map point based on the depth image and color image obtained when the SLAM system 100 re-enters the specific environment The three-dimensional coordinates and their space-time confidence of each map point are updated according to the calculated three-dimensional coordinates of each map point and their space-time confidence, and the three-dimensional coordinates of the corresponding map points and their space on the three-dimensional map stored in the map storage module 132 are updated. time confidence. The updated three-dimensional map can be used as the three-dimensional map constructed by the map constructing unit 130 for subsequent use.

Here, for a specific map point on a 3D map, its spatial-temporal confidence usually decreases over time, and its decreasing speed is usually related to the 3D semantic type of the specific map point. In an exemplary embodiment of the present invention, the spatial-temporal confidence C _c of the specific map point can be determined as the following formula (4):

表示进行三维语义重建时该特定地图点的重投影误差，并且可由以下公式(5)来计算：Wherein, t ₁ represents the current system time of the SLAM system 100 , and t ₀ represents the initial system time of the SLAM system 100 . ω1 and ω2 represent the spatial and temporal weights, respectively.

represents the reprojection error of this particular map point when performing 3D semantic reconstruction, and can be calculated by the following formula (5):

为与该特定地图点对应的投影点的二维坐标，ε表示SLAM系统100在世界坐标系下的位姿，这里可基于初始位姿估计单元120所估计出的初始位姿来获得，X表示该特定地图点在世界坐标系下的三维坐标，π是用于将输入变量转换为二维坐标的函数。In formula (5),

is the two-dimensional coordinate of the projection point corresponding to the specific map point, ε represents the pose of the SLAM system 100 in the world coordinate system, which can be obtained based on the initial pose estimated by the initial pose estimation unit 120, and X represents The three-dimensional coordinates of this particular map point in the world coordinate system, and π is the function used to convert the input variables into two-dimensional coordinates.

Therefore, the update of the three-dimensional coordinates of the specific map point and its spatial accuracy can be achieved respectively as the following formulas (6) and (7):

C _p ′=0.5×(C _p +C _c )......(7)

Wherein, X _p and C _p are respectively the three-dimensional coordinates of the specific map point stored in the map storage module 132 and their spatial and temporal confidence, and X _c is the current three-dimensional coordinates of the specific map point calculated by the map update module 133, X _p ' and C _p ' are the updated three-dimensional coordinates of the particular map point and its spatial-temporal confidence, respectively. P _r represents the initial pose of the SLAM system 100 estimated by the initial pose estimation unit 120 .

After the above-mentioned X _p ' and C _p ' are calculated, the calculated X _p ' and C _p ' can be used to replace X _p and C _p stored in the map storage module 132 respectively, thereby realizing the storage in the map storage module 132 update of the 3D map.

In the illustration of FIG. 4 , the map storage module 132 is shown as a separate module, however, it should be understood that the map storage module 132 may also be integrated with the map update module 133 as one module, by way of example only.

Through the above-mentioned update process of the map update module 133, every time the SLAM system 100 enters the same environment, only the stored 3D map can be updated without rebuilding the 3D map every time, thereby reducing the amount of computation required to obtain the 3D map , and also makes the map more and more accurate, so that the three-dimensional map points can be fixed during the subsequent use of the three-dimensional map, and only the pose of the SLAM system 100 can be optimized.

Next, referring back to FIG. 1 , the pose determination unit 140 according to an exemplary embodiment of the present invention may determine the final pose of the SLAM system 100 based on the estimated initial pose of the SLAM system 100 and the constructed three-dimensional map. In an exemplary embodiment of the present invention, the pose determination unit 140 may use a preset frame set to determine the final pose of the SLAM system 100 based on the initial pose and the three-dimensional map. The determined final pose is the pose in the world coordinate system, and the determined final pose may be stored in the storage unit 150 .

In an exemplary embodiment of the present invention, the final pose may be determined using a global beam adjustment method. Specifically, the pose determination unit 140 may perform a global beam adjustment based on the following equations (8) and (9) to determine the final pose of the SLAM system 100:

ψ ^* = argmin∑ _i∈K ∑ _j∈P ||e ^ij ||  … (8)

e ^ij =x ^ij -π(ε ^iw ,X ^wj )  … (9)

In the above formulas (8) and (9), K is a preset frame set and includes a current frame, for example, may be a set including a key frame and a current frame. P represents the set of feature points extracted by the feature extraction module 121 . e ^ij represents the reprojection error of the j-th feature point in the feature point set P on the i-th frame in the set K, and when there is no j-th feature point in the i-th frame When projecting over frames, ^eij can be set to 0. x ^ij is the two-dimensional coordinate of the projection point of the j-th feature point on the i-th frame. ε ^iw represents the pose of the SLAM system 100 in the world coordinate system for the ith frame. X ^wj represents the three-dimensional coordinates of the j-th feature point in the world coordinate system, and can be determined based on the three-dimensional map provided by the map construction unit 130 .

To find the optimal solution based on formula (8), the pose of the SLAM system 100 corresponding to each frame in the set K (ie, the pose of the SLAM system 100 when obtaining the frames) can be obtained. At this time, not only the final pose of the SLAM system 100 can be determined, but also the obtained poses of the SLAM system 100 for other frames can be used to update the previously determined corresponding poses, thereby continuously improving the positioning accuracy of the SLAM system 100 .

In addition, in the process of finding the optimal solution to the above formula (8), the initial pose determined by the initial pose estimation unit 120 can be used as a reference for ε ^iw (for example, the value of ε ^iw can be initially set as the initial pose pose, and adjust it based on the initial pose in subsequent calculations) to speed up the computation.

In an exemplary embodiment of the present invention, the key frame may be a frame selected according to a preset rule among the multiple frames stored in the storage unit 150, for example, the first frame, the fifth frame selected according to the preset interval frame, frame 9, . . ., or even the entire frame stored.

When the global beam adjustment is performed based on equations (8) and (9) to determine the final pose of the SLAM system 100, for example, for the jth feature point of the current frame, all the corresponding features of the jth feature point can be The map feature points are the co-view frames of the co-view feature points (that is, the corresponding map feature points of the j-th feature point can be seen on these co-view frames) as a feature tracking set related to the j-th feature point. (ie, the preset frame set) to participate in the calculation of the final pose as a whole (for example, in the present invention, the aforementioned full-image tracking module (not shown) can be used to find such co-view frames), thereby Maintain global consistency. However, this takes a long time, cannot meet the real-time performance of the SLAM system 100, and sometimes even has to sacrifice the accuracy of the SLAM system 100 to ensure the real-time performance.

Optionally, when performing global beam adjustment, the pose determination unit 140 may set the preset frame set to include multiple tracking subsets obtained by dividing the feature tracking set related to the current frame. More specifically, the pose determination unit 140 may divide each feature tracking set related to each feature of the current frame into one or more feature tracking subsets, respectively, and perform global beam adjustment based on each feature tracking subset to obtain the result. The final pose of the SLAM system 100 is determined. That is, the preset frame set may include one or more feature tracking subsets obtained by dividing each feature tracking set related to each feature of the current frame respectively. This is described below with reference to FIG. 9 .

FIG. 9 shows a schematic diagram of an example of division of a feature tracking set.

Referring to FIG. 9 , as an example only, it is assumed that C ₁ to C ₄ are co-view frames related to the j-th feature point of the current frame, and K is a co-view key frame related to the j-th feature point of the current frame. That is to say, the corresponding map feature points of the j-th feature point can be seen in the common-view frames C ₁ to C ₄ and the common-view key frame K, and T _j is the feature tracking of the j-th feature point The set is composed of a co-viewing key frame K and co-viewing frames C ₁ to C ₄ .

In an exemplary embodiment of the present invention, referring to FIG. 9 , the pose estimation unit 140 may divide T _j into subsets T _j1 , T _j2 , T _j3 , and T _j4 , where T _j1 consists of the common view key frame K and The common-view frames C ₁ to C ₃ are composed, T _j2 is composed of the common-view key frame K and the common-view frames C ₂ to C ₄ , and T _j3 is composed of the common-view key frame K and the common-view frames C ₁ , C ₃ and C ₄ . , and T _j4 consists of a common-view key frame K and common-view frames C ₁ , C ₂ and C ₄ .

Thus, when the final pose of the SLAM system 100 is calculated using the global beam adjustment method as in Equation (8), the set of frames involved in the sub-item related to the jth feature point in Equation (8) is not related to T _j , but the subsets T _j1 , T _j2 , T _j3 and T _j4 , by calculating the corresponding sub-items based on these 4 shorter subsets, can effectively reduce the computational burden.

Furthermore, in an exemplary embodiment of the present invention, whether a feature tracking set is actually divided may be based on the number of frames in the feature tracking set corresponding to each feature, eg, it may only be in the feature tracking set When the number of frames exceeds the preset threshold, the feature tracking set is divided into several feature tracking subsets of preset size, so that the number of frames in each feature tracking subset is less than or equal to the preset threshold and each Feature tracking subsets all include co-view keyframes, and frames in all feature tracking subsets cover all frames in the feature tracking set.

In addition, in the application of the SLAM system 100 , for a long motion trajectory, a considerable number of key frames will be accumulated for optimization at the back end, which easily causes the SLAM system 100 to fail to work efficiently. Therefore, in another exemplary embodiment of the present invention, the pose determination unit 140 may set a time domain window of a predetermined size for the current frame, and use a frame within the time domain window (for example, a key within the time domain window) frames, which may include co-view keyframes or non-co-view key frames) to determine the final pose of the SLAM system 100 . That is, the aforementioned preset frame set may also include key frames within the time domain window determined by setting a time domain window of a predetermined size for the current frame. Figure 10 shows a schematic diagram of the global beam adjustment based on the time domain window. As shown in FIG. 10 , the time domain window for the current frame may include both co-view key frames and non- co-view key frames.

Optionally, the pose determination unit 140 may further set a time domain window of a predetermined size for the current frame, and use the time domain window to filter the frames in the feature tracking subset generated in the foregoing process, and remove the frames in the time domain For frames outside the window, global beam adjustment is performed based on the filtered feature tracking subset to determine the final pose of the SLAM system 100 , thereby further reducing computational burden and improving image processing efficiency.

However, when there is a loop in the motion trajectory of the SLAM system 100, only using key frames within the time domain window instead of all key frames for processing may cause the loop to fail to close, as shown in (a) of FIG. 11 . In an exemplary embodiment of the present invention, loop closure can be used to close the loop, and the resulting pose difference can be passed back to the trajectory in turn to ensure smooth motion trajectory, the process of which is shown in Figure 11 (a) to Figure 11. 11(d). The closed-loop error cancellation process shown in FIG. 11 is known to those skilled in the art, so for the sake of brevity, it will not be described in more detail here.

In addition, the point feature is used as an example to describe the detailed process of determining the final pose of the SLAM system 100 by the pose determining unit 140. However, when the initial pose estimating unit 120 also extracts line segment features, the line segment features may be further considered to determine the final pose of the SLAM system 100. Perform a global beam adjustment.

Specifically, the SLAM system according to the exemplary embodiment of the present invention may further include a point-line optimization combination module (not shown), which may be based on the line segment features of the feature matching module 132 The matching results are used to establish a three-dimensional collinearity constraint, so that the pose determination unit 140 can also determine the final pose according to the three-dimensional collinearity constraint. By way of example only, a line segment constraint sub-term corresponding to the three-dimensional collinearity constraint may also be added and used in the aforementioned global beam adjustment method. Here, the three-dimensional collinearity constraint means that a point on a line segment of the first frame is on a corresponding line segment of the second frame. More specifically, the line segment matching between two frames is obtained according to the line segment descriptor vector. For any line segment matching, the three-dimensional coordinates of the two matching line segments in the camera coordinate system can be obtained. It can be known that the endpoint of one of the line segments is (Three-dimensional coordinates in space) When the frame pose is transformed to another camera coordinate system, it must be above the line segment matched by this line segment.

In other words, when calculating the final pose of the SLAM system 100 as in Equation (8), the reprojection error involved in the sub-term on the right side of the formula includes not only the reprojection error of point features, but also the reprojection error of line segment features. Here, calculating the reprojection error of the line segment feature is similar to the method of calculating the reprojection error of the point feature shown in formula (9) and is known to those skilled in the art, so it is not repeated here.

In addition, the global beam adjustment method is described above, but the present invention is not limited to this, and other methods capable of determining the pose of the device can also be used.

FIG. 12 is a flowchart illustrating a map reconstruction-based SLAM method according to an exemplary embodiment of the present invention.

Referring to FIG. 12 , at step 1210 of the method, the image acquisition unit 110 may be used to acquire a color image and a depth image of the surrounding environment of the SLAM system 100 .

Then, at step 1220, the initial pose estimation unit 120 may be used to estimate the initial pose of the SLAM system 100 based on the color image and the depth image obtained at step 1210.

More specifically, in step 1220, the initial pose estimation unit 120 may extract point features in the color image, perform point matching according to the point features, and then use the matched point features to estimate the initial pose of the SLAM system 100.

When the number of matched point features is less than the first preset threshold, the initial pose estimation unit 120 may further extract line segment features from the color image, and perform line segment matching according to the line segment features. At this time, the initial pose estimation unit 120 may use the matched point features and line segment features to estimate the initial pose of the SLAM system 100 .

In addition, when the number of matched point features is less than the second preset threshold (here, the second preset threshold is less than the first preset threshold), the initial pose estimation unit 120 may directly determine the initial pose of the SLAM system 100 to remain the same as the relative pose of the SLAM system 100 in the particular frame.

Here, the specific frame may be a previous frame or several frames in the vicinity of the current frame.

Optionally, although not shown in the flowchart of FIG. 12, the method may further include the operation of determining a co-view key frame. In an exemplary embodiment of the present invention, a co-viewing keyframe having a co-viewing point with the current frame may be determined in a previous frame (eg, a previous key frame) using the previously described full-image tracking unit (not shown). Therefore, in step 1220, the initial pose estimation unit 120 may further use these co-view key frames to estimate the initial pose, so that the accuracy of the SLAM system 100 can be enhanced.

Then, at step 1230, a map construction unit 130 may be used to construct a three-dimensional map based on the depth image and the color image.

The map construction unit 130 may reconstruct an initial three-dimensional map based on the first depth image and the first color image of the specific environment when entering the specific environment for the first time, and then based on the second image of the specific environment when entering the specific environment again. A depth image and a second color image are used to update the three-dimensional map.

More specifically, when the SLAM system 100 enters a specific environment for the first time, the map construction unit 130 may reconstruct a three-dimensional map based on the depth image and color image acquired by the image acquisition unit 110 when entering the specific environment for the first time, and determine the reconstructed three-dimensional map. The three-dimensional coordinates of each map point on the map and its spatial accuracy are stored, and the three-dimensional coordinates of each map point on the three-dimensional map with spatial accuracy and its spatial-temporal confidence are stored. Here, since the SLAM system 100 does not obtain the spatial and temporal confidence when entering a specific environment for the first time, the spatial and temporal confidence of the map point on the three-dimensional map at this time can be set as null and stored.

After that, when the SLAM system 100 re-enters the specific environment, the map construction unit 130 may calculate the three-dimensional coordinates of each map point and its space time based on the depth image and color image acquired by the image acquisition unit 110 when re-entering the specific environment Confidence, according to the calculated three-dimensional coordinates of each map point and its spatial and temporal confidence, update the three-dimensional coordinates of the corresponding map points on the stored three-dimensional map and their spatial and temporal confidence. The updated three-dimensional map can be used as the three-dimensional map constructed by the map constructing unit 130 for subsequent use.

At step 1240 , the pose determination unit 140 may be used to determine the final pose of the SLAM system 100 based on the initial pose of the SLAM system 100 estimated at step 1220 and the three-dimensional map constructed at step 1230 .

Here, as described above, the pose determination unit 140 may use a preset frame set to determine the final pose based on the initial pose and the three-dimensional map. The preset frame set may include a plurality of tracking subsets obtained by dividing the tracking set related to the current frame. Optionally, the pose determining unit 140 may also set a time domain window of a predetermined size for the current frame, and based on the initial pose and the three-dimensional map, use the key frames within the time domain window to determine the final pose.

Preferably, after performing the extraction and matching of the line segment features in step 1220, the method may also use a point-line optimization combination unit (not shown) to establish a three-dimensional collinearity constraint based on the matching result of the line segment features, so that the pose is determined Unit 140 may further utilize the three-dimensional collinearity constraints to determine the final pose. As a result, the accuracy of the SLAM system 100 can be further improved

Finally, in step S1250, the current frame related information including the final pose of the SLAM system 100 may be stored by the storage unit 150.

The operations performed by the various components of the SLAM system 100 according to the exemplary embodiment of the present invention in the various steps of FIG. 12 have been described in detail above with reference to FIGS. 1 to 11 , so for brevity, they will not be repeated here. describe.

The SLAM system 100 and the method thereof of the exemplary embodiment of the present invention can ensure the real-time performance of the mapping, reduce the amount of calculation, and also ensure the accuracy of the map. In addition, it should be understood that the exemplary embodiments described in the present specification are only examples shown for the convenience of understanding, and the present application is not limited thereto, without departing from the spirit and scope of the concept of the present invention, any Equivalents, alternatives, and modifications are deemed to fall within the scope of the invention.

Exemplary embodiments of the present invention can also be implemented as computer-readable codes on a computer-readable recording medium. The computer-readable recording medium is any data storage device that can store data which can be thereafter read by a computer system. Examples of computer-readable recording media include: read only memory (ROM), random access memory (RAM), CD-ROMs, magnetic tapes, floppy disks, optical data storage devices, and carrier waves (such as data over the Internet via wired or wireless transmission paths) transmission). The computer-readable recording medium can also be distributed over network coupled computer systems so that the computer-readable code is stored and executed in a distributed fashion. In addition, functional programs, codes and code segments for accomplishing the present invention can be easily construed by programmers of ordinary skill in the art to which the present invention pertains within the scope of the present invention.

Although the present invention has been particularly shown and described with reference to exemplary embodiments thereof, it will be understood by those skilled in the art that form and detail may be made therein without departing from the spirit and scope of the invention as defined in the claims various changes on.

Claims (15)

1. A realization device for real-time positioning and mapping, comprising: an image acquisition unit, configured to acquire a color image and a depth image of the surrounding environment; an initial pose estimation unit configured to estimate an initial pose based on the color image and the depth image; a map construction unit configured to construct a three-dimensional map based on the depth image and the color image; and The pose determination unit is configured to determine the final pose based on the initial pose and the three-dimensional map. 2. The implementation device according to claim 1, wherein the map construction unit comprises: a map reconstruction module configured to reconstruct an initial three-dimensional map based on the first depth image and the first color image of the surrounding environment; A map update module configured to update the three-dimensional map based on a second depth image and a second color image of the surrounding environment. 3. The implementation device according to claim 1, wherein the initial pose estimation unit comprises: a feature extraction module configured to extract point features in the color image; a feature matching module, configured to perform point matching according to the point feature; an estimation module, configured to estimate the initial pose using the matched point features, Wherein, when the number of matched point features is less than the first threshold, the feature extraction module also extracts line segment features in the color image, the feature matching module performs line segment matching according to the line segment features, and the estimation module uses the matched point features and line segments features to estimate the initial pose. 4. The implementation device according to claim 3, wherein the feature matching module performs line segment matching according to geometric structure information around the line segment feature. 5. The implementation device according to claim 1, further comprising: a full-image tracking module configured to determine a key frame having a common viewpoint with the current frame among previous key frames, Wherein, the initial pose estimation unit uses the determined key frame to estimate the initial pose based on the color image and the depth image. 6. A realization method of real-time positioning and mapping, the method comprising: Obtain color images and depth images of the surrounding environment; estimating an initial pose based on the color image and the depth image; constructing a three-dimensional map based on the depth image and color image; and Based on the initial pose and the three-dimensional map, the final pose is determined. 7. The method of claim 6, wherein the step of constructing a three-dimensional map comprises: reconstructing an initial three-dimensional map based on a first depth image and a first color image of the surrounding environment; The three-dimensional map is updated based on a second depth image and a second color image of the surrounding environment. 8. The method of claim 6, wherein the step of estimating the initial pose comprises: Extract point features in color images; perform point matching according to the point feature; Using the matched point features, the initial pose is estimated, Wherein, when the number of matched point features is less than the first threshold, the step of estimating the initial pose further includes: extracting line segment features in the color image, performing line segment matching according to the line segment features, and using the matched point features and line segments features to estimate the initial pose. 9 . The method of claim 8 , wherein the step of performing line segment matching according to the line segment feature comprises: performing line segment matching according to geometric structure information around the line segment feature. 10 . 10. The method of claim 6, further comprising: determining a key frame having a common viewpoint with the current frame in previous key frames, The step of estimating the initial pose includes: using the determined key frame to estimate the initial pose based on the color image and the depth image. 11. The method of claim 8, further comprising: establishing a three-dimensional collinearity constraint based on the matching result of the line segment features, Wherein, the step of determining the final pose includes: further determining the final pose according to the three-dimensional collinear constraint, The three-dimensional collinearity constraint indicates that the point on the line segment of the first frame is on the corresponding line segment of the second frame. 12. The method of claim 6, wherein the step of determining the final pose comprises: using a preset frame set to determine the final pose based on the initial pose and the three-dimensional map, The preset frame set includes: multiple tracking subsets obtained by dividing the tracking set related to the current frame. 13. The method according to claim 6, wherein the step of determining the final pose comprises: setting a time domain window of a predetermined size for the current frame, and based on the initial pose and the three-dimensional map, using the time domain window within the time domain window. keyframes to determine the final pose. 14. A computer-readable storage medium storing instructions, wherein the instructions, when executed by at least one computing device, cause the at least one computing device to perform the method of any one of claims 6 to 13. method. 15. A system comprising at least one computing device and at least one storage device for storing instructions, wherein the instructions, when executed by the at least one computing device, cause the at least one computing device to perform as claimed in claims 6 to 13 The method of any of the claims.

Images