Simultaneous Localization and Mapping Method Based on Point and Line Feature Constraints

Abstract

The present invention proposes a simultaneous localization and mapping method based on point and line feature constraints for pose localization and mapping operations when a device is moving, including the following steps: First, by extracting point and line features in the images captured during the movement of the device and screening the lengths of the line features, point and line features with higher tracking quality are obtained; Second, search for line features at similar positions according to the detected point features within their fixed radius range; Group and couple the use of point and line features; Finally, use the point-line coupled features for joint mapping, define point features, line features, line feature residuals, and point-line coupled feature residuals, and then perform non-linear optimization at the backend to achieve pose estimation and map construction by minimizing the residuals; The present invention uses point-line coupled features for joint mapping, which can more accurately output the robot pose and perform map construction.

Description

Technical Field

[0001] The present invention relates to the field of computer technology / machine vision technology, and in particular to a simultaneous localization and mapping method based on point and line feature constraints. Background Art

[0002] At present, with the rapid development of computer technology / machine vision and automatic control technology, intelligent robots, augmented reality (AR), virtual reality (VR), unmanned aerial vehicles, and intelligent driving based on the integration of multiple technologies have been widely applied to various aspects of daily life and production. In these applications, it is necessary to know the environmental information of the robot's environment and its own pose. Precise and robust map generation is a necessary prerequisite for these products. Therefore, the theoretical and application requirements for simultaneous localization and mapping have become inevitable. As an important means to solve this requirement, the simultaneous localization and mapping technology is also the core technology to achieve high intelligence of robots. It has been widely used in the market. At the same time, with the continuous iteration and update of processor chips, the barrier of computing and processing efficiency has been further broken, providing the possibility of real-time processing of a large amount of data in the simultaneous localization and mapping technology.

[0003] The sensors in the localization and mapping technology are mainly divided into two types: one is the laser simultaneous localization and mapping technology that uses lidar as the main sensor, and the other is the visual simultaneous localization and mapping technology that uses a camera as the main sensor. Due to the relatively high price and the inaccurate point cloud distortion correction model, the lack of texture information in the laser point cloud results in weak loop detection ability of the laser simultaneous localization and mapping technology; the visual simultaneous localization and mapping technology is favored in practical applications due to its low cost and rich texture information.

[0004] In an indoor environment, the visual sensor has become an essential sensor for mobile robots due to its rich semantic information, high precision, and easier loop with texture information. The visual simultaneous localization and mapping technology has also become a major mainstream direction in the research of mobile robot localization.

[0005] On this basis, in order to improve the accuracy of the obtained pose trajectory, geometric features other than point features can be extracted from the obtained visual information. The currently mainly used geometric features are point features, line features, and surface features. Among them, the surface features often cannot be fitted in the outdoor environment, and the actual tracking efficiency is relatively low. The point features and line features have relatively robust performance both in indoor and outdoor environments. The two features can be mixed and used due to their position similarity in the three-dimensional space. Summary of the Invention

[0006] The present invention proposes a simultaneous localization and mapping method based on point and line feature constraints. It uses point-line coupled features for joint mapping, defines line feature residuals and point-line coupled feature residuals, and then performs non-linear optimization, which can more accurately output the robot pose and construct a map, that is, it can effectively improve the accuracy of pose estimation and map construction.

[0007] The present invention adopts the following technical solutions.

[0008] A simultaneous localization and mapping method based on point and line feature constraints is used for pose localization and mapping operations when a device with machine vision function moves, and includes the following steps;

[0009] First, by extracting point and line features in the images captured during the movement of the device and screening the lengths of line features, point and line features with higher tracking quality are obtained;

[0010] Secondly, search for line features at similar positions within a fixed radius of the detected point features; group and couple the point and line features for use;

[0011] Finally, use point-line coupled features for joint mapping, define point features, line features, line feature residuals and point-line coupled feature residuals, and then perform non-linear optimization at the back end to achieve the purpose of pose estimation and map construction by minimizing the residuals.

[0012] The device with machine vision function is a robot, and the method includes the following steps;

[0013] Step S1: For the image data obtained by the camera of the device, one image obtained according to its frequency is taken as one frame. After preprocessing, for each frame of the image, feature extraction is performed on the image obtained from the camera frame based on the point feature shi-tomasi extraction algorithm and the line feature detector (Line Segment Detector, LSD) extraction algorithm;

[0014] Step S2: Define the length threshold for extracting line features according to the number of line feature tracking times, and adjust the visual pyramid in the algorithm to reduce fragmented line segments and line segments with lower tracking times;

[0015] Step S3: Search for the endpoints of line features at a given distance on the normalized plane according to the extracted point features, and couple and use them if the extraction is successful;

[0016] Step S4: Define point features, line features, and point-line coupled feature residuals; track the obtained features to achieve the effect of preliminary pose estimation;

[0017] Step S5: Reproject the previously obtained camera frames using the sliding window method, obtain the optimal pose using the non-linear optimization method, and construct the map.

[0018] Specifically, step S1 is as follows: Image acquisition is performed based on the camera frequency. Each acquired image is used as a frame of the image. To prevent deformation of objects in the image, each frame of the image is de-distorted to facilitate corner extraction. Based on the de-distorted image, grayscale processing of the image is performed, which can better handle problems such as illumination changes. Each preprocessed image is passed as an image frame to the feature extraction module. Based on the point feature shi-tomasi extraction algorithm and the line feature detection sub-algorithm, shi-tomasi corner extraction and LSD line feature extraction are performed on the given frame image to determine whether it is an outlier feature. After removing the outlier features, they are projected onto the normalized plane and the coordinate values on the normalized plane are recorded.

[0019] The point feature shi-tomasi extraction algorithm includes the shi-tomasi corner detection algorithm for detecting corners on a two-dimensional image. When it works, a fixed window slides arbitrarily on the image to compare the gray level changes of pixel blocks in the image. It is specifically divided into three cases:

[0020] Case 1: The gray level change is small in the x or y direction. At this time, it can be considered that the pixels contained in the window are in the plane position, and it is considered that there are no corners.

[0021] Case 2: The change is large in a single x or y direction. At this time, it can be considered that the pixels contained in the window are at the boundary position. It is considered that there are no corners.

[0022] Case 3: The gray level changes significantly in both the x and y directions. At this time, it can be considered that a corner has been detected.

[0023] The specific method of the LSD line segment detection algorithm is as follows: Calculate the gradient of each pixel on the image, convert the image into a gradient representation, and the pixels within a certain tolerance of the gradient are considered to be divided into a region as an alternative for the line segment. The minimum bounding rectangle is made for these alternative regions, and at this time, all pixels within the region are considered to form a line feature.

[0024] The visual pyramid in step S2 is a Gaussian pyramid. Specifically, the number of layers and the sampling times of the Gaussian pyramid are modified, and the line segment length threshold and the line segment density are adjusted. So that overly thin line segments are not detected, as these overly thin line segments have low quality in actual tracking. The line segment length threshold and the line segment density are adjusted.

[0025] In the image processing of step S2, when putting the image into the Gaussian pyramid, set the number of pyramid layers to an n-layer Gaussian pyramid, downsample p times, and blur j times; when processing the image in step S2, the best effect is achieved under a 2-layer Gaussian pyramid, with 1 downsampling and 2 blurs; set the requirement threshold for the alignment points in the closed area in the extraction algorithm to m; set the minimum line segment length constraint to k pixel units for length screening; remove the line segments with less than 70% of the same-sex points in the line features; the same-sex points in the line features are the pixel points with the same gradient in the line feature area; this step is to remove the fragmented line segments in the obtained line features, and the tracking quality of such line segments is poor and they are removed.

[0026] The specific steps of step S3 are as follows: Based on the point and line features obtained in S1 and S2, search for the endpoints of the line features based on the feature points obtained in S1, group them, and then use them in combination; specifically: based on the position similarity of the point and line features in three-dimensional space; group the point features and line features in pairs; first use the radius search algorithm of the K-Dimension Tree search algorithm in the point features to search for the endpoints of the line features, and extract the line feature with the closest distance from the searched line features. If the search is successful, the point feature number and line feature number can be obtained, and the point-line features are regarded as a group of features for combined use; if the search fails, the point feature and line feature numbers are returned.

[0027] During the operation, if due to the complexity of the space environment, the quality of the line features varies greatly between two consecutive frames during tracking, the LSD algorithm is used for screening at this time to remove the line features with low quality; if the tracking of the point features and line features fails due to changes in light or perspective, when not enough features are tracked, the point features are used to make up, and at the same time, point-line coupling features are further formed based on the point features to increase the optimization constraints; achieving the effect of improving the accuracy.

[0028] The specific steps of step S4 are as follows: Set the corresponding residual amount, and based on the epipolar geometry constraint and the Perspective-n-Points (PnP) pose recovery algorithm, recover the pose change of the corresponding feature points between two frames, so as to make a preliminary estimate of the pose between the two frames.

[0029] For the point feature residuals, use the reprojection method to project the feature points of the first frame to the next key frame, and define the distance between the projection and the corresponding feature points of the next key frame as the point residuals; for the line feature residuals, project the line segment to the next key frame, and calculate the distance between the projection and the midpoint of the corresponding line segment of the next key frame, which is defined as the line residuals; for the point-line coupling feature residuals, project the coupling feature in the first key frame to the second key frame, and define the distance between the projection point in the coupling feature and the line feature in the corresponding feature of the second frame as the residuals.

[0030] For the specific operation of tracking corresponding points between two frames, first perform histogram equalization on the obtained image, set the masking operation, determine whether the image is the first frame, extract the same point features and line features between the two frames and track them, and eliminate outlier features on the normalized plane; successfully track the features and assign numbers; calculate the essential matrix according to the same numbers to obtain the initial pose change between the two frames.

[0031] The specific steps of step S5 are as follows: Define the corresponding residuals in the set sliding window and perform minimization processing respectively. Among them, when the number of tracked points and lines is lower than a predetermined number, coupled residuals are used to participate in the constraint for mapping, and the BA (Bundle Adjustment) method is used to output the optimal pose.

[0032] The specific steps of step S5 are as follows: The position and pose data output by the robot through visual odometry are obtained based on the local coordinate system, which are transformed into the world coordinate system according to the pose transformation matrix obtained from the previous frame, and then matched with the existing map in the world coordinate system to obtain the final accurate pose. Finally, the image data of each frame are stitched according to the obtained accurate pose to complete the mapping function of the driving path during the robot's driving process.

[0033] The present invention provides a technology for robot pose estimation and map construction using point and line features. By extracting point and line features and screening the lengths of line features, point and line features with higher tracking quality are obtained; secondly, according to the detected point features, line features at similar positions are obtained by searching within its fixed radius range; the point and line features are grouped and used in a coupled manner; finally, the residuals of point and line features and point-line coupled features are defined, and non-linear optimization is performed at the back end. By minimizing the residuals, the purpose of pose estimation and map construction is achieved. The present invention proposes a line segment screening method, which has outstanding advantages in extracting line features with high tracking quality; at the same time, the present invention uses point-line coupled features for joint mapping, defines the line feature residuals and point-line coupled feature residuals, and then performs non-linear optimization, which can more accurately output the robot pose and perform map construction. The simultaneous localization and mapping method proposed by the present invention can effectively improve the accuracy of pose estimation and map construction.

[0034] The present invention has the following beneficial effects compared with the prior art:

[0035] 1. The line feature screening algorithm of the present invention can ensure that line features with lower tracking quality are eliminated during mapping. By setting the number of Gaussian pyramid layers and setting the length threshold, the extraction amount of line features is reduced, and line features with higher tracking quality during pose estimation are retained. At the same time, screening based on line feature density is performed, effectively reducing the number of useless line features, effectively improving the line feature tracking efficiency, reducing computing resources at the same time, accelerating the computing efficiency, and ensuring real-time performance.

[0036] 2. The present invention groups point and line features according to the position similarity of the geometric features of points and lines in three-dimensional space, enabling the coupled use of point and line features, better restoring the geometric information in three-dimensional space, and being able to calculate the pose change between two frames better during the pose estimation process.

[0037] 3. For line features and coupled features, the present invention projects them onto the next key frame using the reprojection method to define residuals, performs Bundle Adjustment optimization based on the defined residuals, and optimizes the global pose by minimizing the set residuals, effectively improving the accuracy.

[0038] 4. The present invention participates in mapping through multi-residuals of points, lines, and coupled features, thereby improving the accuracy of the estimated trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The following further describes the present invention in detail with reference to the drawings and specific embodiments:

[0040] Att Figure 1 is a schematic flowchart of the simultaneous localization and mapping method based on point and line feature constraints in the method of the present invention;

[0041] Att Figure 2 is a visual display of point features and line features in an embodiment of the present invention;

[0042] Att Figure 3 is a schematic diagram of point-line coupled features in an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0043] As shown in the figure, the simultaneous localization and mapping method based on point and line feature constraints is used for pose localization and mapping operations when a device with machine vision function moves, and includes the following steps;

[0044] First, by extracting point and line features in the images captured during the movement of the device and screening the lengths of line features, point and line features with higher tracking quality are obtained;

[0045] Secondly, search for line features at similar positions within the fixed radius of the detected point features; group and couple the use according to point and line features;

[0046] Finally, use point-line coupled features for joint mapping, define point features, line features, line feature residuals, and point-line coupled feature residuals, and then perform non-linear optimization at the back end to achieve the purpose of pose estimation and map construction by minimizing the residuals.

[0047] The device with machine vision function is a robot, and the method includes the following steps;

[0048] Step S1: For the image data obtained by the device's camera, one image obtained according to its frequency is one frame. After preprocessing, for each frame of image, feature extraction is performed on the image obtained from the camera frame based on the point feature shi-tomasi extraction algorithm and the line feature detector (Line Segment Detector, LSD) extraction algorithm;

[0049] Step S2: Define the length threshold for extracting line features according to the number of line feature tracking times, and adjust the visual pyramid in the algorithm to reduce fragmented line segments and line segments with a lower number of tracking times;

[0050] Step S3: According to the extracted point features, search for the endpoints of line features at a given distance on the normalized plane. If the extraction is successful, couple and use them;

[0051] Step S4: Define the residuals of point features, line features, and point-line coupled features; track the obtained features to achieve the effect of preliminary pose estimation;

[0052] Step S5: Reproject the previously obtained camera frames by the sliding window method, use the non-linear optimization method to obtain the optimal pose, and construct a map.

[0053] The specific content of step S1 is as follows: Image acquisition is performed based on the camera frequency. Each obtained image is used as one frame of the image. To prevent deformation of objects in the image, image undistortion is performed on each frame of the image to facilitate the extraction of corner points. Based on the undistorted image, image grayscale processing is performed, which can better handle problems such as illumination changes; Each preprocessed image is passed to the feature extraction module as an image frame. Based on the point feature shi-tomasi extraction algorithm and the line feature detector extraction algorithm, shi-tomasi corner extraction and LSD line feature extraction are performed on the given frame image to determine whether it is an outlier feature; After removing the outlier features, project them onto the normalized plane and record their coordinate values on the normalized plane;

[0054] In this example, when actually estimating the pose, the point and line features are as Figure 2 shown.

[0055] The point feature shi-tomasi extraction algorithm includes the shi-tomasi corner detection algorithm for detecting corner points on a two-dimensional image. When it works, a fixed window slides arbitrarily on the image to compare the gray-scale changes of pixel blocks in the image; It is specifically divided into three situations;

[0056] Situation 1: The gray-scale change is small in the x or y direction; At this time, it can be regarded that the pixels contained in the window are in the plane position, and it is considered that there are no corner points;

[0057] Case 2: There are large variations in a single direction of x or y. At this time, it can be considered that the pixels contained in the window are at the boundary position and there are no corner points.

[0058] Case 3: There are large gray-scale variations in both the x and y directions. At this time, it can be considered that corner points are detected.

[0059] The specific method of the LSD line segment detection algorithm is as follows: Calculate the gradient of each pixel on the image, convert the image into a gradient representation, and pixels within a certain tolerance of the gradient are considered to be divided into a region as an alternative for a line segment. Make the minimum bounding rectangle for these alternative regions. At this time, all pixels within the region are considered to form a line feature.

[0060] The visual pyramid in step S2 is a Gaussian pyramid. Specifically: Modify the number of layers and sampling times of the Gaussian pyramid, adjust the line segment length threshold and line segment density; so that overly thin line segments are not detected, as these overly thin line segments have low quality in actual tracking; adjust the line segment length threshold and line segment density.

[0061] In the image processing of step S2, when putting the image into the Gaussian pyramid, set the number of pyramid layers to an n-layer Gaussian pyramid, downsample p times, and blur j times; when step S2 processes the image, the best effect is achieved under a 2-layer Gaussian pyramid, with downsampling 1 time and blurring 2 times; set the requirement threshold for the alignment points in the closed region in the extraction algorithm to m; set the minimum line segment length constraint to k pixel units for length screening; eliminate line segments with less than 70% of the same-sex points within the line feature; the same-sex points within the line feature are pixel points with the same gradient within the line feature region; this step is to eliminate the fragmented line segments of the obtained line features, as such line segments have poor tracking quality and are eliminated.

[0062] The specific content of step S3 is as follows: Based on the point and line features obtained in S1 and S2, search for the endpoints of the line features based on the feature points obtained in S1, group them, and then use them in combination; specifically: Based on the position similarity of the points and line features in three-dimensional space; group the point features and line features in pairs; first use the radius search algorithm of the K-Dimension Tree search algorithm in the point features to search for the endpoints of the line features, and extract the line feature with the closest distance from the searched line features. If the search is successful, the point feature number and line feature number can be obtained, and the point-line features are regarded as a group of features for combined use; if the search fails, the point feature and line feature numbers are returned.

[0063] In this example, as Figure 3 shown, where Figure 3In (a), the red feature points marked are the origin points. In the point features, the K-dimensional search tree search algorithm is used for radius search to search for the endpoints of line features, and a threshold search for the endpoints of line features is performed with a radius of y pixel units. In this embodiment, y is set to 5. As Figure 3 shown in (b), within a certain threshold range, two endpoints of line features are searched. The line feature with the closest distance is extracted from the searched line features for point-line feature coupling. If successful, the point feature number and the line feature number can be obtained, and the point-line features are regarded as a group of features for coupled use. If the search fails, the point feature and the line feature numbers are returned. When it is difficult to couple and use features in the next key frame, it is determined that the tracking fails.

[0064] The above is the ideal state. In the actual operation process of this example, due to the complexity of the space environment, there are often significant differences in the quality of line features between two consecutive frames during tracking. At this time, using the modified LSD algorithm for screening can exactly remove the line features with lower quality. The tracking of point features and line features often fails due to changes in light or perspective. Therefore, when not enough features are tracked, point features are used to make up for it, and at the same time, point-line coupled features are further formed based on point features to increase the optimization constraints, achieving the effect of improving the accuracy.

[0065] During the operation, if due to the complexity of the space environment, there are significant differences in the quality of line features between two consecutive frames during tracking, at this time, the LSD algorithm is used for screening to remove the line features with lower quality. If the tracking of point features and line features fails due to changes in light or perspective, then when not enough features are tracked, point features are used to make up for it, and at the same time, point-line coupled features are further formed based on point features to increase the optimization constraints, achieving the effect of improving the accuracy.

[0066] Specifically, step S4 is as follows: Set the corresponding residual amount, and based on the epipolar geometry constraint and the pose recovery algorithm (Perspective-n-Points, PnP), recover the pose change of the corresponding feature points between two frames, so as to make a preliminary estimate of the pose between two frames.

[0067] For the point feature residual, the feature points of the first frame are projected to the next key frame using the reprojection method, and the distance between the projection and the corresponding feature points of the next key frame is defined as the point residual. For the line feature residual, the line segment is projected to the next key frame, and the distance is calculated between the projection and the midpoint of the corresponding line segment of the next key frame, which is defined as the line residual. For the point-line coupled feature residual, the coupled feature in the first key frame is projected into the second key frame, and the distance between the projected point in the coupled feature and the line feature in the corresponding feature of the second frame is defined as the residual.

[0068] For the specific operation of tracking corresponding points between two frames, first perform histogram equalization on the obtained image, set the masking operation, determine whether the image is the first frame, extract the same point features and line features between the two frames and track them, and eliminate outlier features on the normalized plane; successfully track the features and assign numbers; calculate the essential matrix according to the same numbers to obtain the initial pose change between the two frames.

[0069] The specific steps of step S5 are as follows: Define corresponding residuals in the set sliding window and perform minimization processing respectively. Among them, when the number of tracked points and lines is lower than the predetermined number, coupled residuals are used to participate in the constraint for mapping, and the BA (Bundle Adjustment) method is used to output the optimal pose.

[0070] The specific steps of step S5 are as follows: The position and pose data output by the robot through visual odometry are obtained based on the local coordinate system, which are transformed into the world coordinate system according to the pose transformation matrix obtained from the previous frame, and then matched with the existing map in the world coordinate system to obtain the final accurate pose. Finally, the image data of each frame are stitched according to the obtained accurate pose to complete the mapping function of the driving path during the robot's driving process.

[0071] The above are only the preferred embodiments of the present invention, and all equivalent changes and modifications made according to the scope of the patent application of the present invention shall fall within the scope covered by the present invention.

Claims

1. A simultaneous localization and mapping method based on point and line feature constraints, which is used for pose localization and mapping operations when a device with machine vision function moves. It is characterized in that: Firstly, the method obtains point and line features with higher tracking quality by extracting point features and line features in the images captured during the movement of the device and screening the lengths of the line features. Secondly, search for line features at similar positions within a fixed radius of the detected point features; group and couple the point and line features for use. Finally, use the point-line coupled features for joint mapping, define the point features, line features, line feature residuals and point-line coupled feature residuals, and then perform non-linear optimization at the back end to achieve pose estimation and map construction by minimizing the residuals. The device with machine vision function is a robot, and the method includes the following steps; Step S1: For the image data obtained by the camera of the device, one image obtained according to its frequency is taken as one frame. After preprocessing, for each frame of the image, feature extraction is performed on the image obtained from the camera frame based on the point feature extraction algorithm shi-tomasi and the line feature detection sub LSD extraction algorithm. Step S2: Define the length threshold for extracting line features according to the number of times of line feature tracking, and adjust the visual pyramid in the algorithm to reduce fragmented line segments and line segments with lower tracking times. Step S3: Search for the endpoints of line features at a given distance on the normalized plane according to the extracted point features, and couple and use them if the extraction is successful. Step S4: Define the point features, line features, and point-line coupled feature residuals; track the obtained features to achieve the effect of preliminary pose estimation. Step S5: Reproject the previously obtained camera frames by the sliding window method, and use the non-linear optimization method to obtain the optimal pose and construct the map. The specific content of step S1 is as follows: Image acquisition is performed based on the camera frequency, and each obtained image is taken as one frame of the image. To prevent deformation of the objects in the image, image undistortion is performed on each frame of the image to facilitate the extraction of corner points. Based on the undistorted image, image grayscale processing is performed to better handle the problem of illumination change; each preprocessed image is passed to the feature extraction module as an image frame, and based on the point feature extraction algorithm shi-tomasi and the line feature detection sub extraction algorithm, shi-tomasi corner extraction and LSD line feature extraction are performed on the given frame image to determine whether it is an outlier feature. After removing the outlier features, project them onto the normalized plane and record their coordinate values on the normalized plane. The point feature shi-tomasi extraction algorithm includes the shi-tomasi corner detection algorithm for detecting corner points on a two-dimensional image. When it works, a fixed window slides arbitrarily on the image to compare the gray value changes of the pixel blocks in the image; it is specifically divided into three cases; Case 1: The gray value change is not significant in the x or y direction; at this time, it can be regarded that the pixels contained in the window are in the plane position, and it is considered that there are no corner points. Case 2: There is a large change in a single direction of x or y. At this time, it can be considered that the pixels contained in the window are at the boundary position and no corner points are considered. Case 3: There are large changes in the gray level in both the x and y directions. At this time, it can be considered that corner points are detected. The specific method of the LSD line detection algorithm is as follows: Calculate the gradient of each pixel on the image, convert the image into a gradient representation, and the pixels within a certain tolerance of the gradient are considered to be divided into a region as an alternative for a line segment; make the minimum circumscribed rectangle for these alternative regions. At this time, all the pixels within the region are considered to form a line feature. The specific step S3 is as follows: Based on the point and line features obtained in S1 and S2, search for the endpoints of the line feature based on the feature points obtained in S1, group them, and then couple them for use. Specifically: Based on the positional similarity of the point and line features in three-dimensional space. Group the point features and line features in pairs. First, use the K-dimensional search tree search algorithm in the point features to perform radius search to search for the endpoints of the line feature, and extract the line feature with the closest distance from the searched line features. If the search is successful, the point feature number and line feature number can be obtained, and the point-line feature is regarded as a group of features for coupled use. If the search fails, return the point feature and line feature numbers. During the operation, if due to the complexity of the space environment, the quality of the line feature has a large difference between two consecutive frames during tracking, at this time, the LSD algorithm is used for screening to remove the line features with lower quality; if the tracking of the point feature and the line feature fails due to changes in light or perspective, when not enough features are tracked, the point feature is used for supplementation, and at the same time, the point-line coupling feature is further formed based on the point feature to increase the optimization constraint, achieving the effect of improving the accuracy.

2. The simultaneous localization and mapping method based on point and line feature constraints according to claim 1, characterized in that: The visual pyramid in step S2 is a Gaussian pyramid. Specifically: Modify the number of layers and sampling times of the Gaussian pyramid, adjust the line segment length threshold and line segment density, so that small line segments are not detected. These small line segments have low quality in actual tracking; adjust the line segment length threshold and line segment density.

3. The simultaneous localization and mapping method based on point and line feature constraints according to claim 2, characterized in that: In the image processing of step S2, when putting the image into the Gaussian pyramid, set the number of pyramid layers to n-layer Gaussian pyramid, downsample p times, and blur j times; the best effect is achieved when step S2 processes the image with a 2-layer Gaussian pyramid, downsampling 1 time, and blurring 2 times; set the requirement threshold for the alignment points in the closed area in the extraction algorithm to m; set the minimum line segment length constraint to k pixel units for length screening; remove the line segments with less than 70% of the same-sex points within the line feature. The same-sex points within the line feature are the pixels with the same gradient within the line feature region. This step is to remove the fragmented line segments of the obtained line features. Such line segments have poor tracking quality and are removed.

4. The simultaneous localization and mapping method based on point and line feature constraints according to claim 1, characterized in that: The specific steps of step S4 are as follows: Set corresponding residual amounts, and restore the pose changes of corresponding feature points between two frames according to the epipolar geometry constraint and the pose recovery algorithm PnP, so as to perform a preliminary estimation of the pose between two frames.

5. The simultaneous localization and mapping method based on point and line feature constraints according to claim 4, wherein: For the point feature residual, the re-projection method is used to project the feature points of the first frame to the next key frame, and the distance between the projection and the corresponding feature points of the next key frame is defined as the point residual; for the line feature residual, the line segment is re-projected to the next key frame, and the distance is calculated between the projection and the midpoint of the corresponding line segment of the next key frame, which is defined as the line residual; for the point-line coupled feature residual, the coupled features in the first key frame are projected into the second key frame, and the distance between the projected points in the coupled features and the line features in the corresponding features of the second frame is defined as the residual; For the specific operation of tracking corresponding points between two frames, first perform histogram equalization on the obtained image, set a masking operation, determine whether the image is the first frame, extract the same point features and line features between the two frames and perform tracking, and remove outlier features on the normalized plane; Successfully track the features and assign numbers; calculate the essential matrix according to the same numbers to obtain the initial pose change between two frames.

6. The simultaneous localization and mapping method based on point and line feature constraints according to claim 1, wherein: The specific steps of step S5 are as follows: Define corresponding residuals in the set sliding window and perform minimization processing. Among them, when the number of tracked points and lines is lower than a predetermined amount, the coupled residuals are used to participate in the constraint for mapping, and the BA method is used to output the optimal pose.

7. The simultaneous localization and mapping method based on point and line feature constraints according to claim 1, wherein: The specific steps of step S5 are as follows: The position and pose data output by the robot through the visual odometer are obtained based on the local coordinate system, which are converted to the world coordinate system according to the pose transformation matrix obtained from the previous frame, and then matched with the existing map in the world coordinate system to obtain the final accurate pose. Finally, the image data of each frame are stitched according to the obtained accurate pose to complete the mapping function of the driving path during the robot's driving process.

Images