Pose estimation method for monocular visual-inertial odometry system based on structural features

Abstract

The present invention discloses a method for estimating the posture of a monocular visual inertial odometry system based on structural features. The method comprises: obtaining matching point and line features of the image frame newly added to the sliding window and the previous frame; extracting the structural features of the newly added image frame based on a structural feature extraction algorithm; performing an orthogonal operation on the matched point and line features to construct landmark points and landmark lines in a world coordinate system; obtaining an IMU pre-integration error based on constraints between adjacent images; obtaining a point and line feature reprojection error based on the matching relationship between the landmark points, landmark lines, and point and line features; and optimizing a cost function including the point and line feature reprojection error, the IMU pre-integration error, and a priori error to determine the posture of the robot when the newly added image frame is captured.

Description

Technical Field

[0001] The present invention relates to the field of computer vision, and in particular to a method for estimating the position and posture of a monocular visual-inertial odometry system based on structural features. Background Art

[0002] In recent years, with the rapid development of computer vision technology, vision-based simultaneous localization and mapping (VSLAM) has been widely used in applications such as home robots, drones, autonomous driving, and virtual reality. However, compared to laser-based simultaneous localization and mapping, VSLAM suffers from insufficient robustness and poor accuracy. Therefore, incorporating wheel speed sensors and inertial measurement units (IMUs) into VSLAM systems has become a research hotspot. In this process, IMUs have garnered widespread attention due to their low cost, light weight, and complementary nature with cameras. The fusion of visual and inertial measurement information has significantly improved the accuracy and robustness of visual-inertial odometry (VIO). However, in some low-texture man-made scenes, VIO systems that rely solely on point features as visual constraints cannot extract sufficient point features and therefore fail to operate robustly. Low-texture scenes are often man-made, meaning they are often rich in structured objects such as furniture, walls, and appliances, which often contain a high number of line features. Line segment features can provide stronger geometric constraints and are insensitive to lighting changes. However, like point features, line segment features can only provide local constraints, which will cause cumulative errors.

[0003] At the same time, low-texture man-made scenes such as offices, factories, and underground garages often have relatively regular structures, defined as three mutually orthogonal dominant directions that satisfy the Manhattan hypothesis. These structural features, formed by these three mutually orthogonal dominant directions, can provide global rotation constraints, thereby eliminating cumulative rotation errors. However, existing structural feature extraction algorithms are often very time-consuming, severely impacting the real-time performance of VSLAM systems based on these algorithms. Summary of the Invention

[0004] In view of this, the present invention provides a posture estimation method for a monocular visual inertial odometry system based on structural features, which can solve the technical problem of estimating the robot's own posture during movement based on the measurement information obtained by the robot's onboard camera and IMU sensor without prior information about the environment.

[0005] In order to solve the above technical problems, the present invention is implemented as follows.

[0006] A method for determining the pose of a monocular visual-inertial odometry system based on structural features, comprising:

[0007] Step S1: Pre-integrate the IMU measurement values between the latest image frame added to the sliding window and the frame before the latest image frame to form a constraint between adjacent image frames; the sliding window includes ten frames of images obtained by the robot's monocular camera sensor;

[0008] Step S2: Obtain the matching point and line features of the newly added image frame and the previous frame; if the global structural features have not been initialized, proceed to step S3; otherwise, proceed to step S4;

[0009] Step S3: Obtain an initial pose estimate of the robot in the world coordinate system corresponding to each image frame in the sliding window; extract structural features from each image frame in the sliding window; and initialize global structural features in the world coordinate system;

[0010] Step S4: extracting the structural features of the latest added image frame based on the structural feature extraction algorithm;

[0011] Step S5: performing an orthogonal operation on the matched point and line features to construct landmark points and landmark lines in the world coordinate system;

[0012] Step S6: Based on the constraints between adjacent images, obtain the IMU pre-integration error; obtain the point-line feature reprojection error based on the landmark points, landmark lines, and point-line feature matching relationship; optimize the cost function including the point-line feature reprojection error, IMU pre-integration error, and prior error to determine the robot's posture when shooting the latest added image frame.

[0013] Preferably, the step S2 includes:

[0014] Extracting and matching the acquired point and line features, including:

[0015] Point features are matched by optical flow method;

[0016] The line segment features are extracted by the EDLines algorithm;

[0017] For the matching of line segment features, between two adjacent image frames, the matching line segment features belonging to different image frames conform to the following rules:

[0018] The distance between the LBD descriptors of the two line segment features is less than a first preset threshold;

[0019] The distance between the midpoints of the two line segment features is less than a second preset threshold;

[0020] The angle difference between the two line segment features is less than a third preset threshold;

[0021] The average gradient difference between the two line segment features is less than a fourth preset value.

[0022] Preferably, the step S3 includes: obtaining an initial pose estimate of the robot in the world coordinate system corresponding to each image frame in the sliding window based on the point-line feature matching relationship and the constraints between the adjacent image frames; extracting structural features for each image frame in the sliding window; and initializing the global structural features in the world coordinate system based on the initial pose estimate of the robot in the world coordinate system corresponding to each image frame and the structural features.

[0023] Preferably, the initial pose estimation of the robot in the world coordinate system and the initialization of the global structural features in the world coordinate system based on the structural features corresponding to each frame of the image include:

[0024] Step S301: The structural feature MF corresponding to the image frame in the sliding window i Converted to the world coordinate system,

[0025]

[0026] in, MF i In the world coordinate system, i represents the index of the image frame in the sliding window. They are the rotation of the IMU coordinate system relative to the world coordinate system and the rotation of the camera coordinate system relative to the IMU coordinate system; select the one with the most line segment features As the initial value of the global structural feature, the distance between the initial value of the global structural feature and other structural features in the sliding window is then calculated. The distance calculation formula is as follows:

[0027]

[0028] in, is the representation of the structural features of the jth frame in the world coordinate system, and the structural features with a distance less than 0.1 are recorded as inliers;

[0029] Step S302: If the inlier ratio is greater than 0.6, perform Slerp interpolation on all inliers to obtain the global structural feature MF W Otherwise, it is determined that the global structural feature initialization has failed, the sliding window moves, and the process goes to step S1.

[0030] Preferably, the step S4: extracting the structural features of the newly added image frame based on a structural feature extraction algorithm includes:

[0031] Step S41: If there is no new image frame added to the sliding window after the global structural feature is initialized successfully, the method ends; otherwise, proceeds to step S42;

[0032] Step S42: Acquire the latest image frame added in the sliding window; if the latest image frame added in the sliding window is the first image frame successfully initialized with global structural features, proceed to step S43; if not, proceed to step S45;

[0033] Step S43: constructing a polar coordinate grid of the newly added image frame based on the equivalent sphere, wherein the polar coordinate grid is used to store responses of any pair of line segment features in the newly added image frame within the sliding window;

[0034] Step S44: Projecting the gravity direction onto the most recently added image frame in the sliding window, generating multiple structural feature hypotheses based on the polar coordinate grid of the most recently added image frame and information about the most recently added image frame, and verifying the structural feature hypotheses; obtaining structural features corresponding to the most recently added image frame, and obtaining verified structural line segments of the most recently added image frame based on the structural features; updating the most recently added image frame in the sliding window to be the next frame of the most recently added frame in the current sliding window, and proceeding to step S41;

[0035] Step S45: If the number of structural line segments that successfully match the latest image frame added in the sliding window and the previous frame of the latest image frame is lower than the threshold, proceed to step S43; otherwise, a least squares problem is constructed based on the number of successfully matched structural line segments to directly solve the structural features of the latest image frame added in the sliding window, thereby obtaining the verified structural line segments of the latest image frame; the latest image frame in the sliding window is updated to the next frame of the latest image frame, and proceed to step S41.

[0036] Preferably, after step S5, the method further includes step S6: deleting landmark points and landmark lines whose visual reprojection errors exceed a preset threshold based on the optimization result.

[0037] Beneficial effects:

[0038] The present invention is a visual positioning method that constructs an environmental map and estimates the robot's own position during robot movement based on measurement information obtained by the robot's onboard camera and IMU sensor without prior environmental information.

[0039] This paper adopts the EDLines algorithm to extract line segment features. On this basis, a robust and efficient structural feature extraction method is designed based on the line feature matching relationship and gravity direction. The method is then introduced into a monocular visual inertial odometry based on point and line features, which improves the accuracy and robustness of the VIO system while maintaining the real-time performance of the system.

[0040] It has the following technical effects:

[0041] (1) The present invention is a positioning solution based on the fusion of visual information and IMU information. Visual positioning and IMU positioning are complementary to each other. The former is suitable for fast and short-term movements, while the latter is suitable for slow and long-term movements. Visual information can be used to estimate IMU zero bias, and IMU measurement can provide initial pose estimation for the camera.

[0042] (2) Based on the fusion of visual information and IMU information, line segment features are introduced into the monocular visual inertial odometry system, improving the robustness and accuracy of the monocular visual inertial odometry system in low-texture areas. The line segment features are extracted by the EDLines line segment extraction algorithm, which is a highly real-time line segment extraction algorithm. This ensures the real-time performance of the system after the introduction of line segment features. Line segment matching is performed using the LBD descriptor combined with geometric verification, effectively improving the accuracy of line segment matching.

[0043] (3) Based on the matching relationship between the initialization obtained gravity vector and the line segments of the previous and next frame images, an efficient and robust structural feature extraction algorithm is proposed, which greatly improves the speed of structural feature extraction.

[0044] (4) Based on the orthogonality of structural features, it is introduced into the monocular visual inertial odometry system as a global rotation constraint, which effectively reduces the existence of rotation cumulative error and improves the accuracy and robustness of the system.

[0045] (5) The present invention can directly provide position information for robots or other intelligent devices in artificial scenes. The method is simple to implement, easy to promote, and can run in real time. It has potential value in the fields of robotics, virtual reality, augmented reality, and unmanned driving.

[0046] (6) In the absence of prior information about the environment, the present invention, based on the measurement information of the camera and IMU sensor carried by the robot, takes visual positioning as the background, constructs an environmental map during the movement and estimates its own posture as the purpose, and studies the application of point line features and structural features in the simultaneous positioning and mapping system. The EDLines line segment extraction algorithm is adopted to effectively reduce the line segment feature extraction time; based on the line feature matching relationship and the gravity vector, a robust and efficient structural feature extraction algorithm is proposed. In artificial scenes that meet the Manhattan world hypothesis, such as offices, factories and underground garages, structural features can provide global rotation constraints, thereby effectively eliminating the rotation accumulation error. Thanks to the efficient line segment feature and structural feature extraction algorithm, the accuracy, robustness and real-time performance of the monocular visual inertial odometry with the introduction of structural features have been effectively improved. The feasibility and effectiveness of the invented method have been proved by testing on the data set. The present invention has potential value in many application fields, such as autonomous driving, virtual reality, augmented reality, aerospace, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 A flow chart of a method for determining the pose of a monocular visual-inertial odometry system based on structural features provided by the present invention;

[0048] Figure 2 A schematic diagram of the structure of a method for determining the position and posture of a monocular visual-inertial odometry system according to the present invention;

[0049] Figure 3 Schematic diagram of the equivalent sphere used in the structural feature extraction algorithm provided by the present invention;

[0050] Figure 4 A schematic diagram showing the rotation error of the Euroc dataset MH_02_easy sequence provided by the present invention and comparing it with other methods;

[0051] Figure 5 A schematic diagram showing the translation error of the Euroc dataset MH_02_easy sequence provided by the present invention and comparing it with other methods;

[0052] Figure 6 A schematic diagram showing the rotation error of the Euroc dataset MH_04_difficult sequence provided by the present invention and comparing it with other methods;

[0053] Figure 7 This is a schematic diagram of the translation error of the Euroc dataset MH_04_difficult sequence provided by the present invention and its comparison with other methods. DETAILED DESCRIPTION

[0054] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0055] like Figure 1-Figure 2 As shown, the present invention proposes a method for determining the pose of a monocular visual inertial odometry system based on structural features, comprising:

[0056] Step S1: Pre-integrate the IMU measurement values between the latest image frame added to the sliding window and the frame before the latest image frame to form a constraint between adjacent image frames; the sliding window contains the latest ten frames of images obtained by the robot's monocular camera sensor;

[0057] Step S2: Obtain the matching point and line features of the newly added image frame and the previous frame; if the global structural features have not been initialized, proceed to step S3; otherwise, proceed to step S4;

[0058] Step S3: Obtain an initial pose estimate of the robot in the world coordinate system corresponding to each image frame in the sliding window; extract structural features from each image frame in the sliding window; and initialize global structural features in the world coordinate system;

[0059] Step S4: extracting the structural features of the newly added image frame based on a structural feature extraction algorithm;

[0060] Step S5: performing an orthogonal operation on the matched point and line features to construct landmark points and landmark lines in the world coordinate system;

[0061] Step S6: Based on the constraints between adjacent images, obtain the IMU pre-integration error; obtain the point-line feature reprojection error based on the landmark points, landmark lines, and point-line feature matching relationship; optimize the cost function including the point-line feature reprojection error, IMU pre-integration error, and prior error to determine the robot's posture when shooting the latest added image frame.

[0062] In the present invention, the robot estimates the robot's trajectory based on the visual information obtained by the monocular camera sensor and the inertial information obtained by the IMU sensor.

[0063] Furthermore, after step S5, step S6 is further included: deleting landmark points and landmark lines whose visual reprojection errors exceed a preset threshold based on the optimization result.

[0064] Said step S1, wherein:

[0065] The newly added image frame in the sliding window acquired by the robot is pre-integrated with the IMU measurement value of the previous frame of the newly added image frame to form a constraint between adjacent images.

[0066] In this embodiment, the IMU measurements between two image frames are integrated based on the pre-integration theory to form a constraint between the two image frames. The sliding window includes multiple image frames, and the IMU measurements between two adjacent image frames are pre-integrated once to obtain constraints between several adjacent images.

[0067] The sliding window contains the latest ten frames of images obtained by the robot's monocular camera sensor.

[0068] Furthermore, step S2 includes: extracting point and line features from the newly added image frame; matching the point and line features based on data of adjacent image frames of the newly added image frame to obtain successfully matched point and line features, wherein the point and line features include point features and line segment features; if the global structural features have not been initialized, proceeding to step S3; otherwise, proceeding to step S4. The global structural features are the representation of the structural features in the world coordinate system.

[0069] The step S2 includes:

[0070] The extracting and matching the point and line features includes:

[0071] Point features are matched using the optical flow method.

[0072] The line segment features are extracted by the EDLines algorithm with high real-time performance.

[0073] Line segment features are matched by combining LBD descriptors with geometric verification. Between two adjacent image frames, matching line segment features belonging to different image frames meet the following rules:

[0074] The distance between the LBD descriptors of the two line segment features is less than a first preset threshold;

[0075] The distance between the midpoints of the two line segment features is less than a second preset threshold;

[0076] The angle difference between the two line segment features is less than a third preset threshold;

[0077] The average gradient difference between the two line segment features is less than a fourth preset threshold.

[0078] Furthermore, the step S3 includes: obtaining an initial pose estimate of the robot in the world coordinate system corresponding to each image frame in the sliding window based on the point-line feature matching relationship and the constraints between the adjacent image frames; extracting structural features for each image frame in the sliding window; and initializing the global structural features in the world coordinate system based on the initial pose estimate of the robot in the world coordinate system corresponding to each image frame and the structural features.

[0079] Extracting structural features from each image frame within the sliding window and initializing global structural features in the world coordinate system based on the initial pose estimate of the robot corresponding to each image frame and the structural features includes: aligning visual measurement information and inertial measurement information between all adjacent images within the sliding window to obtain information such as the initial pose estimate, scale, and gravity vector of the robot corresponding to each image frame in the sliding window; extracting structural features from each image frame within the sliding window based on an exhaustive superposition random sampling consistent structural feature extraction algorithm; and simultaneously calculating the angle between the three principal axes of the line segment features of the image frame and the corresponding structural features. Line segments with angles less than 4° are structural line segments corresponding to the structural features. Initializing global structural features in the world coordinate system based on the initial pose estimate of the robot corresponding to each image frame and the structural features. If initialization is successful, proceed to step S4; otherwise, move the sliding window and proceed to step S1.

[0080] Furthermore, the initial pose estimation of the robot in the world coordinate system and the structural features corresponding to each frame of the image initializing the global structural features in the world coordinate system include:

[0081] Step S301: The structural feature MF corresponding to the image frame in the sliding window i Converted to the world coordinate system,

[0082]

[0083] in, MF i In the world coordinate system, i represents the index of the image frame in the sliding window. They are the rotation of the IMU coordinate system relative to the world coordinate system and the rotation of the camera coordinate system relative to the IMU coordinate system; select the one with the most line segment features As the initial value of the global structural feature, the distance between the initial value of the global structural feature and other structural features in the sliding window is then calculated. The distance calculation formula is as follows:

[0084]

[0085] in, is the representation of the structural features of the jth frame in the world coordinate system, and the structural features with a distance less than 0.1 are recorded as inliers;

[0086] Step S302: If the inlier ratio is greater than 0.6, perform Slerp interpolation on all inliers to obtain the global structural feature MF W Otherwise, it is determined that the global structural feature initialization has failed, the sliding window moves, and the process goes to step S1.

[0087] Most man-made scenes, such as offices, factories, and underground garages, tend to have regular structures, meaning they satisfy the Manhattan hypothesis, consisting of three mutually orthogonal dominant directions. The vast majority of line features in such scenes are parallel to one of these three orthogonal dominant directions. Based on this premise, an exhaustive superposition random sampling consensus algorithm is used to extract structural features from the image frame based on the line features extracted from the image. This allows us to obtain a representation of these structural features in the world coordinate system, namely the global structural features. The introduction of global structural features can effectively reduce the system's cumulative error.

[0088] Furthermore, the step S4 includes: extracting structural features of the newly added image frame based on the successfully matched line segment features and gravity vectors.

[0089] The step S4: extracting the structural features of the newly added image frame based on a structural feature extraction algorithm, comprises:

[0090] Step S41: If there is no new image frame added to the sliding window after the global structural feature is initialized successfully, the method ends; otherwise, proceeds to step S42;

[0091] Step S42: Acquire the latest image frame added in the sliding window; if the latest image frame added in the sliding window is the first image frame successfully initialized with global structural features, proceed to step S43; if not, proceed to step S45;

[0092] Step S43: constructing a polar coordinate grid of the newly added image frame based on the equivalent sphere, wherein the polar coordinate grid is used to store responses of any pair of line segment features in the newly added image frame within the sliding window;

[0093] Step S44: Projecting the gravity direction onto the most recently added image frame in the sliding window, generating multiple structural feature hypotheses based on the polar coordinate grid of the most recently added image frame and information about the most recently added image frame, and verifying the structural feature hypotheses; obtaining structural features corresponding to the most recently added image frame, and obtaining verified structural line segments of the most recently added image frame based on the structural features; updating the most recently added image frame in the sliding window to be the next frame of the most recently added frame in the current sliding window, and proceeding to step S41;

[0094] Step S45: If the number of structural line segments that successfully match the latest image frame added in the sliding window and the previous frame of the latest image frame is lower than the threshold, proceed to step S43; otherwise, a least squares problem is constructed based on the number of successfully matched structural line segments to directly solve the structural features of the latest image frame added in the sliding window, thereby obtaining the verified structural line segments of the latest image frame; the latest image frame in the sliding window is updated to the next frame of the latest image frame, and proceed to step S41.

[0095] Furthermore, the step S43 includes:

[0096] like Figure 3 As shown, the polar coordinate grid is constructed based on an equivalent sphere, which is a unit sphere with its center at the camera focus, whose X-axis and Y-axis are parallel to the X-axis and Y-axis of the image plane of the latest image frame in the sliding window, and the Z-axis points from the focus of the latest image frame in the sliding window to the image principal point;

[0097] The principal point of the latest image frame in the sliding window is denoted as (x0, y0) T , and the focal length is f, any pixel point (x,y) on the image plane T The coordinates P = [X, Y, Z] in the equivalent spherical coordinate system can be obtained by the following formula T :

[0098]

[0099] Its latitude and longitude coordinates are expressed as:

[0100]

[0101] Where φ∈[0,π / 2], λ∈[0,2π], X, Y, Z are x, y, z axes respectively;

[0102] The polar coordinate grid G has a size of 90×360 and an accuracy of 1°. For t=1, 2, 3, ..., 90, k=1, 2, 3, ..., 360, the initial value of G(t, k) is 0.

[0103] Select any pair of line segment features l1, l2 on the latest image frame in the sliding window and find the longitude and latitude coordinates (φ deg ,λ deg ), and then update the corresponding grid. The formula is as follows:

[0104] G(φ deg ,λ deg )=G(φ deg ,λ deg )+||l1||×||l2||×sin(2θ)

[0105] Where ||l|| is the length of the line segment corresponding to the line segment feature, θ is the angle between the two line segments, and φ deg is the longitude corresponding to the intersection point p, λ deg is the latitude corresponding to the intersection point p, G(φ deg ,λ deg ) is the value corresponding to the polar coordinate grid corresponding to the intersection point p, and a 3×3 Gaussian filter is applied to the obtained polar coordinate grid.

[0106] In this embodiment, the polar coordinate grid construction process is a process of recording the response of each pair of line segments to the polar coordinate grid.

[0107] The step S44: projecting the gravity direction onto the latest image frame in the sliding window, generating a plurality of structural feature hypotheses based on the polar coordinate grid of the latest image frame and information of the latest image frame, and verifying the structural feature hypotheses; obtaining structural features corresponding to the latest image frame, and obtaining verified structural line segments of the latest image frame based on the structural features, including:

[0108] The initial gravity vector obtained by initialization is projected onto the image plane of the latest image frame added in the sliding window to obtain the initial estimate VP of the first principal axis of the structural feature init Calculate the angles between all line segment features of the newly added image frame and the first principal axis of the structural feature. Line segments with angles less than 4° belong to the structural line segment cluster L1 corresponding to the first principal axis of the structural feature. If the number of line segments contained in L1 is greater than 4, then VP initThe optimization is performed to obtain the hypothesis VP1 of the first principal axis vector of the structural feature. The specific formula is as follows:

[0109]

[0110] In this embodiment, the least squares problem is constructed for VP init Optimization is performed to obtain the first principal axis vector VP1 of the structural feature.

[0111] Where S∈R 3×n , T is transpose, R is the real number field, VP is the first principal axis vector of the structural feature to be estimated, s l It is the normal vector of the plane formed by the lth structural line segment and the origin of the camera coordinate system in the camera coordinate system. l is the subscript used to describe that the structural line segment is the lth structural line segment corresponding to the first principal axis VP1 of the structural feature, n is the number of structural line segment features included in L1, K is the camera intrinsic parameter, l jstart ,l jend They correspond to the normalized coordinates of the starting point and end point of the lth line segment respectively.

[0112] Based on the orthogonality of the structural features, the first principal axis VP1 of the given structural feature is (X1, Y1, Z1) T After that, the second principal axis VP2 of the structural feature must lie on its supercircle. Therefore, 360 hypotheses of VP2 are obtained by uniformly sampling the supercircle with an accuracy of 1°. The longitude λ of the m-th hypothesis is m×2π / 360, and the latitude φ is solved by the following two constraints:

[0113]

[0114] X1×X2+Y1×Y2+Z1×Z2=0

[0115] Among them, X1, Y1, and Z1 are the X, Y, and Z axis coordinates of the first principal axis VP1 of the structural feature in the camera coordinate system, and X2, Y2, and Z2 are the X, Y, and Z axis coordinates of the second principal axis VP2 of the structural feature in the camera coordinate system.

[0116] The third principal axis of the structural feature, VP3, is obtained by cross-producting VP1 and VP2. The 360 sets of structural feature hypotheses VP1, VP2, and VP3 are expressed in polar coordinates as (λ1, φ1), (λ2, φ2), and (λ3, φ3). The corresponding line segment response of each structural feature hypothesis is then queried based on the polar coordinate grid:

[0117] U=G(φ1,λ1)+G(φ2,λ2)+G(φ3,λ3)

[0118] Among them, U is the line segment response corresponding to the structural feature hypothesis, φ1, λ1 are the longitude and latitude coordinates of the first principal axis vector VP1 of the structural feature, φ2, λ2 are the longitude and latitude coordinates of the second principal axis vector VP2 of the structural feature, φ3, λ3 are the longitude and latitude coordinates of the third principal axis vector VP3 of the structural feature, G(φ1, λ1) is the line segment response of the first principal axis vector VP1 of the structural feature, G(φ2, λ2) is the line segment response of the second principal axis vector VP2 of the structural feature, and G(φ3, λ3) is the line segment response of the third principal axis vector VP3 of the structural feature; the structural feature hypothesis with the largest line segment response is the optimal estimate. After obtaining the optimal estimate of the structural feature of the latest added image frame, the line segment features of the latest frame of the sliding window are classified based on the optimal estimate of the structural feature to obtain the structural line segment feature set L1, L2, L3 with an angle less than 4° with the VP1, VP2, VP3 vectors.

[0119] Step S45: if the number of successfully matched structural line segments of the latest image frame added in the sliding window and the frame before the latest image frame is less than a threshold, then proceed to step S43; otherwise, construct a least squares problem based on the number of successfully matched structural line segments, directly solve the structural features of the latest image frame added in the sliding window, and thus obtain the verified structural line segments of the latest image frame; and update the latest image frame in the sliding window to the next frame of the latest image frame, including:

[0120] Based on the matching of the line segment features of the latest video frame in the sliding window and the second latest frame, a set of structural line segments that successfully match the latest video frame in the sliding window is obtained, and the set is sorted from large to small according to the number of structural line segment features contained therein to obtain L 1p ,L 2p ,L 3p If L 2p If the number of structure lines included is less than 6, then go to step S43; otherwise, extract the structural features of the latest video frame in the sliding window based on the matched structure line set. 1p ,L 2p Construct a least squares problem to solve and obtain the corresponding vanishing point estimate VP 1init ,VP 2init , then cross-multiply the two to get VP 3init , and finally to VP 1init ,VP 2init ,VP 3init Schmidt orthogonalization is performed to obtain the three principal axis vectors VP1, VP2, and VP3 of the structural features of the frame, and on this basis, the set of structural line segments of the frame is obtained for processing subsequent image frames, and the latest image frame in the sliding window is updated to the next frame of the latest image frame in the sliding window.

[0121] Said step S5, wherein:

[0122] The successfully matched point and line features are in the camera coordinate system. Based on orthogonalization, the passed point and line features in the camera coordinate system are converted to the world coordinate system, and then the landmark points and landmark lines in the world coordinate system are constructed.

[0123] After step S6, the method further includes step S7:

[0124] Based on the optimization results, point and road landmarks whose reprojection errors formed by S5 are greater than a fifth preset threshold are deleted. The purpose is to reduce the errors caused by mismatching.

[0125] Furthermore, another embodiment illustrates the effects of the present invention.

[0126] Hardware environment: computer; correlator

[0127] Software configuration: Linux; ROS Kinetic

[0128] The simultaneous localization and mapping system is described as a system in which a robot equipped with characteristic sensors constructs a map of the environment based on noisy sensor measurements during movement and estimates its own position.

[0129] The present invention was tested and evaluated on the Euroc dataset. To verify the effectiveness of the structural feature extraction algorithm, the experiment included a comparison with the monocular visual-inertial odometry MF-VIO, which uses another well-known structural feature extraction algorithm and has the same other parts as the present invention. The other comparison systems are well-known open source systems in recent years. Table 1 shows the root mean square error (RMSE) of translation and rotation of the monocular visual-inertial odometry FMF-VIO based on structural features on the Euroc dataset, and bolds and marks the two best results for each sequence. The experimental results show that compared with VINS-Mono, which does not contain line segment features and structural features, the FMF-VIO system proposed in the present invention has significantly improved the accuracy of translation and rotation. Compared to PL-VIO, which includes line features but not structural features, FMF-VIO achieves significantly improved rotational accuracy across most sequences. This improvement in rotational accuracy also improves FMF-VIO's translational accuracy across most sequences, with the exception of the MH_01_easy dataset, where it degrades due to the long, slow motion of the drone during recording of this sequence, preventing effective utilization of structural feature measurements. Compared to the MF-VIO system, which also utilizes structural features, the proposed FMF-VIO system achieves similar accuracy.

[0130] Figure 4 and Figure 5This figure intuitively demonstrates the rotation and translation errors of the PL-VIO and FMF-VIO algorithms for the MH_02_easy sequence. As can be seen from the figure, the proposed FMF-VIO system has smaller translation and rotation errors. This is consistent with the conclusions drawn in Table 1.

[0131] Figure 6 and Figure 7 The figure shows the rotation and translation errors of MF-VIO and FMF-VIO using different structural feature extraction algorithms on the MH_04_difficult dataset. As can be seen, the rotation and translation errors obtained by the proposed FMF-VIO system are comparable to those obtained by MF-VIO. This is consistent with the data in Table 1.

[0132] In terms of real-time performance, a comparative analysis was conducted on the EuRoc dataset between the proposed structural feature extraction algorithm G2MF, which is based on the matching relationship between gravity vectors and line segments in adjacent frame images, the structural feature extraction algorithm GMF, which is based solely on gravity vectors, and the well-known structural feature extraction algorithm MF. Table 2 lists the average time taken to extract structural features from each frame in each sequence using the algorithm adopted by the present invention and the comparison algorithm. It can be seen that the average time taken by the algorithm G2MF proposed in this invention to process a single frame of image is much less than that of the open source algorithm MF. Compared with the exhaustive vanishing point extraction algorithm GMF based solely on gravity, the real-time performance of the algorithm proposed in this invention is also improved.

[0133] This example also evaluated the average execution time of PL-VIO and FMF-VIO on the MH_05_difficult dataset, a typical image sequence of man-made scenes with abundant structural line segments and intense camera motion. Table 3 shows the execution time of the two systems in different units. It can be seen that by replacing the LSD algorithm with EDLines and adopting a more robust and efficient structural feature extraction algorithm, FMF-VIO, while introducing structural features, still achieves significant real-time performance improvements compared to PL-VIO. It can run in real time on the Euroc dataset at twice the speed of PL-VIO.

[0134] In summary, the experimental results show that the FMF-VIO system proposed in this paper is superior to VINS-Mono, PL-VIO and MF-VIO, and achieves a good positioning effect.

[0135] Table 1 Root mean square error (RMSE) of translation and rotation

[0136]

[0137] Table 2 Real-time performance of vanishing point extraction algorithm (ms)

[0138]

[0139] Table 3 Average execution time per frame of different units of PL-VIO and FMF-VIO (ms)

[0140]

[0141] The above specific embodiments merely illustrate the design principles of the present invention. The shapes and names of the components described herein may vary and are not limiting. Therefore, those skilled in the art may modify or substitute equivalents for the technical solutions described in the above embodiments. Such modifications and substitutions, without departing from the inventive spirit and technical solutions of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A method for pose estimation of a monocular visual inertial odometry system based on structural features, characterized in that: include: Step S1: Pre-integrate the IMU measurement values between the latest image frame added to the sliding window and the frame before the latest image frame to form a constraint between adjacent image frames; the sliding window includes ten frames of images obtained by the robot's monocular camera sensor; Step S2: obtaining matching point and line features between the newly added image frame and the previous frame; If the global structural features have not been initialized, go to step S3; otherwise, go to step S4; Step S3: Obtain an initial pose estimate of the robot in the world coordinate system corresponding to each image frame in the sliding window; extract structural features for each image frame in the sliding window; Initialize the global structural features in the world coordinate system; Step S4: extracting structural features of the latest added image frame based on a structural feature extraction algorithm; Step S5: performing an orthogonal operation on the matched point and line features to construct the landmark points and landmark lines in the world coordinate system; Step S6: Based on the constraints between adjacent images, obtain the IMU pre-integration error; obtain the point-line feature reprojection error based on the landmark points, landmark lines, and point-line feature matching relationship; optimize the cost function including the point-line feature reprojection error, IMU pre-integration error, and prior error to determine the robot's posture when shooting the latest added image frame.

2. The method according to claim 1, wherein The step S2 includes: Extracting and matching the acquired point and line features, including: Point features are matched by optical flow method; The line segment features are extracted by the EDLines algorithm; For the matching of line segment features, between two adjacent image frames, the matching line segment features belonging to different image frames conform to the following rules: The distance between the LBD descriptors of the two line segment features is less than a first preset threshold; The distance between the midpoints of the two line segment features is less than a second preset threshold; The angle difference between the two line segment features is less than a third preset threshold; The average gradient difference between the two line segment features is less than a fourth preset threshold.

3. The method according to claim 1, wherein: The step S3 includes: obtaining an initial pose estimate of the robot corresponding to each image frame in the sliding window in the world coordinate system based on the point-line feature matching relationship and the constraints between the adjacent image frames; extracting structural features from each image frame in the sliding window; and initializing the global structural features in the world coordinate system based on the initial pose estimate of the robot corresponding to each image frame in the world coordinate system and the structural features.

4. The method according to claim 3, wherein The initial pose estimation of the robot in the world coordinate system and the initialization of the global structural features in the world coordinate system based on the structural features corresponding to each frame of the image include: Step S301: The structural feature MF corresponding to the image frame in the sliding window i Converted to the world coordinate system, in, MF i In the world coordinate system, i represents the index of the image frame in the sliding window. They are the rotation of the IMU coordinate system relative to the world coordinate system and the rotation of the camera coordinate system relative to the IMU coordinate system; select the one with the most line segment features As the initial value of the global structural feature, the distance between the initial value of the global structural feature and other structural features in the sliding window is then calculated. The distance calculation formula is as follows: in, is the representation of the structural features of the jth frame in the world coordinate system, and the structural features with a distance less than 0.1 are recorded as inliers; Step S302: If the inlier ratio is greater than 0.6, perform Slerp interpolation on all inliers to obtain the global structural feature MF W Otherwise, it is determined that the global structural feature initialization has failed, the sliding window moves, and the process goes to step S1.

5. The method according to claim 4, wherein The step S4: extracting the structural features of the newly added image frame based on a structural feature extraction algorithm, comprises: Step S41: If there is no new image frame added to the sliding window after the global structural feature is initialized successfully, the method ends; otherwise, proceeds to step S42; Step S42: Acquire the latest image frame added in the sliding window; if the latest image frame added in the sliding window is the first image frame whose global structural features are successfully initialized, proceed to step S43; otherwise, proceed to step S45; Step S43: constructing a polar coordinate grid of the newly added image frame based on the equivalent sphere, wherein the polar coordinate grid is used to store responses of any pair of line segment features in the newly added image frame within the sliding window; Step S44: Projecting the gravity direction onto the most recently added image frame in the sliding window, generating multiple structural feature hypotheses based on the polar coordinate grid of the most recently added image frame and information about the most recently added image frame, and verifying the structural feature hypotheses; obtaining structural features corresponding to the most recently added image frame, and obtaining verified structural line segments of the most recently added image frame based on the structural features; updating the most recently added image frame in the sliding window to the next frame of the most recently added frame in the current sliding window, and proceeding to step S41; Step S45: If the number of structural line segments that successfully match the latest image frame added in the sliding window and the previous frame of the latest image frame is lower than the threshold, proceed to step S43; otherwise, a least squares problem is constructed based on the number of successfully matched structural line segments to directly solve the structural features of the latest image frame added in the sliding window, thereby obtaining the verified structural line segments of the latest image frame; the latest image frame in the sliding window is updated to the next frame of the latest image frame, and proceed to step S41.

6. The method according to any one of claims 1 to 5, wherein After step S5, the method further includes step S6: deleting landmark points and landmark lines whose visual reprojection errors exceed a preset threshold based on the optimization result.

Images