Pose estimation method based on point-line features and electronic equipment

Abstract

The invention provides a point and line feature-based pose estimation method and electronic equipment. The method comprises the following steps of extracting point features and line features from an environment image; calculating one or more vanishing points in the environment image based on the extracted line features; according to the geometrical relationship between each line feature and the vanishing point, grouping the line features into different dominant direction sets, and removing the line features which do not belong to any dominant direction set to obtain an optimized line feature set; and fusing the point features and the optimized line feature set, taking the fused point features and the optimized line feature set as input together, performing pose estimation through a visual SLAM algorithm, and outputting pose information of the camera. According to the method, the weak texture environment is effectively dealt with, the point features and the line features are fused, and the line features are optimized by using the vanishing points, so that the key problems that the traditional visual SLAM which only depends on the point features is rare in features and easy to fail in tracking in the low-texture structured environment are solved, and stable and high-precision positioning in the severe environment is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of visual positioning navigation algorithm, in particular to a pose estimation method based on point-line features and an electronic device. BACKGROUND

[0002] With the rapid development of global military technology, positioning and navigation functions are very important in the fields of life, industry, military, etc. The common navigation and positioning method for cars, mobile phones, intelligent robots in life, satellites, aircraft, missiles in the military industry is to use GPS for positioning and navigation. For satellites, it is very dependent. Once signal interference, shielding and deception occur, key positioning errors or positioning failures may occur. SUMMARY

[0003] In view of the above defects or deficiencies in the prior art, the present application aims to provide a pose estimation method based on point-line features and an electronic device.

[0004] In a first aspect, the present application provides a pose estimation method based on point-line features, comprising the following steps: Collecting an environment image and extracting point features and line features from the environment image; Based on the extracted line features, calculating one or more vanishing points in the environment image; According to the geometric relationship between each line feature and the vanishing point, grouping the line features into different dominant direction sets, and eliminating the line features not belonging to any dominant direction set to obtain an optimized line feature set; Fuse the point features and the optimized line feature set together as input, and perform pose estimation through a visual SLAM algorithm to output the pose information of the camera.

[0005] According to the technical scheme provided by the embodiments of the present application, the point features and line features are extracted from the environment image, comprising the following steps: Gaussian down-sampling is performed on the environment image to reduce noise; Calculate the gradient value and total gradient direction of each pixel point in the environment image in the x direction and the y direction; Pseudo-sorting the pixel points according to the gradient values and setting a level threshold; Taking the pixel point with the largest gradient value as a base point, generating a line segment support domain according to its gradient direction, and including the pixel points close to the gradient direction of the base point in the support domain; Construct a minimum circumscribed rectangle based on the pixel points in the line segment support domain as a candidate line segment; Verify whether the candidate line segment meets the preset alignment degree and pixel density conditions to determine the final extracted line features.

[0006] According to the technical scheme provided by the embodiment of the present application, the one or more vanishing points in the environment image are calculated, including the following steps: The line features are clustered to form groups, and a vanishing point is calculated for each group of line features; wherein for a group containing multiple spatial straight lines, the homogeneous coordinates of the vanishing point are initially estimated by solving a singular value decomposition of a matrix composed of the coefficients of the straight lines in the group; The homogeneous coordinates of the initially estimated vanishing point are optimized to minimize the sum of the distances between the midpoints of the straight lines in the group and the connecting lines between the midpoints and the vanishing point, to obtain the position coordinates of the vanishing point.

[0007] According to the technical scheme provided by the embodiment of the present application, the line features are grouped into different dominant direction sets, and the line features not belonging to any of the dominant direction sets are removed, including the following steps: For each line feature to be determined, the following operations are performed: The midpoint of the line feature is calculated; The midpoint is connected to each of the vanishing points to form multiple auxiliary straight lines; The sum of the distances between the two end points of the line feature and each of the auxiliary straight lines is calculated; If there is an auxiliary straight line such that the sum of the distances is less than a first preset threshold, it is determined that the line feature belongs to the vanishing point group and the dominant direction corresponding to the auxiliary straight line; If the sum of the distances between the line feature and all auxiliary straight lines is not less than the first preset threshold, the line feature is removed as a false matching line feature.

[0008] According to the technical scheme provided by the embodiment of the present application, before the point features are fused with the optimized line feature set, the following steps are further included: Each line feature in the optimized line feature set is described using an LBD descriptor; Wherein, a local coordinate system is established with the direction of the line segment and the normal direction thereof as the axes, the line segment support domain is divided into multiple strips, the gradient statistical features of the pixel points in each strip are calculated, and the feature vectors of all strips are combined to form the final LBD descriptor, which is used for subsequent feature matching and pose solving.

[0009] According to the technical scheme provided by the embodiment of the present application, point features are extracted from the environment image, including the following steps: Point feature extraction is performed using an improved FAST corner detection algorithm; Wherein, when calculating the BRIEF descriptor for each key point, the direction description characteristic of the key point is added, so that the point feature has rotation invariance.

[0010] According to the technical scheme provided in the embodiment of the application, whether the candidate line segment meets the preset alignment degree and pixel density conditions is verified to determine the final extracted line feature, including the following steps: An isopleth direction of a pixel point in the area of the minimum circumscribed rectangle is calculated. The isopleth direction is compared with the direction of the minimum circumscribed rectangle, and a direction difference is calculated. The number of pixel points covered by the isopleth in the area of the minimum circumscribed rectangle is counted, and a ratio of the number to the total number of pixel points in the rectangle is calculated. If the direction difference is less than a second preset angle threshold and the ratio is greater than a preset ratio threshold, it is determined that the candidate line segment is an effective line feature and is extracted; otherwise, the candidate line segment is rejected.

[0011] According to the technical scheme provided in the embodiment of the application, the gradient statistical features of the pixel points in each strip are calculated, including the following steps: For each strip, a global gradient vector and a local gradient vector are calculated respectively; the global gradient vector is calculated by weighting the gradients of all pixels in the strip by a global Gaussian function; and the local gradient vector is calculated by weighting the gradients of each row of pixels in the strip by a local Gaussian function. The global gradient vector and the local gradient vector are combined to construct a feature vector of the strip.

[0012] According to the technical scheme provided in the embodiment of the application, the homogeneous coordinates of the estimated vanishing point are optimized so that the sum of distances between the midpoint of any straight line in a group and the connecting line between the midpoint and the vanishing point is minimized, including the following steps: An error function with the minimization of the sum of distances as the target is constructed. The error function is solved by using a nonlinear least squares method, and the position coordinates of the vanishing point are iteratively optimized until a convergence condition is met.

[0013] In a second aspect, the application provides an electronic device, including: One or more processors; A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the point-line feature based pose estimation method as described above.

[0014] Compared with the prior art, the beneficial effects of the present application are that the algorithm breaks through the environment dependence of traditional GPS and two-dimensional code positioning, only relies on the camera carried by the carrier itself as a visual sensor, completes camera pose estimation positioning through a series of image processing algorithms and the point-line visual features proposed in this paper, and then calculates the motion trajectory of the camera in the world coordinate system. At the same time, the effective frames collected by the camera are combined with the pose estimation value, so that the corresponding positioning result can be obtained, and a three-dimensional point cloud map can be spliced and constructed according to the pose positioning result and the feature point information collected by the camera in space. The map can be applied to the fields of building, investigation, unknown environment perception and virtual reality. BRIEF DESCRIPTION OF DRAWINGS

[0015] Figure 1 A step flowchart of the pose estimation method based on point-line features provided by the embodiment of the present application is provided. Figure 2 A FAST corner detection diagram provided by the embodiment of the present application is provided. Figure 3 A schematic diagram of the minimum circumscribed rectangle provided by the embodiment of the present application is provided. Figure 4 A schematic diagram of the line segment support domain strip provided by the embodiment of the present application is provided. Figure 5 A schematic diagram of the vanishing point diagram provided by the embodiment of the present application is provided. Figure 6 A schematic diagram of the vanishing two-point optimization diagram provided by the embodiment of the present application is provided. Figure 7 A schematic diagram of the determination of the dominant direction of the line segment provided by the embodiment of the present application is provided. Figure 8 A schematic diagram of the line feature extraction experiment provided by the embodiment of the present application is provided. Figure 9 A comparison result diagram provided by the embodiment of the present application is provided. DETAILED DESCRIPTION

[0016] The present application will be further described in detail below with reference to the accompanying drawings and embodiments. It can be understood that the specific embodiments described herein are only used to explain the related application, and not to limit the application. In addition, it should be noted that only the parts related to the application are shown in the drawings for ease of description.

[0017] It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict. The present application will be described in detail below with reference to the accompanying drawings and embodiments.

[0018] Embodiment 1 As mentioned in the background, in order to solve the problems in the prior art, the present application provides a pose estimation method based on point-line features, as shown in Figure 1 The method comprises the following steps: S1, collecting an environment image and extracting point features and line features from the environment image; S2, calculating one or more vanishing points in the environment image based on the extracted line features; S3, grouping the line features into different dominant direction sets according to the geometric relationship between each line feature and the vanishing point, and eliminating the line features that do not belong to any dominant direction set to obtain an optimized line feature set; S4, fusing the point features and the optimized line feature set as inputs, and performing pose estimation through a visual SLAM algorithm to output the pose information of the camera.

[0019] In a preferred embodiment, extracting point features and line features from the environment image comprises the following steps: Gaussian down-sampling the environment image to reduce noise; Calculating the gradient value and total gradient direction of each pixel point in the environment image in the x direction and the y direction; Pseudo-sorting the pixel points according to the gradient value and setting a level threshold; Taking the pixel point with the largest gradient value as a base point, generating a line segment support domain according to its gradient direction, and including the pixel points close to the gradient direction of the base point in the support domain; Constructing a minimum circumscribed rectangle based on the pixel points in the line segment support domain as a candidate line segment; Verifying whether the candidate line segment meets the preset alignment degree and pixel density conditions to determine the final extracted line features.

[0020] In a preferred embodiment, calculating one or more vanishing points in the environment image comprises the following steps: Clustering the line features for grouping and calculating a vanishing point for each line feature group; wherein for a group containing multiple spatial straight lines, the homogeneous coordinates of the initial estimated vanishing point are obtained by solving the singular value decomposition of a matrix composed of the coefficients of the straight lines in the group; Optimizing the homogeneous coordinates of the initial estimated vanishing point to minimize the sum of the distances between the midpoints of any straight lines in the group and the connecting lines between the midpoints and the vanishing point, to obtain the position coordinates of the vanishing point.

[0021] In a preferred embodiment, grouping the line features into different dominant direction sets and eliminating the line features that do not belong to any dominant direction set comprises the following steps: For each of the line features to be determined, the following operations are performed: Calculate the midpoint of the line feature; Connect the midpoint to each of the vanishing points to form a plurality of auxiliary straight lines; Calculate the sum of distances from the two endpoints of the line feature to each of the auxiliary straight lines; If there is an auxiliary straight line such that the sum of distances is less than a first preset threshold, determine that the line feature belongs to the vanishing point group and its dominant direction corresponding to the auxiliary straight line; If the sum of distances between the line feature and all auxiliary straight lines is not less than the first preset threshold, eliminate it as a mis-matched line feature.

[0022] In a preferred embodiment, before fusing the point features with the optimized line feature set, the following steps are further included: Describe each line feature in the optimized line feature set using an LBD descriptor; Wherein, a local coordinate system is established with the direction of the line segment and its normal direction as the axes, the line segment support domain is divided into a plurality of strips, the gradient statistical features of the pixel points in each strip are calculated, and the feature vectors of all strips are combined to form the final LBD descriptor for subsequent feature matching and pose solving.

[0023] In a preferred embodiment, the point features are extracted from the environment image, including the following steps: Point feature extraction is performed using an improved FAST corner detection algorithm; Wherein, when calculating the BRIEF descriptor for each key point, the directional description characteristics of the key point are added to make the point feature rotationally invariant.

[0024] In a preferred embodiment, the candidate line segment is verified for whether it meets the preset alignment and pixel density conditions to determine the final extracted line feature, including the following steps: Calculate the contour direction of the pixel points in the area of the minimum circumscribed rectangle; Compare the contour direction with the direction of the minimum circumscribed rectangle to calculate the direction difference; Statistically count the number of pixel points covered by the contour in the minimum circumscribed rectangle area, and calculate the proportion thereof to the total pixel points in the rectangle; If the direction difference is less than a second preset angle threshold, and the proportion is greater than a preset proportion threshold, determine that the candidate line segment is a valid line feature and extract it; otherwise, eliminate the candidate line segment.

[0025] In a preferred embodiment, the gradient statistical features of the pixel points in each strip are calculated, including the following steps: For each strip, a global gradient vector and a local gradient vector are calculated respectively; the global gradient vector is calculated by weighting the gradients of all pixels in the strip by a global Gaussian function; the local gradient vector is calculated by weighting the gradients of each row of pixels in the strip by a local Gaussian function; The global gradient vector and the local gradient vector are combined to construct a feature vector of the strip.

[0026] In a preferred embodiment, the homogeneous coordinates of the initial estimated vanishing point are optimized so that the sum of distances between any straight line in the group and the midpoint of the straight line to the connecting line between the two endpoints of the straight line and the vanishing point is minimized, comprising the following steps: An error function is constructed to minimize the sum of distances; A nonlinear least squares method is used to solve the error function, and the position coordinates of the vanishing point are iteratively optimized until the convergence condition is met.

[0027] Embodiment 2 The embodiment provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the point-line feature-based pose estimation method as described in Embodiment 1.

[0028] Specifically, the application is mainly applied to the positioning and navigation field. Traditional positioning uses GPS for positioning, but signal shielding, noise interference, and deceptive positioning may occur, resulting in positioning failure. Visual positioning and navigation is a self-positioning and navigation method. Traditional methods are based on point features extracted from collected images for pose estimation. The extraction algorithm of ORB feature points mainly includes key point extraction and descriptor calculation. Key point extraction is to extract improved FAST corner points, mainly to extract the pixel position of the feature point. Meanwhile, the directional description characteristics of the feature points are added, so that the corresponding feature points can be accurately identified even after rotating the feature points. The key point extraction principle is as shown in Figure 2 However, under the condition of low-texture images, accurate point feature extraction cannot be completed, so point-line feature comprehensive extraction is performed on structured features, and a point-line feature-based pose positioning algorithm is designed to improve the positioning accuracy in a structured low-texture environment.

[0029] The specific embodiments are described below: First, LSD line feature extraction Compared with ORB feature point extraction and matching, line feature extraction has higher accuracy and higher dimensional geometric information such as length and angle. Considering the extraction accuracy and real-time performance, the LSD (Line Segment Detector) algorithm is first introduced, and then the descriptor of LBD (Line band descriptor) is introduced to design the matching relationship of line features.

[0030] The core content of the LSD algorithm is: first, Gaussian down-sampling is performed on the image to avoid noise interference, then the pixel gradient is calculated, and the pixel points are classified and merged according to the similar gradient type. Finally, the line segment region (LSR) is formed by the classified and merged pixel groups, and the minimum pixel line segment is verified according to the classification to build the circumscribed rectangle of the line feature.

[0031] The specific steps of the LSD algorithm are as follows: Step 1: Process the input image, first perform Gaussian down-sampling to reduce noise, then scale the pixel size of the image, and then reduce the pixel step relationship of the image.

[0032] Step 2: Calculate the pixel gradient value and direction as shown in the following formula:

[0033] The above formula represents the pixel gray value, and are the gradients in the , directions, is the total gradient direction, is the gradient value calculated according to the gradients of the two directions.

[0034] Step 3: Sort the pixels in the image according to the gradient value, and set the corresponding level according to the threshold value.

[0035] Step 4: Set the pixel point with the maximum gradient value as the base point, and obtain the line segment support domain direction from the gradient direction of the base point. Then, starting from the base point, add the points close to the base point gradient direction to the line segment support domain, and update the corresponding direction.

[0036] Step 5: Construct the minimum circumscribed rectangle according to the centroid of the discrete line segment support domain, and set it as the candidate line segment for extraction and matching. The minimum circumscribed rectangle is shown in Figure 3 .

[0037] Step 6: Calculate the contour of the pixel points in the minimum circumscribed rectangle, select the contour close to the direction of the rectangle, and then calculate the ratio of the number of pixel points. If the ratio result is within the threshold value, the selected straight line feature is located.

[0038] Step 7: Traverse the entire image and extract all line features.

[0039] LBD descriptor is based on the base descriptor, and the weight coefficient is added to improve the real-time performance. The line segment strip support domain is as shown in Figure 4 .

[0040] Establish the direction of the line segment itself and its clockwise perpendicular direction as the axis to determine the main direction of the line segment and increase the rotation invariance. The global Gaussian function and the local Gaussian function are used for optimization design to reduce the interference and influence of the edge gradient and the strip.

[0041] The LBD descriptor is composed of the feature vectors of all strips:

[0042] wherein, is the feature vector of the strip , and the pixel gradient of the first row of the strip is:

[0043] wherein, is the Gaussian coefficient. The gradients of each row of the strip are combined to obtain its description matrix :

[0044] wherein, is the number of rows required to calculate the strip descriptor, and the values are as follows:

[0045] According to the mean vector and the standard deviation vector of the matrix , the line segment LBD descriptor is calculated:

[0046] According to the experimental conclusion and experience, when the number of strips and the width are best.

[0047] Then, the LSD extraction and matching are improved: ​​Currently, the traditional LSD algorithm has low real-time performance and high computational cost in line extraction, which greatly reduces the real-time performance of SLAM systems. Furthermore, it is prone to mismatches during the online feature descriptor calculation and matching process. Therefore, this paper proposes a line feature classification and extraction method, which is beneficial to improving the real-time performance and accuracy of matching.

[0048] like Figure 5 As shown, a set of parallel spatial lines Connecting to the optical center The three lines on the imaging plane are obtained from the two ends of the straight line. When straight lines in space extend to infinity, they will intersect at the same projection point in the image; this is known as linear projection. intersection That point is the line. The vanishing point can identify all building structure lines in the same dominant direction. The J-Linkage algorithm can group the detected lines according to the vanishing point.

[0049] First, find the vanishing point of each set of lines:

[0050] in, Represent a The matrix, The number of straight lines. represents the desired vanishing point Homogeneous coordinates.

[0051] By performing SVD decomposition on the above equation, we can obtain the vanishing point and perform pose estimation using nonlinear least squares optimization, so that any straight line within the group... endpoints , to the midpoint of the line segment With vanishing point The distance between the lines ( , The shortest path yields the most accurate vanishing point coordinates. For example... Figure 6 As shown.

[0052] After obtaining the precise coordinates of the vanishing point, the direction of the structural lines in the world coordinate system can be calculated using the vanishing point:

[0053] in, For the desired dominant direction and building structure line direction, This is the inverse matrix of the camera intrinsic parameters. Then, mismatched line features are removed: First, extract line features, filtering and removing excessively short line segments. Then, group the line features by vanishing point, selecting features whose direction aligns with the dominant direction. Remove line features that do not correspond to a vanishing point. Dominant direction The back projection is:

[0054] The dominant direction of structural lines is determined as follows: For each unfiltered line segment, its center point is connected to each vanishing point to construct auxiliary lines. The spatial relationship between the line segment and the auxiliary lines is evaluated to determine its affiliation: when the distance between the line segment and an auxiliary line is within a preset range, the line segment is determined to belong to that vanishing point and its corresponding dominant direction; conversely, if the distance exceeds the set range, it is not assigned. The specific criteria are as follows: calculate the sum of the distances from the two endpoints of the line segment to the auxiliary lines. If this cumulative value is less than a preset threshold, the line segment is confirmed to belong to that vanishing point and also to the corresponding dominant direction structural line. This determination process is consistent with the principle of method one shown in the diagram above. Figure 7 As shown.

[0055] Furthermore, to compare the effectiveness of the improved LSD line feature extraction and matching algorithm designed in this application for SLAM localization, and to comprehensively verify the line extraction method and pose estimation localization performance adopted in this paper, extensive comparative experiments were conducted on the EuRoC and TUM datasets. These datasets cover typical scenarios of various point and line feature experiments, including indoor and outdoor environments, and scenarios with missing structured features, allowing for comprehensive verification of the algorithm's stability and reliability. Figure 8 The diagram shows a comparison of line feature matching before and after the improvement of LSD line feature extraction and matching. Before the improvement, there were more mismatches, and localization and feature tracking easily failed after the line feature mismatch was completed. The improved line feature extraction and matching accuracy is significantly improved, greatly reducing visual mismatch results and localization failure rate. Localization experimental data analysis. As a control, this invention also selected two classic open-source visual SLAM schemes that also use point and line features: PL-SLAM and PL-VIO. PL-VIO uses the classic LSD algorithm to extract line features, and then uses LBD to calculate the descriptor of each line feature. PL-SLAM uses the same strategy as PL-VIO, but PL-SLAM modifies the front-end feature point extraction on the traditional ORBSLAM2 framework, adjusting it to feature line extraction based on LSD. This experiment conducted dataset verification experiments on the visual localization SLAM system using traditional line feature extraction and the improved line feature extraction and matching algorithm, such as... Figure 9 The figure shows a comparative experiment conducted using the TUM dataset and the freiburg1_desk database to verify the improved SLAM system against open-source PL-SLAM and PL-VIO. Figure 9(a) shows the trajectory error of the improved visual SLAM system compared with the ground truth value, the maximum error is 0.050 m, the minimum error is 0.002 m, but the overall trajectory error is relatively low, and the basic coincidence degree of the trajectory is basically blue. Figure 9 (c) and (d) show the error analysis results of the SLAM three-axis position and attitude after improving the point-line feature extraction, the black dotted line is the ground truth value, the blue curve is the PL-SLAM pose, and the green curve is the PL-VIO result, by comparison, only the red curve is closest to the ground truth value and has the highest coincidence degree. In order to verify the robustness and wide applicability of the algorithm, verification experiments were carried out on the data sets of EuRoC in various environments, the error results were compared and analyzed and summarized as shown in Table 1, it can be seen that on all data set sequences, the APE of the algorithm MODIFIED proposed in this paper is optimal on the whole, and the average is improved by 66% compared with the PL-SLAM based on ORB-SLAM2, and the improvement effect is obvious.

[0056] Table 1 APE (m) of different algorithms

[0057] In this paper, the LSD algorithm is improved, and the algorithm is evaluated from the angles of running time and positioning accuracy, and the running time overhead of the two key modules is tested and compared, and the experimental results are shown in Table 2.

[0058] Table 2 Running time comparison results (s)

[0059] In conclusion, the improved point line feature algorithm is used to improve the visual positioning algorithm in the image, the application of visual positioning navigation in the robot, military industry, intelligent driving and other fields is introduced, the problems such as signal loss, interference and deception of GPS are solved, the traditional ORB feature point extraction pose estimation algorithm cannot directly perform accurate pose estimation in the weak texture structured environment, and the problems such as feature loss are prone to occur, the application provides a pose estimation algorithm based on feature point line combination extraction, and the precision of visual positioning in the weak texture complex environment is improved. Specifically, the following is realized: effectively dealing with weak texture environment: by fusing point features and line features, and optimizing the line features by using vanishing points, the key problems that the traditional visual SLAM relying on point features only is prone to feature sparsity and tracking failure in low texture and structured environment (such as white wall, corridor and pure color workshop) are solved, so that stable and high-precision positioning in such harsh environment is realized. Improve the quality of geometric constraints: the space straight lines are grouped by using vanishing points, the line features with strong geometric significance (such as representing the main structure direction of the building) are screened out, and a large number of messy and short mismatched line segments are removed. This feature screening mechanism based on advanced geometric understanding (main direction) greatly improves the quality of the data source for pose estimation. Optimize the algorithm to improve the precision: in the line feature matching and vanishing point calculation, the nonlinear least square method and other optimization methods are used for iterative refinement, so that the expression of the line feature and the estimation of the space direction are more accurate, and the error of the pose estimation is reduced from the source.

[0060] The principles and implementation modes of the present application are described by applying specific examples in the present paper, and the above embodiment descriptions are only used to help understand the method and its core idea. The above descriptions are only preferred embodiments of the present application, and it should be pointed out that due to the limitation of language expression, there are infinite specific structures, and for ordinary skilled persons in the technical field, some improvements, decorations or changes can be made without departing from the principles of the present application, and the above technical features can be combined in an appropriate way; these improvements, decorations, changes or combinations, or the direct application of the inventive concept and technical scheme to other occasions without improvement, should be regarded as the protection scope of the present application.

Claims

1. A pose estimation method based on point and line features, characterized in that, Includes the following steps: Acquire environmental images and extract point and line features from the environmental images; Based on the extracted line features, one or more vanishing points in the environmental image are calculated; Based on the geometric relationship between each line feature and the vanishing point, the line features are grouped into different dominant direction sets, and line features that do not belong to any dominant direction set are removed to obtain an optimized line feature set. The point features and the optimized line feature set are fused together and used as input. The pose is estimated by the visual SLAM algorithm, and the pose information of the camera is output.

2. The pose estimation method based on point and line features according to claim 1, characterized in that: Extracting point and line features from the environmental image includes the following steps: The environmental image is downsampled using Gaussian to reduce noise; Calculate the gradient values ​​of each pixel in the x and y directions and the total gradient direction in the environmental image; The pixels are pseudo-sorted according to the gradient values ​​and a level threshold is set. Using the pixel with the largest gradient value as the base point, a line segment support domain is generated based on its gradient direction, and pixels with gradient directions close to the base point are included in this support domain. Construct a minimum bounding rectangle based on the pixels of the line segment support domain as a candidate line segment; Verify whether the candidate line segments meet the preset alignment and pixel density conditions to determine the final extracted line features.

3. The pose estimation method based on point and line features according to claim 1, characterized in that: Calculating one or more vanishing points in the environmental image includes the following steps: The line features are clustered into groups, and a vanishing point is calculated for each group of line features; wherein, for a group containing multiple spatial lines, the homogeneous coordinates of its vanishing point are initially estimated by solving the singular value decomposition of a matrix, the matrix being composed of the coefficients of each line in the group; The homogeneous coordinates of the initially estimated vanishing point are optimized to minimize the sum of the distances from the two endpoints of any straight line within the group to the line connecting the midpoint of that line and the vanishing point, thereby obtaining the position coordinates of the vanishing point.

4. The pose estimation method based on point and line features according to claim 1, characterized in that: Grouping the line features into different dominant direction sets and removing line features that do not belong to any of the dominant direction sets includes the following steps: For each line feature to be determined, perform the following operations: Calculate the midpoint of the line feature; Connect the midpoint to each of the vanishing points to form multiple auxiliary straight lines; Calculate the sum of the distances from the two endpoints of the line feature to each of the auxiliary lines; If there exists an auxiliary straight line such that the sum of the distances is less than a first preset threshold, then the line feature is determined to belong to the vanishing point group and its dominant direction corresponding to the auxiliary straight line. If the sum of the distances between the line feature and all auxiliary lines is not less than the first preset threshold, then it is discarded as a mismatched line feature.

5. The pose estimation method based on point and line features according to claim 1, characterized in that: Before fusing the point features with the optimized line feature set, the following steps are also included: The LBD descriptor is used to describe each line feature in the optimized line feature set; Specifically, a local coordinate system is established with the line segment's own direction and its normal direction as the axes. The line segment's support domain is divided into multiple strips. The gradient statistical features of pixels in each strip are calculated, and the feature vectors of all strips are combined to form the final LBD descriptor, which is used for subsequent feature matching and pose calculation.

6. The pose estimation method based on point and line features according to claim 1, characterized in that: Extracting point features from the environmental image includes the following steps: An improved FAST corner detection algorithm is used for point feature extraction; Specifically, when calculating the BRIEF descriptor for each key point, the orientation description characteristics of the key point are incorporated to make the point features rotationally invariant.

7. The pose estimation method based on point and line features according to claim 2, characterized in that: Verifying whether the candidate line segments meet preset alignment and pixel density conditions to determine the final extracted line features includes the following steps: Calculate the contour lines of the pixels within the region of the minimum bounding rectangle; Compare the direction of the contour lines with the direction of the minimum bounding rectangle, and calculate the difference in direction. Count the number of pixels covered by the contour lines within the minimum bounding rectangle, and calculate the ratio of the number of pixels covered to the total number of pixels within the rectangle; If the directional difference is less than a second preset angle threshold and the ratio is greater than a preset ratio threshold, then the candidate line segment is determined to be a valid line feature and is extracted; otherwise, the candidate line segment is discarded.

8. The pose estimation method based on point and line features according to claim 5, characterized in that: Calculate the gradient statistical features of pixels within each strip, including the following steps: For each strip, its global gradient vector and local gradient vector are calculated separately. The global gradient vector is calculated by weighting the gradients of all pixels in the strip with a global Gaussian function. The local gradient vector is calculated by weighting the gradients of each row of pixels in the strip with a local Gaussian function. The global gradient vector and the local gradient vector are combined to construct the feature vector of the strip.

9. The pose estimation method based on point and line features according to claim 3, characterized in that: Optimizing the homogeneous coordinates of the initially estimated vanishing point to minimize the sum of the distances from the two endpoints of any straight line within the group to the line connecting the midpoint of that line and the vanishing point includes the following steps: Construct an error function with the objective of minimizing the sum of the distances; The error function is solved using the nonlinear least squares method, and the position coordinates of the vanishing point are iteratively optimized until the convergence condition is met.

10. An electronic device, characterized in that, include: One or more processors; Memory, used to store one or more programs; When one or more programs are executed by one or more processors, the one or more processors implement the pose estimation method based on point and line features as described in any one of claims 1 to 9.

Images