Dark light and structured environment-oriented point-line fusion visual inertial odometer method

Abstract

The invention belongs to the technical field of crossing of computer vision, automation and artificial intelligence, and particularly relates to a point-line fusion visual inertial odometer method for a dark light and structured environment, which adopts a lightweight neural network XFat to extract feature points and descriptors, and combines optical flow tracking and local area matching strategies to obtain a point-line fusion visual inertial odometer. And the real-time performance is ensured, and meanwhile, the feature tracking robustness in dark light and weak texture scenes is remarkably improved. In order to solve the problems that line feature expression is redundant and environment structuring priori is not fused in an existing point-line fusion scheme, a two-parameter minimization expression method is adopted to represent a structural line in a structured environment, line feature parameters are reduced from traditional four parameters to two parameters, and orthogonal constraint is implicitly fused. According to the design, the optimization difficulty is reduced, meanwhile, the geometric constraint of the structured lines is strengthened, and the positioning precision and the system robustness in the low-texture structured environment are remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the cross - field of computer vision, automation, and artificial intelligence, and specifically proposes a point - line fusion visual - inertial odometry method for low - light, low - texture, and structured environments. Background Art

[0002] Visual - inertial odometry (VIO) estimates the motion trajectory of a device in real - time by fusing data from visual sensors and inertial measurement units (IMUs), and is a core technology in fields such as robotics, drones, augmented reality, and autonomous driving. Traditional methods are based on a tightly - coupled optimization framework, which relies on manually - designed feature points to extract environmental features, and combines IMU pre - integration to construct a non - linear optimization problem to minimize the visual reprojection error and the IMU pre - integration error. Such methods have high accuracy in scenes with rich textures, but in weak - texture or low - light environments, they are prone to positioning failures due to lost or mismatched feature points. In addition, traditional feature descriptors are sensitive to view changes, and their robustness drops significantly.

[0003] Feature enhancement schemes based on deep learning extract robust features through convolutional neural networks, improving performance in weak - texture scenes. However, these schemes usually require multi - network cascades to achieve feature detection and matching. For example, SuperPoint is used for the extraction of feature points and descriptors, and LightGlue is used for the matching between feature points. This processing method has a high computational complexity and a large number of model parameters, making it difficult to meet the real - time requirements of embedded platforms. In addition, existing methods do not fully resolve the contradiction between feature - matching efficiency and computational resource consumption.

[0004] To improve the performance of traditional point - feature schemes in weak - texture environments, many schemes incorporating line features have been proposed in recent years. Point - line fusion schemes enhance positioning stability in structured scenes by introducing line features to construct joint constraints. However, in existing methods, line features are usually expressed with four parameters, resulting in redundant parameters, low optimization efficiency, and being prone to falling into local optima. At the same time, existing technologies do not fully utilize the structured priors in man - made environments, the constraint modeling is insufficient, and it is difficult to exert the geometric advantages of line features. There are still significant bottlenecks in existing visual - inertial odometry technologies: traditional hand - crafted features lack robustness in weak - texture scenes, while deep - learning methods are difficult to balance real - time performance and accuracy due to excessive computational overhead; line - feature modeling is redundant and does not combine with the structured laws of the environment, and the optimization efficiency and constraint strength are insufficient. These problems limit the practicality and reliability of visual - inertial odometry in resource - constrained platforms and weak - texture structured environments. Therefore, the present invention proposes a point - line fusion visual - inertial odometry method for low - light and structured environments. Summary of the Invention

[0005] The object of the present invention is to provide a point-line fusion visual inertial odometry method for low-light and structured environments, which is applicable to real-time autonomous positioning of devices such as drones in complex environments. The present invention aims to overcome the deficiencies of existing solutions and proposes a lightweight solution for the problems of large computational overhead of traditional multi-network cascading schemes based on deep learning and difficulty in running in real time on resource-constrained embedded devices. The present invention uses a lightweight neural network XFeat to extract feature points and descriptors, combines optical flow tracking and local region matching strategies, and significantly improves the robustness of feature tracking in low-light and weak-texture scenarios while ensuring real-time performance. Aiming at the problems of redundant line feature expression and lack of incorporation of environmental structure priors in existing point-line fusion schemes, the present invention uses a two-parameter minimization expression method to represent the structural lines in a structured environment, reduces the line feature parameters from traditional four parameters to two parameters, and implicitly incorporates orthogonal constraints. This design reduces the optimization difficulty while strengthening the geometric constraints of structured lines, and significantly improves the positioning accuracy and system robustness in low-texture structured environments.

[0006] The technical solutions adopted by the present invention are as follows:

[0007] It is applicable to resource-constrained computing platforms such as drones and unmanned vehicles. The system constructed by this method consists of two parts: front-end feature processing and back-end sliding window optimization, and mainly includes the following processing processes:

[0008] Extract and track-match point features and line features from the input image, and use two threads to process point features and line features simultaneously to improve processing efficiency.

[0009] In the point feature processing thread, the image is input into the lightweight feature extraction neural network XFeat, and a feature point heat map and a descriptor feature map are output.

[0010] For the feature points of the previous frame image, the bidirectional LK optical flow is used to predict their positions in the current frame to achieve preliminary tracking of feature points. For devices with GPUs, the LK optical flow uses the GPU version in the OpenCV library to make full use of GPU resources to improve real-time performance.

[0011] For the feature points that fail in optical flow tracking, they are used as target feature points for re-tracking. The feature point that is closest to the target feature point and has successful optical flow tracking is found. According to the two-dimensional optical flow vector of the successfully tracked point, the position of the target feature point in the current frame is predicted, and a square ROI with an adaptive width is taken at the predicted position. The width depends on the length of the optical flow vector. According to the feature point heat map and descriptor feature map output by XFeat previously, feature points and corresponding descriptors are extracted in the ROI to form candidate matching feature points for the target feature point, and the point with the largest cosine distance from the target feature point descriptor among the candidate feature points is selected as the final matching point, so as to achieve re-tracking of feature points.

[0012] After the LK optical flow tracking and re-tracking processes, if the number of successfully tracked frame feature points is less than a certain threshold, a certain number of feature points are extracted from the feature point heat map, and after non-maximum suppression processing, they are supplemented into the current frame feature points to ensure the number and uniformity of feature points in the image.

[0013] In the online feature processing thread, the LSD algorithm is used to extract line features in the image, and the LBD algorithm is used to extract descriptors of line features. For the line features of the previous frame, a multi-index based nearest neighbor search is used to implement the matching of line features between two frames. For unassociated line features, elements with a length less than a certain threshold are filtered out, and redundant line features are filtered out using non-maximum suppression based on angle criterion and point-line distance criterion, and the remaining ones are added to the current frame feature set as new line features.

[0014] Undistort the point features and the endpoints of line features, calculate their moving speeds in the image plane, and send them to the backend for subsequent processing after sorting.

[0015] When the backend receives the current frame point-line features, first add the point features to the feature point manager, calculate the feature point disparity between two frames to determine whether it is a key frame, and then perform triangulation on the point features to obtain the initial value of the inverse depth of the feature points.

[0016] This method models line features as mutually orthogonal structural line features, which are divided into vertical lines and horizontal lines. The vertical lines are parallel to the gravity direction, and the horizontal lines have two mutually orthogonal main directions, and the main directions depend on the current structural environment.

[0017] Based on the assumption of the Atlanta world, the space is divided into different local Manhattan worlds (LMHWs). Each LMHW is the result of rotating the initial Manhattan world by different angles around the gravity direction. Define the LMHW deflection angle as the angle between the X-axis of the LMHW coordinate system and the X-axis of the world coordinate system.

[0018] Vertical lines are always parallel to the gravity direction. Considering that the gravity direction is almost unchanged within the working range of the odometer, vertical lines do not participate in the LMHW assumption.

[0019] Since the backend adopts a sliding window optimization strategy, only key frames with a fixed window length are retained, and the window length is fixed to a small value. Therefore, all key frames within the sliding window are regarded as being in the same LMHW.

[0020] The processing and optimization of structural line features are divided into normal mode and LMHW update mode.

[0021] In the system initialization stage, only point features are used to initialize the odometer to ensure the accuracy and stability of the initialization, and the structural line feature processing is in the LMHW update mode.

[0022] In the normal mode, for the currently received frame line features, only the existing line features are added to the line feature manager, the new line features are separated, and the structural line features that have not been initialized in the manager are initialized to obtain the initial values of their two-parameter expressions.

[0023] There are two ways to triangulate the structural line features. The first way uses the plane intersection method based on dual Plücker coordinates. However, it is found in practice that the success probability of completing the initialization through this method is relatively low. When the first initialization method fails, the present invention adopts a structural line feature initialization scheme based on point-line association, which associates the feature points with the structural line features based on the point-line distance, and selects the optimal feature points from all the associated feature points to calculate the initial two-parameter expression of the structural line features.

[0024] Construct constraint terms to perform non-linear optimization on all state variables within the sliding window. The constraint terms include point feature visual constraint terms, structural line feature visual constraint terms, IMU pre-integration constraint terms, and marginalization prior constraint terms. The optimization variables include the poses, positions, velocities, IMU biases, camera-IMU synchronization time differences, extrinsic parameters, inverse depths of point features, two-parameter coordinates of structural line features, and local Manhattan world yaw angles of all key frames.

[0025] After optimization, according to the optimized current frame pose, gravity vector, and LMHW yaw angle, the newly added structural line features are divided into vertical line segments and horizontal line segments, and added to the structural line feature manager. If the number of lines that have not been successfully divided is greater than a certain proportion, it is determined that a new LMHW has been entered, and the line feature processing enters the LMHW update mode.

[0026] In the LMHW update mode, only the vertical line segments are triangulated in the structural line feature triangulation stage, and only the vertical line segments are added to the structural line feature visual constraint term of the sliding window optimization. All frames that enter the sliding window in the LMHW update mode are called LMHW update frames.

[0027] After optimization, the vertical line segments are first divided to separate the current frame horizontal line segments, including the already divided horizontal line segments and the newly undivided horizontal line segments.

[0028] Based on the current frame horizontal line segments, the LMHW yaw angle of the current frame is calculated using the 1-Line RANSAC algorithm. The newly successfully divided horizontal line segments are added to the manager, and the identified LMHW yaw angle is added to the LMHW manager.

[0029] The LMHW manager maintains the LMHW deflection angle of all LMHW update frames in the form of a sliding window. When the maintained sliding window is filled with LMHW update frames, calculate the difference in LMHW deflection angles between each frame. If the average difference is less than a certain threshold, it is determined that the entire sliding window has completely entered the new LMHW. Take the average of all LMHW deflection angles maintained in the manager as the initial value of the latest LMHW deflection angle, exit the update mode and enter the normal mode. Otherwise, continue to update the LMHW until the fusion condition is met.

[0030] After completing the above process, perform the sliding window operation. According to whether the second-newest frame is determined to be a key frame, marginalize the oldest frame or the second-newest frame to form a marginalization prior constraint factor to be added to the next round of optimization.

[0031] The technical effects achieved by the present invention are as follows:

[0032] In the present invention, the lightweight front-end design ensures the real-time performance of the deep learning-based method on resource-constrained platforms. A feature point re-tracking method combining optical flow and local feature matching is designed, which significantly improves the accuracy and stability of front-end feature point tracking. The structure characteristics of line features are modeled by reasonably applying the Atlanta world hypothesis, effectively identifying the structural line features in the environment, and fully coupling with the system architecture based on sliding window optimization, which can update the LMHW in a timely manner when the structural characteristics of the environment change. A null space correction method for structural line feature parameters is proposed, which effectively eliminates the drift of structural line parameters caused by unobservable degrees of freedom.

[0033] In summary, the present invention significantly improves the positioning accuracy and system robustness of the visual inertial odometer in low-light, weak-texture, and structured environments, solves the performance bottleneck of traditional solutions in resource-constrained platforms and complex scenarios, and provides an efficient and reliable positioning solution for embedded autonomous navigation systems. Brief Description of the Drawings

[0034] Figure 1 It is a block diagram of the point-line fusion visual inertial odometer method proposed by the present invention for complex structured environments;

[0035] Figure 2 It is the architecture of the lightweight feature point extraction neural network XFeat model adopted in the present invention;

[0036] Figure 3 It is a flowchart of the feature point extraction, optical flow, and fast feature point re-tracking method based on local area matching in the front-end system of the present invention;

[0037] Figure 4 It is a schematic diagram of the Atlanta world hypothesis adopted by the present invention to model the structural line features;

[0038] Figure 5Schematic diagram of the two-parameter expression method of the structure line adopted by the present invention;

[0039] Figure 6 Comparison of the running effects of the present invention and PL-VINS on the EuRoC dataset;

[0040] Figure 7 Comparison chart of the trajectories tested by the method proposed by the present invention and VINS-Fusion, PL-VINS on the EuRoC dataset;

[0041] Figure 8 Comparison chart of the trajectory evaluation indexes of the method proposed by the present invention and VINS-Fusion, PL-VINS on the EuRoC dataset. Detailed implementation manners

[0042] In order to make the purpose and advantages of the present invention clearer, the present invention will be specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific implementation manners of the present invention, and does not strictly limit the scope of protection specifically requested by the present invention.

[0043] As Figures 1 - 8 shown, a point-line fusion visual inertial odometry method for low-light and structured environments, and the point-line fusion visual inertial odometry method for complex structured environments consists of two subsystems: a front end and a back end. The front end is mainly responsible for image processing related tasks, which are divided into point feature processing and line feature processing.

[0044] In the actual use of the present invention, the point feature processing uses the lightweight convolutional neural network XFeat to extract feature points and descriptors. Its architecture is as Figure 2 shown. XFeat adopts a lightweight network backbone design. By reducing the number of channels in the early convolutional layers and adopting a multi-resolution feature fusion strategy, the computational burden is reduced, and an independent key point detection branch is adopted to avoid the computational overhead caused by simultaneously performing key point detection during descriptor extraction. Therefore, XFeat shows excellent real-time performance on resource-constrained hardware platforms, and while maintaining high accuracy, it has achieved a significant improvement in robustness compared with traditional hand-designed solutions, showing obvious superiority.

[0045] In actual use of the present invention, the traditional LK optical flow is adopted for the tracking and matching of feature points to ensure real-time performance. In order to make full use of the descriptors extracted by XFeat, a feature point re-tracking algorithm based on local region matching is designed, which makes up for the deficiency of the feature point tracking performance of LK optical flow in some scenarios. For the processing of line features, an improved LSD algorithm is used to extract line features, and the LBD algorithm is used to extract line feature descriptors. The matching method adopts the nearest neighbor search based on multi-index. Aiming at the problem that the line features extracted by LSD are dense and disorderly, non-maximum suppression based on angle criterion and point-line distance criterion is used to filter out redundant line features. The line features processed in the front end do not consider their structural characteristics, and the back end performs structural modeling on the line features.

[0046] The back end receives the point features, line features and IMU data from the front end, constructs relevant constraint terms and uses a non-linear optimization algorithm to optimize all state variables in the sliding window. At the same time, it also performs structural processing on the line features, including structural division, parameter initialization, LMHW update, and null space correction.

[0047] In actual use of the present invention, the structural characteristics of line features are modeled based on the assumption of the Atlanta world. The structural line features are uniquely determined by their local coordinate system parameters, starting frame positions, and the local Manhattan world to which they belong. The processing process is divided into the normal mode and the LMHW update mode. In the normal mode, based on the determined LMHW, the newly input line features are structurally divided, the starting frame of the structural line is established, and a local coordinate system is established based on its structural attributes on the basis of the starting frame. Combining the Plücker representation of the line features and the necessary pose transformation, the two-parameter initialization of the structural line features is realized. The residual term of the structural line features is constructed based on the point-line distance and added to the sliding window optimization together with other residual terms. Based on the success ratio of the structural line division in the normal mode, it is determined whether the sliding window enters another LMHW; if so, it enters the LMHW update mode.

[0048] In the LMHW update mode, the horizontal lines are unavailable, and only the vertical lines that do not depend on the LMHW for processing and optimization are processed. The 1Line-RANSAC is used to identify the LMHW, and the LMHW sliding window manager is constructed. When the conditions for LMHW fusion are met, the LMHW of the current sliding window is updated and enters the normal mode.

[0049] The implementation steps of the front end and the back end are described in detail below.

[0050] The front end reads an image and starts two parallel threads to perform point feature processing and line feature processing respectively.

[0051] The present invention specifically includes the following steps:

[0052] Step 1: Extract and track-match point features and line features from the input image, and adopt a dual-thread parallel processing for point features and line features;

[0053] Step 2: Extract the heat map of image feature points and descriptors through the lightweight convolutional neural network XFeat, and adopt a bidirectional optical flow tracking and local region matching strategy to achieve re-tracking of feature points;

[0054] The processes of feature extraction, optical flow tracking and re-tracking in point feature processing are as Figure 3 shown. First, input the image into the XFeat network to output the heat map of feature points and the descriptor feature map. Among them, the heat map of feature points is a two-dimensional matrix, and the value of the element in the matrix represents the probability value that the position where the element is located is a key point. The descriptor feature map is a three-dimensional matrix with 64 channels. Sampling according to the feature point position in the descriptor feature map can obtain the descriptor of the feature point.

[0055] In step 2, the steps of feature point extraction, optical flow tracking and re-tracking based on XFeat specifically include:

[0056] Step 201: Input the image into XFeat to output the heat map of key points and the descriptor feature map;

[0057] Step 202: Perform bidirectional optical flow tracking on the feature points of the previous frame, and perform re-tracking operations on the points where the tracking fails;

[0058] Step 203: Traverse the successfully tracked feature points, select the feature point closest to the target point as the auxiliary point. If the distance exceeds the preset threshold, it is determined that the re-tracking fails;

[0059] Step 204: Calculate the optical flow movement vector of the auxiliary point and apply it to the target point to obtain the predicted point;

[0060] Step 205: Let the length of the movement vector be l. With the predicted point as the center, set a square ROI (Region of Interest) with a width of . Extract the feature points and descriptors in the ROI according to the XFeat output to obtain the candidate matching feature points of the target point;

[0061] Step 206: Calculate the similarity between the target point and the candidate points based on the cosine distance, and select the candidate point with the highest similarity as the final matching point to complete the re-tracking;

[0062] Step 207: Perform bilinear interpolation on the descriptor feature map to update the descriptors of all feature points in the current frame for use in the re-tracking of feature points in the next frame.

[0063] Step 3: Extract line features based on the LSD algorithm, and use a non-maximum suppression method based on angle criterion and point-line distance criterion to filter out redundant line features;

[0064] In the said Step 3, the non-maximum suppression steps based on angle criterion and point-line distance criterion specifically include:

[0065] Step 301: Define the distance calculation method between two line segments in the image, including translation distance and angle distance, where the translation distance is defined as the distance between the average endpoints of the two line segments, and the angle distance is defined as the size of the included angle formed after the two line segments are extended and intersect; if the translation distance or angle distance between two line segments is less than a preset threshold, they are determined to be similar line segments;

[0066] Step 302: According to the LBD matching result, divide the line features of the current frame into successfully matched line features and new line features;

[0067] Step 303: Traverse the new line features, and eliminate the new line features that are too close to any successfully matched line feature according to the defined distance;

[0068] Step 304: Sort the remaining new line features by line segment length and add them to a queue, with the longest line feature at the head of the queue;

[0069] Step 305: Take out the line features at the head of the queue and add them to the result set in turn, and traverse the line features in the queue to eliminate the elements that are too close to any line feature in the result set;

[0070] Step 306: Repeat the above steps until the queue is empty, and the obtained result set is the newly added line features after redundant elimination. The division of structural lines depends on state variables such as camera pose and local Manhattan. This method performs the processing related to structural lines at the back end and after the optimization of the current frame to improve the accuracy of the processing related to structural lines.

[0071] Step 4: Based on the assumption of the Atlanta world, divide the environment into multiple local Manhattan worlds (LocalManhattan Hypothesis World, LMHW), and model the line features as a two-parameter polar coordinate model;

[0072] In the said Step 4, the structural line feature model based on the assumption of the Atlanta world is specifically:

[0073] Step 401: The structural line features are uniquely determined by their structural types, local parameter plane coordinates, starting frame positions, and the local Manhattan world (LMHW) they belong to;

[0074] Step 402: Based on the assumption of the Atlanta world, the space is divided into different LMHWs. Each LMHW is the result of rotating the initial Manhattan world by different angles around the gravity direction. Define the deflection angle of the LMHW as the angle between the X-axis of the LMHW coordinate system and the X-axis of the world coordinate system.

[0075] Step 403: Define the starting frame S of the structural line feature. The origin of its coordinate system is the origin of the camera frame coordinate system where the structural line feature is first observed. The Z-axis is aligned with the gravity, and the X-axis and Y-axis are aligned with the X and Y axes of the LMHW.

[0076] Step 404: Define the local parameter coordinate system L. The structural line feature is defined as a straight line perpendicular to the XY plane in the local parameter coordinate system. The intersection point of the line and the XY plane is M, with coordinates. Convert it to polar coordinates, define the inverse depth of the structural line, and uniquely represent a structural line with two-parameter polar coordinates. For different types of structural lines, their local parameter coordinate systems can be transformed to the starting frame coordinate system through a rotation matrix.

[0077] Step 5: Initialize the parameters of the structural line feature using the plane intersection method based on the dual Plücker matrix.

[0078] In the above Step 5, the specific steps of the structural line parameter initialization process include:

[0079] Step 501: For the line features observed for more than 4 frames, calculate the parametric equations of the planes formed by connecting the optical centers at both ends of the line in each observation frame, and find the two planes with the maximum cosine distance of the normal vectors.

[0080] Step 502: Calculate the dual Plücker matrix according to the parametric equations of the two planes, and use the dual Plücker matrix to calculate the Plücker coordinates of the structural line feature in the world coordinate system.

[0081] Step 503: According to the pose of the starting frame and the type of the structural line, transform the Plücker coordinates of the structural line from the world coordinate system to the local parameter coordinate system.

[0082] Step 504: Calculate the intersection point with the local parameter plane, and use the intersection point as the initial parameters of the structural line.

[0083] Step 6: Construct a dynamic update mechanism for the LMHW to adjust the local Manhattan constraint in real time to adapt to environmental structure changes.

[0084] In the above Step 6, the specific steps of the LMHW dynamic update mechanism include:

[0085] Step 601: Divide the structural line processing flow into a normal mode and an LMHW update mode.

[0086] Step 602: In the normal mode, based on the principal direction vanishing point matching principle, structurally divide the line features output by the front end according to the current camera frame pose and LMHW;

[0087] Step 603: If the proportion of successfully divided structural line features is lower than the preset threshold, it is determined that most of the line features in the current environment no longer satisfy the principal direction of LMHW, and enter the LMHW update mode;

[0088] Step 604: In the LMHW update mode, perform a vertical line division on the line features output by the front end that is independent of LMHW, and separate the horizontal lines to be divided; use the 1-Line RANSAC algorithm to identify the LMHW of the current frame according to the horizontal lines to be divided;

[0089] Step 605: Construct an LMHW sliding window manager to perform a synchronous sliding window operation on the LMHW recognition result, and detect whether the root mean square of the adjacent errors of the LMHW within the sliding window is lower than the preset threshold; if so, it is determined that the LMHW fusion condition is satisfied, take the average of all LMHWs within the sliding window as the updated result of LMHW, exit the update mode and enter the normal mode, and add LMHW as a state variable to the sliding window optimization.

[0090] Step 7: Construct a sliding window optimization framework, fuse the reprojection errors of point and line features, the IMU pre-integration error, and the marginalization prior error, and solve the camera pose through non-linear optimization to output the optimized camera trajectory and structured map;

[0091] Step 8: Perform null space correction on the parameters of the structural line features to eliminate the drift caused by the unobservable degrees of freedom.

[0092] In the above Step 8, the null space correction method for the structural line features is as follows:

[0093] Let the optimized parameter coordinates of the structural line features be The LMHW parameter is θ M , and the change amount of the first frame pose in the yaw direction after optimization in the sliding window is δθ Z ;

[0094] For vertical line features, only θ in the local parameters drifts in the null space, and the drift amount is equal to the yaw drift amount of the first frame in the sliding window. The correction method for vertical line features is defined as:

[0095]

[0096] For horizontal line features, their parameter drift is equivalent to the LMHW drift, and the drift amount is equal to the yaw drift amount of the first frame in the sliding window. The correction method for horizontal line features is defined as:

[0097] θ` M = θM -δθ Z 。

[0098] In the actual use of the present invention, based on the Atlanta world hypothesis, the environment where the odometer is located is divided into several local Manhattan worlds (LMHWs). As Figure 4 shown in the schematic diagram of the Atlanta world hypothesis, W is the world coordinate system, and M1 and M2 are two LMHWs. Assume that the Z-axis of the world coordinate system is perpendicular to the ground, W is the world coordinate system, the origin of the LMHW coordinate system coincides with the world system, the Z-axis is also perpendicular to the ground, and the deflection angle of the LMHW is defined as the angle between the X-axis of the LMHW and the X-axis of the world system, with a range of 0-90°. The LMHW is uniquely determined by its deflection angle.

[0099] In an LMHW, there are three main orthogonal directions of x, y, and z. Among them, the z main direction is called the vertical direction, and x and y are called the horizontal directions. The structural line features are divided according to the three main directions of the LMHW. The following explains the two-parameter expression method of the structural lines adopted by the present invention.

[0100] Define the starting frame S of the structural line feature. The origin of the starting frame coordinate system is the origin of the first frame camera frame coordinate system where the structural line feature is observed. The Z-axis is aligned with gravity, and the X-axis and Y-axis are aligned with the X-axis and Y-axis of the LMHW.

[0101] As Figure 5 shown, define the local parameter coordinate system L. The structural line feature is defined as a straight line perpendicular to the XY plane in the local parameter coordinate system, and its intersection point with the XY plane is M, with coordinates (a, b). Convert it to polar coordinates (ρ, θ), and define the inverse depth of the structural line which can be uniquely represented by two parameters for a structural line.

[0102] For different types of structural lines, their local parameter coordinate systems can be transformed to the starting frame coordinate system through a rotation matrix For the structural line in the x direction, For the structural line in the y direction, For the structural line in the z direction,

[0103] It should be noted that the vertical line is always parallel to the gravity direction. Therefore, its parameters are not restricted by the LMHW. The definition of the starting frame coordinate system for the vertical line is: the origin of the coordinate system is the origin of the first frame camera frame coordinate system where it is observed, and the X-axis and Y-axis are aligned with the X-axis and Y-axis of the world system.

[0104] To facilitate the transformation of the structural line features between coordinate systems, the Plücker coordinates are also used to represent the straight line in the local parameter coordinate system: l P=(b, -a, 0, 0, 0, 1).

[0105] The present invention divides the structural line processing flow into a normal mode and an LMHW update mode, enabling the method to efficiently and accurately update the LMHW without loss of accuracy when the LMHW changes.

[0106] In the normal mode, the line features output by the front end are structurally divided according to the current camera frame pose and LMHW. The specific division steps are as follows:

[0107] Obtain the unit vector of the main direction of the current LMHW:

[0108] v x =[cosθ, sinθ, 0]

[0109] v y =[-sinθ, cosθ, 0]

[0110] v z =[0, 0, 1]

[0111] Convert the unit vector of the main direction to the camera system according to the camera frame pose:

[0112]

[0113] According to D x 、D y 、D z Calculate the vanishing point of the main direction in the image in the normalized coordinate system:

[0114]

[0115] For each line feature l1, calculate its membership degree to each vanishing point. The calculation process is as follows: Connect the midpoint of the line feature to the vanishing point to obtain l2, calculate the average distance from the two endpoints of l1 to l2 as the point-line distance, and calculate the angular distance between l1 and l2 as the angular distance. If the point-line distance and the angular distance are respectively less than the preset threshold, then normalize the point-line distance and the angular distance according to the threshold and take the average as the membership degree of the line feature to the vanishing point. If either the point-line distance or the angular distance does not meet the threshold condition, it is determined that the line feature does not belong to the vanishing point.

[0116] Divide the line features according to the vanishing point with the highest membership degree. For example, if l1 has the highest membership degree to v px , then it is divided into a structural line in the horizontal x direction. If it does not belong to any vanishing point, the division of the line feature fails.

[0117] If the proportion of successfully divided structural line features is less than a certain threshold, it indicates that most of the line features in the current environment no longer satisfy the main direction of LMHW, which is the horizontal direction. The present invention designs an LMHW update mode to re-identify LMHW.

[0118] In the LMHW update mode, vertical line division is performed on the line features output by the front end without relying on LMHW to separate the horizontal lines to be divided. The 1-Line RANSAC algorithm is used to identify the LMHW of the current frame based on the horizontal lines to be divided. The steps are as follows:

[0119] According to the pose of the current camera frame, calculate the expression of the vanishing line of the horizontal ground in the image normalized coordinate system:

[0120]

[0121] Randomly select a line from the input line features and calculate its intersection point v with the vanishing line px , and use it as the vanishing point in the x direction.

[0122] Connect the optical center to v px , and normalize it to obtain the unit vector D in the main x direction x , and convert it to the main direction v in the world coordinate system according to the pose of the current frame x .

[0123] Normalize v x , and limit it within the range of 0 to 90° to ensure the consistency of each main direction recognition. The normalization method is as follows: if v x is in the second quadrant of the world coordinate system, rotate v x clockwise by 90°; if v x is in the third quadrant of the world coordinate system, take the opposite of v x ; if v x is in the fourth quadrant of the world coordinate system, rotate v x counterclockwise by 90°.

[0124] According to the normalized v x and the gravity vector, obtain v y =v z ×v x , and according to the pose of the current frame convert the main direction v x , v y to the camera coordinate system, and according to the structural line division method described above, perform horizontal line division on the line features to be divided, and count the proportion of successful divisions.

[0125] Repeat the above steps a specified number of times, select the one with the highest proportion of successful horizontal line divisions, and use the angle between it and the X axis of the world coordinate system as the LMHW recognition result.

[0126] In the LMHW update mode, LHMT recognition is performed on each input frame, and a LMHW sliding window manager is used to perform synchronous sliding window operations on the LMHW recognition results. It is detected whether the root mean square of the adjacent errors of the LMHW deflection angles within the sliding window is less than a certain threshold. If so, it is determined that the LMHW fusion condition is satisfied, and the average of all LMHW deflection angles within the sliding window is taken as the update result of the LMHW, and the update mode is exited to enter the normal mode. The LMHW deflection angle will be added to the sliding window optimization as a state variable.

[0127] After dividing the structural line features, it is necessary to initialize their parameters, and the steps are as follows:

[0128] For the line features observed for more than 4 frames, calculate the plane parametric equation π = [x, y, z, w] formed by connecting the optical center at both ends of the line in each observation frame. Let the plane formed by the first frame be π1, and find the plane with the largest cosine distance between the normal vectors in the remaining planes, denoted as π2.

[0129] Calculate the dual Plücker matrix according to π1 and π2:

[0130]

[0131] where [m] × represents taking the skew-symmetric matrix of m.

[0132] According to the dual Plücker matrix, extract the corresponding part to obtain the Plücker coordinates of the line feature in the world coordinate system

[0133] l w =(n w ,v w ).

[0134] Define the transformation matrix from the world coordinate system w to the starting frame coordinate system s

[0135] Transform l w to the starting frame coordinate system s:

[0136] According to the type of the structural line feature, obtain the transformation matrix from the starting frame coordinate system s to the local parameter coordinate system Transform l s to the local parameter coordinate system:

[0137]

[0138] Calculate the intersection point m of l l with the xy plane.

[0139] From the intersection point m = (a, b), obtain the initial parameters of the structural line feature:

[0140]

[0141] The reprojection residual term of the structural line feature is constructed by the point-line distance. Given the two-parameter expression of the structural line, it is converted into Plücker coordinates. According to the state variables such as the body pose, external parameters, and LMHW, the expression of the result line l in the camera system is obtained. c =(n c , v c ).

[0142] From l c , the parametric equation of the straight line of the structural line projection in the image normalized coordinate system l n =n c =(l1, l2, l3).

[0143] The reprojection error of the line feature is defined as the distance from the endpoints of the observed line segment in the image to the predicted line of the reprojection regression normalization plane from space:

[0144]

[0145] where p1 and p2 are the two endpoints of the observed line segment in the image.

[0146] After steps such as structural line division, parameter initialization, LMHW update, and point feature processing, all the reprojection residual terms of the structural line feature, the reprojection residual terms of the point feature, the IMU pre-integration residual terms, and the marginalization prior residual terms in the sliding window are constructed. A non-linear optimization algorithm is used to minimize the sum of the residual terms to optimize all the state variables in the sliding window.

[0147] Since the visual-inertial odometry method proposed by the present invention has no external observation of the absolute pose, the estimated pose will have the problem of non-observability of four degrees of freedom (yaw angle, x, y, z). The method based on sliding window optimization usually adopts the method of fixing the first frame in the window to prevent the state variables from drifting into the null space during the optimization process. The method adopted by the present invention is similar to it. After optimization, the change amounts of the first frame in the yaw angle, x, y, z are calculated, and all the state variables in the sliding window are corrected according to these change amounts on the premise of ensuring that the overall residual remains unchanged, so that the unobservable state variables of the first frame remain unchanged before and after optimization.

[0148] The modeling method of the structural line feature in the present invention determines that it will drift in the null space during the optimization process. To solve this problem, the present invention proposes a null space correction method after the optimization of the structural line feature according to experimental phenomena.

[0149] Suppose the parameter coordinates of the structural line feature after optimization are The LMHW parameter is θ M , and the change amount of the first frame pose in the yaw direction after optimization in the sliding window is δθZ ;

[0150] For the vertical line feature, only θ in the local parameters drifts in the null space, and the drift amount is equal to the yaw drift amount of the first frame of the sliding window. The correction method for the vertical line feature is defined as:

[0151]

[0152] For the horizontal line feature, the drift of its parameters in the null space is mainly reflected in the drift of LMHW, and the drift amount is equal to the yaw drift amount of the first frame of the sliding window, while the parameter coordinates thereof do not drift. Therefore, the correction method for the horizontal line feature is to correct only LMHW, and the corrected LMHW is:

[0153] θ` M = θ M - δθ Z .

[0154] The method VINS-PSL proposed by the present invention and other methods are compared and tested using the EuRoC dataset. As Figure 6 shown is the test effect of VINS-PSL and PL-VINS in the test case MH-05-difficult. It can be seen that PL-VINS does not consider the structural constraints between line features, and the line features in the constructed map are relatively messy, with a large difference from the actual environmental features; while VINS-PSL proposed by the present invention effectively identifies the structural characteristics of the environment, and obvious orthogonal characteristics can be found between the line features in the constructed map, which is consistent with the actual environment.

[0155] As Figure 7 shown is the comparison chart of the output trajectory and the ground truth of VINS-PSL, VINS-Fusion, and PL-VINS in the test case MH-05-difficult. The above curve graph reflects the change of the absolute pose error over time. It can be seen that in the entire test case, the maximum absolute pose error of VINS-PSL does not exceed 0.35 m, and the average value is about 0.17 m, while the absolute pose errors of PL-VINS and VINS-Fusion are close to or even exceed 0.45 m, and the average value is also above 0.25 m. By comparison, VINS-PSL has higher accuracy.

[0156] As Figure 8The test results of VINS-PSL, VINS-Fusion, and PL-VINS in more test cases are shown. The measured precision metric is the root mean square error of the absolute pose error. Among them, the scenarios of the test cases starting with MH are relatively wide indoor scenes with obvious structured features and satisfying the Atlanis world assumption. The test cases starting with V are small-scale indoor scenes with intense motion and rapid field-of-view changes. There are cases with missing texture features and low light in some cases. From the data comparison, the metrics of VINS-PSL are better than those of VINS-Fusion and PL-VINS, indicating that VINS-PSL can effectively utilize the structural characteristics in the environment to improve the odometer accuracy. With the support of a lightweight feature point extraction network and a re-tracking algorithm, it can effectively adapt to scenes with low light, weak texture, and rapid view changes, making it more robust and practical than other methods.

[0157] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. The structures, devices, and operation methods not specifically described and explained in the present invention are implemented according to the conventional means in the art without special explanation and limitation.

Claims

1. A point-line fusion visual-inertial odometry method for dark light and structured environments, characterized by: The following steps are involved: Step 1: Extract, track and match point features and line features of the input image, and use dual threads to process point features and line features in parallel; Step 2: Extract image feature point heatmap and descriptor through lightweight convolutional neural network XFeat, and use bidirectional optical flow tracking and local area matching strategy to achieve feature point re-tracking; Step 3: Extract line features based on the LSD algorithm, and use the non-maximum suppression method based on angle criterion and point-line distance criterion to filter out redundant line features; Step 4: Based on the Atlanta world hypothesis, the environment is divided into multiple local Manhattan worlds, and the line features are modeled as a two-parameter polar coordinate model; Step 5: Initialize the parameters of the structural line features using the plane intersection method based on the dual Plücker matrix; Step 6: Construct a dynamic update mechanism for LMHW to adjust the local Manhattan constraints in real time to adapt to changes in environmental structure; Step 7: Build a sliding window optimization framework, integrate the point and line feature reprojection error, IMU pre-integration error and marginalization prior error, solve the camera pose through nonlinear optimization, and output the optimized camera trajectory and structured map; Step 8: Perform zero-space correction on the parameters of the structural line features to eliminate the drift caused by the unobservable degrees of freedom.

2. The point-line fusion visual inertial odometer method for dark light and structured environment according to claim 1, characterized in that: In step 2, the steps of feature point extraction, optical flow tracking and re-tracking based on XFeat specifically include: Step 201: Input the image to XFeat and output the key point heat map and descriptor feature map; Step 202: performing bidirectional optical flow tracking on the feature points of the previous frame, and performing a re-tracking operation on the points that failed to be tracked; Step 203: Traverse the successfully tracked feature points, select the feature point closest to the target point as the auxiliary point, and if the distance exceeds a preset threshold, determine that the re-tracking fails; Step 204: Calculate the optical flow motion vector of the auxiliary point and apply it to the target point to obtain a predicted point; Step 205: Set the length of the moving vector to 11, take the prediction point as the center, and set the width to The square ROI is extracted, and the feature points and descriptors in the ROI are extracted according to the XFeat output to obtain the candidate matching feature points of the target point; Step 206: Calculate the similarity between the target point and the candidate point based on the cosine distance, select the candidate point with the highest similarity as the final matching point, and complete the re-tracking; Step 207: Perform bilinear interpolation on the descriptor feature map to update the descriptors of all feature points in the current frame in preparation for re-tracking the feature points in the next frame.

3. The point-line fusion visual inertial odometer method for dark light and structured environment according to claim 2, characterized in that: In step 3, the non-maximum suppression step based on the angle criterion and the point-line distance criterion specifically includes: Step 301: defining a distance calculation method between two line segments in an image, including translation distance and angle distance, wherein the translation distance is defined as the distance between the average endpoints of the two line segments, and the angle distance is defined as the angle formed by the two line segments being extended and intersected; if the translation distance or angle distance of the two line segments is less than a preset threshold, they are determined to be similar line segments; Step 302: according to the LBD matching result, the line features of the current frame are divided into successfully matched line features and new line features; Step 303: traverse the new line features, and remove the new line features that are too close to any successfully matched line features according to the defined distance; Step 304: sort the remaining new line features by line segment length and add them to a queue, with the longest line feature at the head of the queue; Step 305: sequentially taking out the line features at the head of the queue and adding them to the result set, and traversing the line features in the queue to remove the elements that are too close to any line feature in the result set; Step 306: Repeat the above steps until the queue is empty, and the result set obtained is the newly added line features after redundant elimination.

4. The point-line fusion visual inertial odometer method for dark light and structured environment according to claim 3, characterized in that: In step 4, the structural line feature model based on the Atlanta world hypothesis is specifically: Step 401: The structural line feature is uniquely determined by its structural type, local parameter plane coordinates, starting frame position and the local Manhattan world to which it belongs; Step 402: Based on the Atlanta world hypothesis, the space is divided into different LMHWs, each LMHW is the result of the initial Manhattan world rotating around the gravity direction at different angles, and the LMHW deflection angle is defined as the angle between the X-axis of the LMHW coordinate system and the X-axis of the world system; Step 403: define the starting frame S of the structure line feature, the origin of its coordinate system is the origin of the camera frame coordinate system where the structure line feature is first observed, the Z axis is aligned with gravity, and the X axis and Y axis are aligned with the X axis and Y axis of LMHW; Step 404: define the local parameter coordinate system L, the structural line feature is defined as a straight line perpendicular to the XY plane in the local parameter coordinate system, its intersection with the XY plane is M, the coordinates are, convert it into polar coordinates, define the inverse depth of the structural line, and use two-parameter polar coordinates to uniquely represent a structural line; for different types of structural lines, their local parameter coordinate systems can be converted to the starting frame coordinate system through a rotation matrix.

5. The point-line fusion visual inertial odometer method for dark light and structured environment according to claim 4, characterized in that: In step 5, the specific steps of the structure line parameter initialization process include: Step 501: For line features observed in more than 4 frames, calculate the plane parameter formula formed by connecting the optical center of the two end points of the line in each observation frame, and find the two planes with the largest normal vector cosine distance; Step 502: Calculate the dual Plücker matrix according to the parameter formula of the two planes, and calculate the Plücker coordinates of the structural line features in the world coordinate system using the dual Plücker matrix; Step 503: according to the starting frame pose and the type of the structure line, the Plücker coordinates of the structure line are converted from the world coordinate system to the local parameter coordinate system; Step 504: Calculate the intersection point with the local parameter plane, and use the intersection point as the initial parameter of the structure line.

6. The point-line fusion visual inertial odometer method for dark light and structured environment according to claim 5, characterized in that: In step 6, the specific steps of the LMHW dynamic update mechanism include: Step 601: Divide the structure line processing flow into a normal mode and a LMHW update mode; Step 602: In normal mode, based on the main direction vanishing point matching principle, the line features output by the front end are structurally divided according to the current camera frame pose and LMHW; Step 603: If the proportion of successful division of structural line features is lower than a preset threshold, it is determined that most of the line features in the current environment no longer meet the main direction of LMHW, and the LMHW update mode is entered; Step 604: in the LMHW update mode, the line features outputted by the front end are divided into vertical lines independent of the LMHW to separate the horizontal lines to be divided; the 1-Line RANSAC algorithm is used to identify the LMHW of the current frame according to the horizontal lines to be divided; Step 605: construct an LMHW sliding window manager to perform synchronous sliding window operation on the LMHW recognition result, and detect whether the root mean square of the adjacent errors of the LMHW in the sliding window is lower than the preset threshold; if so, it is determined that the LMHW fusion condition is met, and the average of all LMHWs in the sliding window is taken as the update result of the LMHW, exit the update mode and enter the normal mode, and add LMHW as a state variable to the sliding window optimization.

7. The point-line fusion visual inertial odometer method for dark light and structured environment according to claim 6, characterized in that: In step 8, the null space correction method of the structural line feature is: Assume that the parameter coordinates of the structural line feature after optimization are The LMHW parameter is θ M , the change in yaw after the first frame pose optimization in the sliding window is δθ Z ; For the vertical line feature, only the local parameter θ drifts in the null space, and the drift amount is equal to the yaw drift amount of the first frame of the sliding window. The correction method of the vertical line feature is defined as: For the horizontal line feature, its parameter drift is equivalent to the LMHW drift, and the drift amount is equal to the yaw drift amount of the first frame of the sliding window. The correction method of the horizontal line feature is defined as: θ` M =θ M -δθ Z .

Images