Tightly coupled point-line visual-inertial odometry with line feature geometry optimization

Abstract

The patent provides a tight coupling visual inertial odometer method based on point and geometric optimization line features, including the following steps: in the front end, filtering out redundant short line segments through length threshold, geometric constraint merging of broken line segments based on direction consistency and spatial proximity, and adopting endpoint distance verification mechanism to ensure the geometric consistency of the merging result; the optimized two-dimensional line segment is triangulated into a three-dimensional space line through epipolar geometry constraint; in the back end, a sliding window tight coupling optimization framework is constructed by fusing point line re-projection residual, IMU pre-integral residual and prior information, and each residual term is minimized to realize high-precision state estimation. Through line feature geometric optimization and tight coupling optimization framework, the patent effectively improves the pose estimation accuracy and system robustness in challenging scenes such as weak texture and repetitive structure.

Description

Technical Field

[0001] This patent pertains to high-precision state estimation tasks in visual inertial odometry, based on point and line feature fusion and geometric optimization strategies, and is designed for real-time localization and mapping in challenging environments such as those with weak textures. Background Technology

[0002] Visual inertial odometry (VIO) achieves real-time state estimation and environmental mapping for mobile robots by fusing data from cameras and inertial measurement units (IMUs), making it a core technology in fields such as autonomous navigation and augmented reality. VIO can be broadly categorized into two methods based on sensor data fusion strategies: loosely coupled and tightly coupled. In the loosely coupled method, the camera and IMU modules independently perform pose estimation, which is then fused. The tightly coupled method, on the other hand, directly fuses the raw data from both sensors, achieving state estimation through joint optimization. This approach better utilizes the complementary properties of various sensors and offers higher estimation accuracy and robustness, making it the current mainstream research direction.

[0003] From the perspective of visual information processing, VIO methods are mainly divided into two categories: feature point methods and direct methods. Direct methods estimate camera pose by minimizing photometric errors between images, making them suitable for weakly textured scenes, but they are easily affected by factors such as lighting changes and motion blur. Feature point methods select representative pixels on the image for feature matching, and then use algorithms such as epipolar geometry or PnP to estimate the relative motion of the camera. However, in environments with weak textures, repetitive structures, or drastic lighting changes, point features are prone to degradation, leading to tracking loss and pose drift. It is worth noting that rich line features in artificial environments can still be stably extracted and tracked in weakly textured regions, providing stronger geometric constraints, thus providing crucial support for improving the robustness and accuracy of VIO.

[0004] In the early stages of VIO research, line features were primarily used for offline 3D reconstruction and small-scale visual odometry, initially validating their geometric recovery capabilities. However, limitations in computational efficiency and matching robustness made it difficult to meet real-time requirements. Subsequently, researchers continuously improved line feature VIO from multiple dimensions. Pumarola et al. first proposed a pure line feature initialization mechanism and constructed a point-line tightly coupled SLAM system, achieving cold start in weakly textured scenes. However, the full processing of line features led to a decrease in real-time performance. Rong et al. introduced the EDLines algorithm to accelerate line feature extraction and proposed a screening mechanism based on entropy scale and geometric constraints. However, the shortcomings of EDLines in extraction accuracy and line segment integrity limited its potential to provide strong geometric constraints. Furthermore, existing methods still mainly rely on cross-frame line segment matching and tracking during feature processing, with insufficient consideration for geometric information and line segment structural integrity within a single frame. This results in redundant short line segments and fragmented broken line segments affecting the accuracy of backend optimization.

[0005] As VIO demands increasing real-time performance and robustness in fields such as drones and autonomous driving, designing a tightly coupled point-line VIO method that can effectively utilize single-frame geometric information at the front end to eliminate line segment redundancy, construct stable representations, and thus provide high-quality geometric constraints for the back end, thereby reducing positioning drift and computational burden caused by line segment fragmentation, remains a current research focus. Summary of the Invention

[0006] This patent proposes a tightly coupled visual-inertial odometry method based on point and geometrically optimized line features. First, to improve the quality of line features, a front-end geometric optimization strategy is designed, including short segment selection based on length thresholds, merging of broken line segments based on directional consistency and spatial proximity, and verification of the merged results based on endpoint distance constraints. Then, the optimized two-dimensional line segments are triangulated into three-dimensional spatial lines through epipolar geometric constraints, employing a hybrid parameterization method combining Plück coordinates and orthogonal normalization representation. Based on the aforementioned high-quality line features, a line reprojection residual model is constructed, fusing IMU pre-integration residuals, point reprojection residuals, and prior information. Within a sliding window tightly coupled optimization framework, each residual term is jointly minimized to achieve high-precision robust state estimation. Experimental results on publicly available datasets show that this patented method reduces absolute pose error by approximately 5% to 15% compared to existing methods, and improves line feature processing efficiency by approximately 4%.

[0007] The contributions of this patent can be summarized in the following three aspects: 1. A line feature geometry optimization method is proposed, which outputs high-quality line features with continuous structure and consistent geometry through length filtering and multi-constraint merging strategies combined with endpoint distance verification mechanism; 2. An optimized line feature triangulation method based on epipolar geometry is designed to reconstruct the geometrically optimized two-dimensional line segments into three-dimensional spatial lines, providing high-quality geometric constraints for the backend; 3. By integrating the tightly coupled optimization framework of geometrically optimized line features, the line feature reprojection residuals are deeply integrated with point features and IMU pre-integration residuals, which improves the pose estimation accuracy and robustness of the system in challenging environments.

[0008] 1. A tightly coupled visual-inertial odometry method based on point and geometrically optimized line features, comprising the following steps:

[0009] First, front-end line feature geometry optimization processing

[0010] This patent sets key parameters for the LSD algorithm and uses them to remove excessively short line segments from the image. First, the number of pyramid layers is set to 2 to create a multi-scale representation, thereby capturing the geometric features of line segments at different resolutions. Second, the scale factor is adjusted to 0.5 for image downsampling. Furthermore, the minimum density threshold is set to 0.6 to filter low-density line segment features, ensuring that the extracted line segments have sufficient geometric saliency. Finally, redundant short line segments are filtered out using a length threshold, defined as follows:

[0011]

[0012] Where η is the length scaling factor, W I H I For the image width and height, This is a rounding up operation;

[0013] Geometric constraints are applied to merge broken line segments based on directional consistency and spatial proximity. The longer line segment is selected as a reference, and the direction vectors of the two line segments are calculated. Their directional consistency is determined by the dot product and the included angle. If the dot product is negative, it indicates that the two line segments are in opposite directions; in this case, the endpoints of the shorter line segment are swapped to unify their directions. Then, the included angle between the two vectors is calculated. If the included angle is less than a set threshold, the directional consistency condition is satisfied. Next, the spatial proximity between the line segments is evaluated using the perpendicular distance and projected distance between the endpoints. If any pair of endpoints simultaneously satisfies both the angle difference condition and the two distance threshold conditions, the two line segments are merged into a complete line segment, such as... Figure 2 As shown.

[0014] This patent proposes a geometric consistency verification method based on endpoint distance constraints, which aims to evaluate the rationality of the merging result. The method determines the straight line model in which the two endpoints of the merged new line segment are located, and calculates the distances from the two points in the two original line segments that were not used as endpoints to the straight line. If both distances are less than a preset threshold, the merged line segment is retained for subsequent feature tracking and matching; otherwise, it is determined to be an invalid merge and is removed.

[0015] Second, optimize the triangulation and spatial representation of line features.

[0016] Based on the stable line segments after geometric optimization, triangulation reconstruction is performed. The optimized and matched two-dimensional line segments are then reconstructed into three-dimensional spatial lines through epipolar geometric constraints, such as... Figure 3As shown, computational efficiency is improved by combining the two independent triangulations of the broken short line segment in the traditional method into a single triangulation of the complete long line segment. The spatial line features obtained from the triangulation are parameterized using a combination of Plücker coordinates and orthogonal normalized representation; Plücker coordinates are used for front-end initialization and coordinate transformation, while orthogonal normalized representation is used for back-end optimization to avoid over-parameterization and improve optimization efficiency. Plücker coordinates can be converted into a standard orthogonal representation (U, W) through QR decomposition, and the corresponding orthogonal basis matrix is ​​defined as follows:

[0017]

[0018] in d represents the rotation angle around the x-, y-, and z- axes in the camera coordinate system, where n is the direction vector and d is the normal vector. Plücker coordinates can be expressed by the following formula:

[0019]

[0020] To concisely represent the positional components, the matrix W is defined as follows:

[0021] After optimization, the corresponding spatial straight line The coordinates of P1ücker can be recovered using the following formula:

[0022]

[0023] Where u i It represents the i-th column vector of matrix U.

[0024] 3. Sliding window tightly coupled joint optimization

[0025] Construct a tightly coupled nonlinear optimization framework based on a sliding window, with the overall objective function... It includes IMU pre-integration residuals, point feature reprojection residuals, line feature reprojection residuals, and prior information; the line feature reprojection residual is the perpendicular distance between the two-dimensional line projected from the three-dimensional space line onto the image plane and the endpoints of the observed line segment; the objective function incorporates the Huber robust kernel function. ρ( By weighting each residual term and solving a nonlinear least squares problem, the maximum a posteriori estimate of the system state is achieved. The objective function is calculated as follows:

[0026]

[0027] In the formula, Let r be the system state vector within the sliding window. b For the IMU pre-integration residual, r p r lThese are the reprojection residuals for point features and line features, respectively. These are the corresponding observation sets. The corresponding measurement covariance matrix is ​​used; and the optimized system state is output by iteratively solving using the Ceres Solver library. Attached image description:

[0028] Figure 1 This is a system block diagram of this patent;

[0029] Figure 2 This refers to the extended form of the merged line segment;

[0030] Figure 3 This is the triangulation process for merging line segments;

[0031] Figure 4 A comparison of the line feature extraction method of this patent with that of PL-VINS;

[0032] Figure 5 This is a plot of positional error curves for this patent and PL-VINS under two sequences;

[0033] Figure 6 A comparison of the heatmaps of this patented method and PL-VINS under two sequences;

[0034] Figure 7 This is a comparison diagram of the estimated trajectory and the true value plane under two sequences of the method in this patent and the comparison method;

[0035] Figure 8 The time-domain error curves of the method in this patent and the comparison method are shown in two sequences.

[0036] Figure 9 The results of 3D mapping under two sequences. Detailed implementation method:

[0037] Example 1

[0038] 1. Experimental Setup

[0039] To verify the effectiveness and advancement of the proposed method, this embodiment uses the publicly available EuRoC MAV dataset to complete all comparative experiments. This dataset covers three typical environments: industrial plants, semi-structured office scenarios, and open outdoor areas. It fully encompasses challenging scenarios such as high-speed drone movement, lighting changes, weak textures, and sensor noise, enabling a comprehensive evaluation of the accuracy and robustness of the visual inertial odometry system. This research experiment was conducted on a portable computing platform running Ubuntu 20.04 and the ROS Noetic framework, with the following hardware configuration: Core TMIt features an i7-14650HX processor, 16GB of DDR5 memory, and an NVIDIA GeForce RTX 4060 Laptop GPU.

[0040] To ensure the fairness and reliability of the comparative experiments, all algorithms involved in the comparison were run with loop closure detection disabled. The pixel noise model, IMU noise parameters, camera intrinsic parameters, and extrinsic parameters between the camera and IMU used in the experiments were consistent with the benchmark method, and the remaining parameter settings strictly followed the default configuration of the open-source code.

[0041] Second, experimental results

[0042] Table I presents the RMSE comparison results of the proposed method with VINS-Mono and PL-VINS on all 11 sequences of EuRoC MAV. Experimental data shows that the proposed method achieves the best accuracy in 8 sequences, with a mean absolute pose error of 0.1642m, a reduction of 14.79% compared to VINS-Mono and 5.41% compared to PL-VINS. In the MH_04_difficult scenario with intense motion, the proposed method achieves an accuracy of 0.1990m, significantly outperforming the comparison methods. The experimental results verify that the proposed line feature geometry optimization strategy can effectively improve the pose estimation accuracy and robustness of the system in challenging environments. Figure 4 The results demonstrate that, in the highly dynamic sequences V1_03_difficult and V2_02_medium, the line features extracted by the method described in this patent outperform PL-VINS in terms of structural integrity and spatial distribution stability.

[0043] Table I. Comparison of RMSE (Relative Mean Square Error) of the Proposed Method and the Comparison Method on the EuRoC MAV Dataset (Unit: m)

[0044]

[0045] To analyze the performance of the algorithm in typical complex scenarios in depth, Figure 5 The curves showing the evolution of position errors of the patented method and PL-VINS over time in the MH_04_difficult and V2_03_difficult sequences are presented. As can be seen from the figure, the patented method maintains a lower error amplitude and a smaller fluctuation range in most time periods, indicating that it has better estimation stability and temporal consistency. This advantage is particularly prominent in the V2_03_difficult sequence.

[0046] Figure 6A comparison of APE heatmaps of the proposed method and PL-VINS in the V1_02_medium and V2_01_easy sequences is presented, with colors from blue to red representing the magnitude of the error. It can be seen that the proposed method produces lower errors in most areas of the trajectory, with a particularly significant advantage in regions with noticeable drift. Overall, the proposed method demonstrates a clear advantage in highly structured environments with rich line features, further validating the effectiveness of the proposed geometry enhancement strategy.

[0047] To evaluate the trajectory estimation performance of different methods from a global perspective, Figure 7 The paper presents a two-dimensional comparison of the estimated trajectories and true values ​​of each method in the MH_05_difficult and V2_03_difficult sequences. Figure 8 The temporal curves of the position estimation in the x, y, and z directions of the above sequence are further presented, intuitively reflecting the accuracy performance of each algorithm in different dimensions. The results show that the estimated trajectory of the method in this patent is highly consistent with the real trajectory in both overall trend and local details, and has the smallest error fluctuation in each coordinate direction. These results jointly verify the effectiveness of the proposed geometric enhancement strategy in improving the accuracy and consistency of trajectory estimation.

[0048] Table II presents the comparison results of the line feature processing time of the proposed method and PL-VINS on all 11 sequences of the EuRoC MAV dataset. Experimental data show that the proposed method exhibits superior efficiency on all sequences, with an average processing time of 12.87 ms, a reduction of 4.31% compared to PL-VINS. In the challenging scenario MH_04_difficult sequence, the efficiency improvement is even more significant, with a processing time reduction of 16.44%. Therefore, the proposed method can improve the overall efficiency of line feature processing in diverse scenarios, providing a reliable guarantee for the real-time operation of the system.

[0049] Table II. Comparison of online feature processing time between this patented method and PL-VINS (unit: ms)

[0050]

[0051] To systematically verify the spatial structure reconstruction capability of the algorithm in complex scenarios, the V1_03_difficult and MH_04_difficult scenarios in the EuRoC dataset were selected. These two scenarios have rich structural features. Figure 9 The mapping results of the proposed method on these sequences are shown. Visualization results demonstrate that the proposed method can effectively reconstruct the main structural outlines of indoor scenes. In areas with significant lighting variations or sparse textures, the proposed method can still extract key structural features and generate coherent and structurally complete maps.

Claims

1. A tightly coupled point-line visual inertial odometry method with optimized line feature geometry, comprising the following steps: First, front-end line feature geometry optimization processing First, redundant short line segments are filtered out using a length threshold, which is defined as follows: Where η is the length scaling factor, W I H I For the image width and height, For the rounding up operation; based on directional consistency and spatial proximity, the broken line segments are geometrically constrained and merged. Taking the longer line segment as the reference, the angle between the direction vectors of the two line segments and the perpendicular distance and projection distance between the endpoints are calculated. If the threshold condition is met, the two line segments are merged into a complete line segment, as shown in Figure 2. The two endpoints of the merged line segment are used to determine the straight line model. The distances from the two points in the two original line segments that were not used as endpoints before merging to the straight line are calculated respectively. If both distances are less than the preset threshold, the merging result is retained; otherwise, it is discarded. Second, optimize the triangulation and spatial representation of line features. The merged and matched two-dimensional line segments are reconstructed into three-dimensional spatial lines through epipolar geometric constraints, as shown in Figure 3. The features of the triangulated spatial lines are then parameterized using a combination of Plück coordinates and orthogonal normalization representation. Plück coordinates are used for front-end initialization and coordinate transformation, while orthogonal normalized representation is used for back-end optimization to avoid overparameterization; their representations are as follows: in Let be the Plück coordinates, n be the direction vector, and d be the normal vector. This is an orthogonal normalized representation. The conversion relationship between the two is:

3. Sliding window tightly coupled joint optimization Using a sliding window framework, an objective function is constructed that incorporates IMU pre-integration residuals, point feature reprojection residuals, line feature reprojection residuals, and prior information. The reprojection residual of the line feature is the perpendicular distance between the two-dimensional straight line projected from the three-dimensional line onto the image plane and the endpoints of the observed line segment. The objective function introduces the Huber robust kernel function to weight each residual term, and achieves the maximum a posteriori estimate of the system state by solving a nonlinear least squares problem. The formula for calculating the objective function is: In the formula, Let r be the system state vector within the sliding window. b For the IMU pre-integration residual, r p r l These are the reprojection residuals for point features and line features, respectively. These are the corresponding observation sets, ρ(·) is the Huber robust kernel function, and the optimized system state is output by iteratively solving using the Ceres Solver library.

Images