A point-line feature fusion type visual inertial odometry method and system based on a Fast-EDLines algorithm

Abstract

The application discloses a kind of point line feature fusion type visual inertial odometry method and system based on Fast-EDLines algorithm, first to the gray collection image is carried out Gaussian filter smoothing processing and obtains the image after processing, again the gradient calculation is carried out to the image after processing and obtains gradient amplitude, then according to gradient amplitude to significant gradient change pixel point is screened and obtains edge point, again according to edge point is carried out edge drawing and obtains edge pixel chain;The application effectively realizes the function of point line feature fusion type visual inertial odometry of image data using based on Fast-EDLines algorithm to the image sensor acquisition image, and through AVX2 instruction set acceleration gradient calculation and combined long short line segment merging strategy can extract continuous long line segment feature, not only long line segment detection efficiency in dark environment compared with traditional LSD algorithm is improved several times, also significantly enhance the geometric constraint of front end pose estimation, reduce positioning error, suitable for being widely promoted and use.

Description

Technical Field

[0001] This invention relates to the field of image sensor data processing technology, specifically to a visual inertial odometry method and system based on the Fast-EDLines algorithm, which integrates point and line features. Background Technology

[0002] Visual Simultaneous Localization and Mapping (Visual SLAM) is a technology that uses cameras (monocular, binocular, RGB-D, etc.) as the primary sensor to estimate its own trajectory in real time and simultaneously build a map of its environment. It is widely used in robotics, autonomous driving, augmented reality (AR), and virtual reality (VR). Although Visual SLAM technology has been developing for decades, it still faces many challenges in practical applications.

[0003] Currently, existing visual SLAM systems are highly sensitive to changes in illumination, sparse textures, and carrier motion noise. Furthermore, image contrast decreases in low-light environments, leading to frequent feature extraction failures. Additionally, the number of trackable corner points is limited in low-texture scenes, easily causing front-end tracking failures. Handheld device jitter not only exacerbates motion blur and feature loss but also accumulates pose estimation errors. Most existing visual SLAM systems rely on point features for pose estimation, resulting in mismatches in repetitive textures or structured scenes. For example, point features in scenes like corridors and walls lack uniqueness, easily leading to inter-frame matching errors. Therefore, a visual inertial odometry method and system based on the Fast-EDLines algorithm, which fuses point and line features, is needed. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and to better and more effectively address the problems of existing visual SLAM systems being highly sensitive to changes in lighting, sparse textures, and carrier motion noise, and experiencing reduced image contrast in low-light environments, leading to easy failure of feature extraction. Furthermore, the limited number of trackable corner points in low-texture scenes easily causes front-end tracking failures. Handheld device jitter not only exacerbates motion blur and feature loss but also accumulates pose estimation errors. Existing visual SLAM systems largely rely on point features for pose estimation, resulting in mismatches in repetitive textures or structured scenes, such as corridors and walls where point features lack uniqueness, easily leading to inter-frame matching errors. This invention provides a point-line feature fusion visual inertial odometry method and system based on the Fast-EDLines algorithm. It achieves visual inertial odometry calculation based on point-line feature fusion using the Fast-EDLines algorithm on image data acquired by image sensors. Furthermore, by accelerating gradient calculation through the AVX2 instruction set and combining a long-short line segment merging strategy, it can extract continuous long line segment features. This not only improves long line segment detection efficiency in low-light environments compared to traditional LSD systems... The algorithm has improved several times over and significantly enhanced the geometric constraints of front-end pose estimation, reducing positioning errors.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A visual-inertial odometry method based on point-line feature fusion using the Fast-EDLines algorithm includes the following steps:

[0007] Step A: Perform Gaussian filtering on the grayscale acquired image to smooth it and obtain the processed image. Then, perform gradient calculation on the processed image and obtain the gradient magnitude.

[0008] Step B: Filter pixels with significant gradient changes based on gradient magnitude and obtain edge points, then draw edges based on the edge points and obtain edge pixel chains;

[0009] Step C: Extract the first set of line segments by using the linear least squares fitting method on the edge pixel chain, and then remove the short line segments in the first set of line segments according to the Helmholtz principle to obtain the second set of line segments.

[0010] Step D: Use the false alarm number NFA to verify the significance of the line ends in the second set of line segments and remove the false line segments in the second set of line segments to obtain the third set of line segments.

[0011] Step E: Perform line segment matching and fusion on the line segments in the third set of line segments and remove isolated line segments to obtain the fourth set of line segments;

[0012] Step F: The Shi-Tomasi corner detection method is used to extract corner points in the processed image and use them as the first feature point set. Then, the first feature point set is tracked, removed, filtered and supplemented to obtain the second feature point set.

[0013] Step G: Use discrete-time integration to perform recursive integration calculation on the inertial measurement values ​​between consecutive image frames and obtain the IMU pre-integration;

[0014] Step H involves using the fourth line segment set, the second feature point set, and the IMU pre-integration as the observation inputs to optimize the backend module and complete the visual inertial odometry calculation.

[0015] The aforementioned visual inertial odometry method based on point-line feature fusion using the Fast-EDLines algorithm includes step A, which involves Gaussian filtering and smoothing the grayscale acquired image to obtain the processed image, and then calculating the gradient of the processed image to obtain the gradient magnitude. The specific steps are as follows.

[0016] Step A1: Perform Gaussian filtering smoothing on the grayscale acquired image and obtain the processed image. Specifically, the Gaussian filtering smoothing is performed using a 5*5 Gaussian filter.

[0017] Step A2: Calculate the gradient of the processed image and obtain the gradient magnitude, where the gradient calculation is shown in formula (1).

[0018] (1)

[0019] in, Represents pixels in an image grayscale value at that location Represents pixels gradient direction, Represents pixels Horizontal gradient information at the location, Represents pixels Vertical gradient information at the location, This represents the gradient magnitude.

[0020] The aforementioned visual inertial odometry method based on the Fast-EDLines algorithm, using point-line feature fusion, involves step B: filtering pixels with significant gradient changes based on gradient magnitude to obtain edge points, and then drawing edges based on these edge points to obtain edge pixel chains. The specific steps are as follows:

[0021] Step B1: Filter pixels with significant gradient changes based on gradient magnitude and obtain edge points. If a pixel's gradient magnitude is greater than the gradient magnitude of its neighboring pixels and also greater than a preset gradient threshold... When the gradient threshold is applied, the pixel is identified as an edge point. The calculation process is shown in formula (2).

[0022] (2)

[0023] in, Indicates angular tolerance. Indicates boundary error;

[0024] Step B2 involves drawing edges based on edge points and obtaining the edge pixel chain, specifically by calculating the gradient threshold. Then traverse the gradient graph and remove gradient values ​​less than the gradient threshold. The pixels are identified to obtain the edge region of the image. Then, the local gradient peaks in the edge region are used as anchor points, and pixels are gradually connected from the anchor points according to the gradient direction and magnitude to form an edge pixel chain.

[0025] The aforementioned visual inertial odometry method based on point-line feature fusion using the Fast-EDLines algorithm, in step C, involves extracting and obtaining a first set of line segments from the edge pixel chains using the linear least squares fitting method. Then, short line segments in the first set of line segments are removed according to the Helmholtz principle to obtain a second set of line segments. The specific steps are as follows.

[0026] Step C1 involves extracting and obtaining the first set of line segments from the edge pixel chains using the linear least squares fitting method. The specific steps are as follows.

[0027] Step C11, let the edge pixel sequence obtained by edge detection be... , … and the current subsequence length is Then the equation for the fitted line is shown in formula (3).

[0028] (3)

[0029] in, and All represent coefficients;

[0030] Step C12: Solve for the coefficients using least squares. and ,set up and They represent Pixels in and The mean value of the direction is then calculated as shown in formula (4).

[0031] (4)

[0032] Step C13: Calculate the vertical distance from each pixel to the line segment. As shown in formula (5),

[0033] (5);

[0034] Step C14: Set the error threshold If the mean square error of all pixels Not greater than the error threshold If the mean square error of all pixels is 1, then the current subsequence can be fitted into a valid straight line segment. Greater than the error threshold Then, the current line segment is truncated and the above fitting process is repeated for the remaining pixels until the entire pixel chain is processed, where the mean square error is... As shown in formula (6),

[0035] (6);

[0036] Step C2: According to Helmholtz's principle, remove the shortest line segments from the first set of line segments to obtain the second set of line segments. Let the minimum length of the line segment be... Then, according to Helmholtz's principle, the minimum length The calculation process is shown in formula (7).

[0037] (7)

[0038] in, Indicates the side length of the image. This represents the probability that the gradient directions are consistent.

[0039] The aforementioned visual inertial odometry method based on the Fast-EDLines algorithm, using point-line feature fusion, involves step D: verifying the saliency of line ends in the second set of line segments using the False Alarm Number (NFA) and removing false line segments to obtain the third set of line segments. The specific steps are as follows.

[0040] Step D1: Introduce the false alarm number (NFA) as the judgment criterion. Let the length be... In line segment A, at least one If the gradient direction of each pixel is consistent with the direction of the line segment, then the calculation process of the false alarm number NFA of line segment A is as shown in formula (8).

[0041] (8)

[0042] in, This indicates the number of potential line segments in the image. To indicate from Select from pixels One way;

[0043] Step D2, set the NFA threshold If the false alarm number (NFA) of line segment A is not greater than the NFA threshold If the false alarm rate (NFA) of line segment A is significant and has a high confidence level, then the line segment is retained; if the false alarm rate (NFA) of line segment A is less than the NFA threshold, then the line segment is retained. If so, then the pseudo-line segment is removed.

[0044] The aforementioned visual inertial odometry method based on point-line feature fusion using the Fast-EDLines algorithm, in step E, involves performing line segment matching and fusion on the line segments in the third set of line segments and removing isolated line segments to obtain the fourth set of line segments. The specific steps are as follows.

[0045] Step E1, let the initial set of line segments extracted after line segment detection and verification be... and each line segment Represented by the starting point and the ending point as According to the length threshold Set of line segments The set of long line segments is divided by Euclidean distance calculation. and short line segment set As shown in formula (9),

[0046] (9);

[0047] Step E2: Calculate the initial set of merged line segments. ,in Specifically, it refers to the set of short line segments. Each short line segment in Calculation and long line segment set Each long line segment in Given the direction and distance between them, find the long line segment that satisfies both the direction and position conditions. The specific steps are as follows:

[0048] Step E21: Find the long line segment that satisfies the direction condition. The specific steps are as follows:

[0049] Step E211, the angle between the short line segment and the long line segment. Perform the calculations as shown in formula (10).

[0050] (10)

[0051] Among them, the direction vector of the short line segment directional vector of long line segment ;

[0052] Step E212: Determine the angle between the shorter line segment and the longer line segment. Is it less than the angle threshold? As shown in formula (11),

[0053] (11)

[0054] in, This represents a small tolerance value; if formula (11) holds true, it means that the short line segment and the long line segment are close in direction;

[0055] Step E22: Find the long line segment that satisfies the positional conditions, specifically by calculating the points. to line segment The shortest distance is found, and the specific steps are as follows.

[0056] Step E221: Calculate the direction vector and magnitude of the line segment, as shown in formulas (12) and (13).

[0057] (12)

[0058] (13)

[0059] Where the coordinates of the starting point s are The coordinates of the endpoint e are V represents the displacement vector from the starting point s to the ending point e;

[0060] Step E222: Calculate the projection of the point onto the line; specifically, calculate the point... The projection point on the straight line Normalized position parameters As shown in formula (14),

[0061] (14)

[0062] Among them, the molecular part is The projected inner product along the straight line, where the projected inner product represents a point. The projection length along the straight line, where the denominator represents the square of the magnitude of the line segment direction vector, and the square of the magnitude is used to normalize the projection scale;

[0063] Step E223, if When, then the projection point Located inside the line segment as shown in formula (15), and the shortest distance from the point to the line. As shown in formula (16),

[0064] (15)

[0065] (16);

[0066] Step E224, if When the projection point is located on the extension line of the starting point of the line segment, the shortest distance is the length from the point to the starting point of the line segment, as shown in formula (17).

[0067] (17)

[0068] Step E225, if When the projection point is located on the extension line of the endpoint of the line segment, the shortest distance is the length from the point to the endpoint of the line segment, as shown in formula (18).

[0069] (18)

[0070] Step E226, calculate the short line segment starting point and the end point Reaching the long segment The distance is calculated using the following formula.

[0071] (19)

[0072] in, It represents the shortest distance from the shortest line segment to the longest line segment;

[0073] Step E227: Determine the shortest distance from the shorter line segment to the longer line segment. Is it less than the distance threshold? As shown in formula (20), if formula (20) is satisfied, it means that the short line segment and the long line segment are close in position. If not satisfied, the short line segment is regarded as an isolated line segment and eliminated.

[0074] (20);

[0075] Step E23, if a long line segment exists If both the direction and position conditions are met, then the short line segment will be... Integrate into long line segments Next, set the maximum number of iterations for merging line segments and remove the line segments involved in the merging from the third set of line segments. Update the endpoints of the merged long line segments as shown in formula (21).

[0076] (twenty one)

[0077] in, These are the coordinates of the endpoints of the merged long line segment.

[0078] The aforementioned visual inertial odometry method based on the Fast-EDLines algorithm, using point-line feature fusion, involves step F: extracting corner points from the processed image using the Shi-Tomasi corner detection method and using them as the first feature point set; then, tracking, removing, filtering, and supplementing the first feature point set to obtain the second feature point set. The specific steps are as follows.

[0079] Step F1: The corner points in the image are extracted using the Shi-Tomasi corner detection algorithm and the feature points are tracked using the KLT sparse optical flow method. The scale invariance of the point features is enhanced by using the image pyramid structure. In the process of feature tracking, if the tracking fails or the feature points exceed the image boundary, invalid points are removed using feature tracking state markers.

[0080] Step F2 involves using a basic matrix to remove outliers and improve the accuracy of feature matching. Then, the extracted corner points are filtered and the tracking points are sorted according to the number of tracking attempts, thereby optimizing the feature point distribution.

[0081] Step F3 involves homogenizing the densely concentrated feature areas and then dynamically adding newly extracted corner points.

[0082] The aforementioned point-line feature fusion visual inertial odometry method based on the Fast-EDLines algorithm, in step G, uses discrete-time integration to perform recursive integration calculation on the inertial measurement values ​​between consecutive image frames and obtains IMU pre-integration. Specifically, by accelerometer and gyroscope data between adjacent keyframes, the relative motion parameters of the sensor at adjacent image acquisition times can be calculated. The relative motion parameters include displacement, velocity, and attitude changes.

[0083] The aforementioned point-line feature fusion visual inertial odometry calculation method based on the Fast-EDLines algorithm, in step H, takes the fourth line segment set, the second feature point set, and IMU pre-integration as the observation input to the back-end optimization module and completes the visual inertial odometry calculation. The specific implementation process of the back-end optimization module adopts the VINS-Mono algorithm.

[0084] A visual inertial odometry system based on the Fast-EDLines algorithm and point-line feature fusion includes a gradient calculation module, an edge rendering module, a line segment extraction module, a line segment verification module, a line segment fusion module, a feature point extraction module, an IMU pre-integration acquisition module, and an observation optimization module. The gradient calculation module performs Gaussian filtering on the grayscale acquired image to obtain a processed image, and then calculates the gradient magnitude of the processed image. The edge rendering module filters pixels with significant gradient changes based on the gradient magnitude to obtain edge points, and then performs edge rendering based on these edge points to obtain an edge pixel chain. The line segment extraction module extracts the edge pixel chain using a linear least squares fitting method to obtain a first set of line segments, and then removes short line segments from the first set of line segments according to the Helmholtz principle to obtain a second set of line segments. The line segment verification module... The first module uses the False Alarm Number (NFA) to verify the saliency of the line ends in the second set of line segments and removes false line segments to obtain the third set of line segments. The second module uses the line segment fusion module to perform line segment matching and fusion on the line segments in the third set of line segments and removes isolated line segments to obtain the fourth set of line segments. The third module uses the Shi-Tomasi corner detection method to extract corner points in the processed image and use them as the first set of feature points. Then, the first set of feature points is tracked, removed, filtered, and supplemented to obtain the second set of feature points. The fourth module uses the IMU pre-integration module to perform recursive integration calculation on the inertial measurement values ​​between consecutive image frames using discrete-time integration to obtain the IMU pre-integration. The fifth module uses the fourth set of line segments, the second set of feature points, and the IMU pre-integration as the observation input to the back-end optimization module and completes the visual inertial odometry calculation.

[0085] The beneficial effects of this invention are as follows: This invention provides a point-line feature fusion visual inertial odometry method and system based on the Fast-EDLines algorithm. First, a grayscale acquired image is smoothed using Gaussian filtering to obtain a processed image. Then, gradient calculation is performed on the processed image to obtain the gradient magnitude. Next, pixels with significant gradient changes are filtered based on the gradient magnitude to obtain edge points. Then, edge drawing is performed based on the edge points to obtain edge pixel chains. Subsequently, the edge pixel chains are extracted using the linear least squares fitting method to obtain a first set of line segments. Then, short line segments in the first set of line segments are removed according to the Helmholtz principle to obtain a second set of line segments. Then, the false alarm number (NFA) is used to verify the saliency of the line ends in the second set of line segments and false line segments are removed to obtain a third set of line segments. Finally, the third set of line segments... The system performs line segment matching and fusion on the line segments in the set, and removes isolated line segments to obtain a fourth set of line segments. Then, the Shi-Tomasi corner detection method is used to extract corner points from the processed image and use them as the first set of feature points. The first set of feature points is then tracked, removed, filtered, and supplemented to obtain a second set of feature points. Finally, discrete-time integration is used to perform recursive integration calculations on the inertial measurement values ​​between consecutive image frames to obtain the IMU pre-integration. The fourth set of line segments, the second set of feature points, and the IMU pre-integration are then used as the observation input to the back-end optimization module to complete the visual inertial odometry calculation. This effectively realizes the function of visual inertial odometry calculation based on the Fast-EDLines algorithm for point-line feature fusion of image data acquired by image sensors, and it is also supported by AVX2. The instruction set accelerates gradient calculation and, combined with a long-short line segment merging strategy, can extract continuous long line segment features, solving the problem of sparse point features in low-texture scenes in existing technologies. This invention improves the long line segment detection efficiency in low-light environments by several times compared to the traditional LSD algorithm, with long line segments accounting for 100%, significantly enhancing the geometric constraints of front-end pose estimation. Through the fusion of point and line features, this invention significantly improves the accuracy of carrier motion estimation in weak-texture and low-light environments, and reduces positioning errors compared to traditional single-point feature methods. Simultaneously, trajectory drift is effectively suppressed, ensuring stable system operation. This solves the bottlenecks in positioning accuracy, real-time performance, and device adaptability in complex environments in existing technologies, breaking through the limitations of traditional point feature dependence and environmental interference. Attached Figure Description

[0086] Figure 1 This is an overall flowchart of a visual inertial odometry method based on the Fast-EDLines algorithm, according to the present invention.

[0087] Figure 2This is a schematic diagram of the operation process of a visual inertial odometry system based on the Fast-EDLines algorithm and point-line feature fusion according to the present invention.

[0088] Figure 3 This is a diagram of the algorithm code for accelerating the Sobel gradient calculation process using AVX2, as described in this invention.

[0089] Figure 4 This is a diagram of the algorithm code for the long / short line processing of this invention;

[0090] Figure 5 This is a comparison diagram of the detection effects of LSD, EDLines and the Fast-EDLines algorithm proposed in this invention in the embodiments of the present invention;

[0091] Figure 6 These are the underground space A sequence plan and motion trajectory diagram in the embodiments of the present invention;

[0092] Figure 7 This is a schematic diagram comparing the VIO trajectories of sequence A in underground space in an embodiment of the present invention;

[0093] Figure 8 This is a schematic diagram of the evo measurement results of the underground space A sequence trajectory in an embodiment of the present invention. Detailed Implementation

[0094] The present invention will now be further described with reference to the accompanying drawings.

[0095] like Figure 1-2 As shown, the present invention provides a visual inertial odometry method based on point-line feature fusion using the Fast-EDLines algorithm, comprising the following steps:

[0096] like Figure 3 As shown, step A involves performing Gaussian filtering to smooth the grayscale image and obtaining the processed image. Then, gradient calculation is performed on the processed image to obtain the gradient magnitude. The specific steps are as follows:

[0097] Step A1: Perform Gaussian filtering smoothing on the grayscale acquired image and obtain the processed image. Specifically, the Gaussian filtering smoothing is performed using a 5*5 Gaussian filter.

[0098] It effectively suppresses high-frequency noise and random noise, thereby improving image quality and providing a stable foundation for subsequent gradient calculation;

[0099] Step A2: Calculate the gradient of the processed image and obtain the gradient magnitude, where the gradient calculation is shown in formula (1).

[0100] (1)

[0101] in, Represents pixels in an image grayscale value at that location Represents pixels gradient direction, Represents pixels Horizontal gradient information at the location, Represents pixels Vertical gradient information at the location, This represents the gradient magnitude.

[0102] Traditional methods require separate calculation of horizontal gradient, vertical gradient, and gradient magnitude, resulting in repeated pixel readings and multiple memory accesses. In contrast, this invention fully utilizes the 256-bit parallel computing capability of the AVX2 instruction set, loading 16 pixels at a time to jointly calculate the horizontal and vertical gradients and their absolute values. The gradient magnitude is then directly obtained through the _mm256_abs_epi16 function, thereby significantly reducing the calculation steps and memory overhead.

[0103] Step B involves filtering pixels with significant gradient changes based on their gradient magnitudes to obtain edge points, then drawing edges based on these edge points to obtain edge pixel chains. The specific steps are as follows.

[0104] Step B1: Filter pixels with significant gradient changes based on gradient magnitude and obtain edge points. If a pixel's gradient magnitude is greater than the gradient magnitude of its neighboring pixels and also greater than a preset gradient threshold... When the gradient threshold is applied, the pixel is identified as an edge point. The calculation process is shown in formula (2).

[0105] (2)

[0106] in, Indicates angular tolerance. Indicates boundary error;

[0107] Step B2 involves drawing edges based on edge points and obtaining the edge pixel chain, specifically by calculating the gradient threshold. Then traverse the gradient graph and remove gradient values ​​less than the gradient threshold. The pixels are identified to obtain the edge region of the image. Then, the local gradient peaks in the edge region are used as anchor points, and pixels are gradually connected from the anchor points according to the gradient direction and magnitude to form an edge pixel chain.

[0108] The maximum boundary error between two consecutive pixels is typically 2, while according to the settings in the EDLines and LSD algorithms, when the experimental angle tolerance is... At that time, the length and alignment of the line segments were better;

[0109] Step C involves extracting the edge pixel chain using the linear least squares fitting method to obtain the first set of line segments. Then, based on the Helmholtz principle, short line segments in the first set of line segments are removed to obtain the second set of line segments. The specific steps are as follows.

[0110] Step C1 involves extracting and obtaining the first set of line segments from the edge pixel chains using the linear least squares fitting method. The specific steps are as follows.

[0111] Step C11, let the edge pixel sequence obtained by edge detection be... , … and the current subsequence length is Then the equation for the fitted line is shown in formula (3).

[0112] (3)

[0113] in, and All represent coefficients;

[0114] Step C12: Solve for the coefficients using least squares. and ,set up and They represent Pixels in and The mean value of the direction is then calculated as shown in formula (4).

[0115] (4)

[0116] Step C13: Calculate the vertical distance from each pixel to the line segment. As shown in formula (5),

[0117] (5);

[0118] Step C14: Set the error threshold If the mean square error of all pixels Not greater than the error threshold If the mean square error of all pixels is 1, then the current subsequence can be fitted into a valid straight line segment. Greater than the error threshold Then, the current line segment is truncated and the above fitting process is repeated for the remaining pixels until the entire pixel chain is processed, where the mean square error is... As shown in formula (6),

[0119] (6);

[0120] Step C2: According to Helmholtz's principle, remove the shortest line segments from the first set of line segments to obtain the second set of line segments. Let the minimum length of the line segment be... Then, according to Helmholtz's principle, the minimum length The calculation process is shown in formula (7).

[0121] (7)

[0122] in, Indicates the side length of the image. This represents the probability that the gradient directions are consistent.

[0123] like Figure 4 As shown, in step D, the significance of the line ends in the second set of line segments is verified using the false alarm number NFA, and false line segments in the second set of line segments are removed to obtain the third set of line segments. The specific steps are as follows.

[0124] Step D1: Introduce the false alarm number (NFA) as the judgment criterion. Let the length be... In line segment A, at least one If the gradient direction of each pixel is consistent with the direction of the line segment, then the calculation process of the false alarm number NFA of line segment A is as shown in formula (8).

[0125] (8)

[0126] in, This indicates the number of potential line segments in the image. To indicate from Select from pixels One way;

[0127] Step D2, set the NFA threshold If the false alarm number (NFA) of line segment A is not greater than the NFA threshold If the false alarm rate (NFA) of line segment A is significant and has a high confidence level, then the line segment is retained; if the false alarm rate (NFA) of line segment A is less than the NFA threshold, then the line segment is retained. If so, then the pseudo-line segment is removed.

[0128] Step E involves performing line segment matching and merging on the line segments in the third set of line segments, and removing isolated line segments to obtain the fourth set of line segments. The specific steps are as follows.

[0129] Step E1, let the initial set of line segments extracted after line segment detection and verification be... and each line segment Represented by the starting point and the ending point as According to the length threshold Set of line segments The set of long line segments is divided by Euclidean distance calculation. and short line segment set As shown in formula (9),

[0130] (9);

[0131] Step E2: Calculate the initial set of merged line segments. ,in Specifically, it refers to the set of short line segments. Each short line segment in Calculation and long line segment set Each long line segment in Given the direction and distance between them, find the long line segment that satisfies both the direction and position conditions. The specific steps are as follows:

[0132] Step E21: Find the long line segment that satisfies the direction condition. The specific steps are as follows:

[0133] Step E211, the angle between the short line segment and the long line segment. Perform the calculations as shown in formula (10).

[0134] (10)

[0135] Among them, the direction vector of the short line segment directional vector of long line segment ;

[0136] Step E212: Determine the angle between the shorter line segment and the longer line segment. Is it less than the angle threshold? As shown in formula (11),

[0137] (11)

[0138] in, This represents a small tolerance value; if formula (11) holds true, it means that the short line segment and the long line segment are close in direction;

[0139] Step E22: Find the long line segment that satisfies the positional conditions, specifically by calculating the points. to line segment The shortest distance is found, and the specific steps are as follows.

[0140] Step E221: Calculate the direction vector and magnitude of the line segment, as shown in formulas (12) and (13).

[0141] (12)

[0142] (13)

[0143] Where the coordinates of the starting point s are The coordinates of the endpoint e are V represents the displacement vector from the starting point s to the ending point e;

[0144] Step E222: Calculate the projection of the point onto the line; specifically, calculate the point... The projection point on the straight line Normalized position parameters As shown in formula (14),

[0145] (14)

[0146] Among them, the molecular part is The projected inner product along the straight line, where the projected inner product represents a point. The projection length along the straight line, where the denominator represents the square of the magnitude of the line segment direction vector, and the square of the magnitude is used to normalize the projection scale;

[0147] Step E223, if When, then the projection point Located inside the line segment as shown in formula (15), and the shortest distance from the point to the line. As shown in formula (16),

[0148] (15)

[0149] (16);

[0150] Step E224, if When the projection point is located on the extension line of the starting point of the line segment, the shortest distance is the length from the point to the starting point of the line segment, as shown in formula (17).

[0151] (17)

[0152] Step E225, if When the projection point is located on the extension line of the endpoint of the line segment, the shortest distance is the length from the point to the endpoint of the line segment, as shown in formula (18).

[0153] (18)

[0154] Step E226, calculate the short line segment starting point and the end point Reaching the long segment The distance is calculated using the following formula.

[0155] (19)

[0156] in, It represents the shortest distance from the shortest line segment to the longest line segment;

[0157] Step E227: Determine the shortest distance from the shorter line segment to the longer line segment. Is it less than the distance threshold? As shown in formula (20), if formula (20) is satisfied, it means that the short line segment and the long line segment are close in position. If not satisfied, the short line segment is regarded as an isolated line segment and eliminated.

[0158] (20);

[0159] Step E23, if a long line segment exists If both the direction and position conditions are met, then the short line segment will be... Integrate into long line segments Next, set the maximum number of iterations for merging line segments and remove the line segments involved in the merging from the third set of line segments. Update the endpoints of the merged long line segments as shown in formula (21).

[0160] (twenty one)

[0161] in, These are the coordinates of the endpoints of the merged long line segment.

[0162] Step F involves extracting corner points from the processed image using the Shi-Tomasi corner detection method and using them as the first feature point set. Then, the first feature point set is further processed by tracking, removing, filtering, and supplementing feature points to obtain the second feature point set. The specific steps are as follows.

[0163] Step F1: The corner points in the image are extracted using the Shi-Tomasi corner detection algorithm and the feature points are tracked using the KLT sparse optical flow method. The scale invariance of the point features is enhanced by using the image pyramid structure. In the process of feature tracking, if the tracking fails or the feature points exceed the image boundary, invalid points are removed using feature tracking state markers.

[0164] Step F2 involves using a basic matrix to remove outliers and improve the accuracy of feature matching. Then, the extracted corner points are filtered and the tracking points are sorted according to the number of tracking attempts, thereby optimizing the feature point distribution.

[0165] Step F3 involves homogenizing the densely concentrated feature areas and then dynamically adding newly extracted corner points.

[0166] Step G involves using discrete-time integration to perform recursive integration calculations on the inertial measurement values ​​between consecutive image frames and obtaining IMU pre-integration. Specifically, by accelerometer and gyroscope data between adjacent keyframes, the relative motion parameters of the sensor at adjacent image acquisition times can be calculated. The relative motion parameters include displacement, velocity, and attitude changes.

[0167] Step H involves using the fourth line segment set, the second feature point set, and IMU pre-integration as observation inputs to the backend optimization module and completing the visual inertial odometry calculation. The specific implementation of the backend optimization module adopts the VINS-Mono algorithm.

[0168] A visual inertial odometry system based on the Fast-EDLines algorithm and point-line feature fusion includes a gradient calculation module, an edge rendering module, a line segment extraction module, a line segment verification module, a line segment fusion module, a feature point extraction module, an IMU pre-integration acquisition module, and an observation optimization module. The gradient calculation module performs Gaussian filtering on the grayscale acquired image to obtain a processed image, and then calculates the gradient magnitude of the processed image. The edge rendering module filters pixels with significant gradient changes based on the gradient magnitude to obtain edge points, and then performs edge rendering based on these edge points to obtain an edge pixel chain. The line segment extraction module extracts the edge pixel chain using a linear least squares fitting method to obtain a first set of line segments, and then removes short line segments from the first set of line segments according to the Helmholtz principle to obtain a second set of line segments. The line segment verification module... The first module uses the False Alarm Number (NFA) to verify the saliency of the line ends in the second set of line segments and removes false line segments to obtain the third set of line segments. The second module uses the line segment fusion module to perform line segment matching and fusion on the line segments in the third set of line segments and removes isolated line segments to obtain the fourth set of line segments. The third module uses the Shi-Tomasi corner detection method to extract corner points in the processed image and use them as the first set of feature points. Then, the first set of feature points is tracked, removed, filtered, and supplemented to obtain the second set of feature points. The fourth module uses the IMU pre-integration module to perform recursive integration calculation on the inertial measurement values ​​between consecutive image frames using discrete-time integration to obtain the IMU pre-integration. The fifth module uses the fourth set of line segments, the second set of feature points, and the IMU pre-integration as the observation input to the back-end optimization module and completes the visual inertial odometry calculation.

[0169] like Figure 5-8As shown in the specific embodiments of the present invention, the detection efficiency of long line segments in low-light environments is improved by 3 times compared with the traditional LSD algorithm, with long line segments accounting for 100%, significantly enhancing the geometric constraints of front-end pose estimation. Furthermore, through point-line feature fusion, the present invention can significantly improve the accuracy of carrier motion estimation in weak texture and low-light environments. The implementation results show that compared with the traditional single-point feature method, the positioning error of the present invention is reduced by 63.99%, while trajectory drift is effectively suppressed, ensuring the stable operation of the system.

[0170] In summary, the present invention provides a point-line feature fusion visual inertial odometry method and system based on the Fast-EDLines algorithm. First, a grayscale acquired image is smoothed using Gaussian filtering to obtain a processed image. Then, gradient calculation is performed on the processed image to obtain the gradient magnitude. Next, pixels with significant gradient changes are filtered based on the gradient magnitude to obtain edge points. Then, edge drawing is performed based on the edge points to obtain edge pixel chains. Subsequently, the edge pixel chains are extracted using the least squares linear fitting method to obtain a first set of line segments. Then, short line segments in the first set of line segments are removed according to the Helmholtz principle to obtain a second set of line segments. Then, the false alarm rate (NFA) is used to verify the saliency of the line ends in the second set of line segments and false line segments are removed to obtain a third set of line segments. Finally, the third set of line segments... The system performs line segment matching and fusion on line segments in the image, and removes isolated line segments to obtain a fourth set of line segments. Then, the Shi-Tomasi corner detection method is used to extract corner points from the processed image and use them as the first set of feature points. The first set of feature points is then tracked, removed, filtered, and supplemented to obtain a second set of feature points. Finally, discrete-time integration is used to perform recursive integration calculations on the inertial measurement values ​​between consecutive image frames to obtain IMU pre-integration. The fourth set of line segments, the second set of feature points, and the IMU pre-integration are then used as the observation input to the back-end optimization module to complete the visual inertial odometry calculation. This effectively realizes the function of visual inertial odometry calculation based on the Fast-EDLines algorithm for point-line feature fusion of image data acquired by image sensors, and it is also supported by AVX2. The instruction set accelerates gradient calculation and, combined with a long-short line segment merging strategy, can extract continuous long line segment features, solving the problem of sparse point features in low-texture scenes in existing technologies. This invention improves the long line segment detection efficiency in low-light environments by several times compared to the traditional LSD algorithm, and the proportion of long line segments reaches 100%, significantly enhancing the geometric constraints of front-end pose estimation. This invention can significantly improve the accuracy of carrier motion estimation in weak-texture and low-light environments through the fusion of point and line features, and reduces positioning errors compared to traditional single-point feature methods. At the same time, trajectory drift is effectively suppressed, ensuring the stable operation of the system. It solves the bottlenecks of existing technologies in terms of positioning accuracy, real-time performance, and device adaptability in complex environments, and breaks through the limitations of traditional point feature dependence and environmental interference.

[0171] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A visual inertial odometry method based on point-line feature fusion using the Fast-EDLines algorithm, characterized in that: Includes the following steps, Step A: Perform Gaussian filtering on the grayscale acquired image to smooth it and obtain the processed image. Then, perform gradient calculation on the processed image and obtain the gradient magnitude. Step B: Filter pixels with significant gradient changes based on gradient magnitude and obtain edge points, then draw edges based on the edge points and obtain edge pixel chains; Step C: Extract the first set of line segments by using the linear least squares fitting method on the edge pixel chain, and then remove the short line segments in the first set of line segments according to the Helmholtz principle to obtain the second set of line segments. Step D: Use the false alarm number NFA to verify the significance of the line ends in the second set of line segments and remove the false line segments in the second set of line segments to obtain the third set of line segments. Step E: Perform line segment matching and fusion on the line segments in the third set of line segments and remove isolated line segments to obtain the fourth set of line segments; Step F: The Shi-Tomasi corner detection method is used to extract corner points in the processed image and use them as the first feature point set. Then, the first feature point set is tracked, removed, filtered and supplemented to obtain the second feature point set. Step G: Use discrete-time integration to perform recursive integration calculation on the inertial measurement values ​​between consecutive image frames and obtain the IMU pre-integration; Step H involves using the fourth line segment set, the second feature point set, and the IMU pre-integration as the observation inputs to optimize the backend module and complete the visual inertial odometry calculation.

2. The visual inertial odometry method based on point-line feature fusion according to claim 1, characterized in that: Step A involves performing Gaussian filtering to smooth the grayscale image and obtaining the processed image. Then, gradient calculation is performed on the processed image to obtain the gradient magnitude. The specific steps are as follows. Step A1: Perform Gaussian filtering smoothing on the grayscale acquired image and obtain the processed image. Specifically, the Gaussian filtering smoothing is performed using a 5*5 Gaussian filter. Step A2: Calculate the gradient of the processed image and obtain the gradient magnitude, where the gradient calculation is shown in formula (1). （1） in, Represents pixels in an image grayscale value at that location Represents pixels gradient direction, Represents pixels Horizontal gradient information at the location, Represents pixels Vertical gradient information at the location, This represents the gradient magnitude.

3. The visual inertial odometry method based on point-line feature fusion according to claim 2, characterized in that: Step B involves filtering pixels with significant gradient changes based on their gradient magnitudes to obtain edge points, then drawing edges based on these edge points to obtain edge pixel chains. The specific steps are as follows. Step B1: Filter pixels with significant gradient changes based on gradient magnitude and obtain edge points. If a pixel's gradient magnitude is greater than the gradient magnitude of its neighboring pixels and also greater than a preset gradient threshold... When the gradient threshold is applied, the pixel is identified as an edge point. The calculation process is shown in formula (2). （2） in, Indicates angular tolerance. Indicates boundary error; Step B2 involves drawing edges based on edge points and obtaining the edge pixel chain, specifically by calculating the gradient threshold. Then traverse the gradient graph and remove gradient values ​​less than the gradient threshold. The pixels are identified to obtain the edge region of the image. Then, the local gradient peaks in the edge region are used as anchor points, and pixels are gradually connected from the anchor points according to the gradient direction and magnitude to form an edge pixel chain.

4. The visual inertial odometry method based on point-line feature fusion according to claim 3, characterized in that: Step C involves extracting the edge pixel chain using the linear least squares fitting method to obtain the first set of line segments. Then, based on the Helmholtz principle, short line segments in the first set of line segments are removed to obtain the second set of line segments. The specific steps are as follows. Step C1 involves extracting and obtaining the first set of line segments from the edge pixel chains using the linear least squares fitting method. The specific steps are as follows. Step C11, let the edge pixel sequence obtained by edge detection be... , … and the current subsequence length is Then the equation for the fitted line is shown in formula (3). （3） in, and All represent coefficients; Step C12: Solve for the coefficients using least squares. and ,set up and They represent Pixels in and The mean value of the direction is then calculated as shown in formula (4). （4） Step C13: Calculate the vertical distance from each pixel to the line segment. As shown in formula (5), （5）； Step C14: Set the error threshold If the mean square error of all pixels Not greater than the error threshold If the mean square error of all pixels is 1, then the current subsequence can be fitted into a valid straight line segment. Greater than the error threshold Then, the current line segment is truncated and the above fitting process is repeated for the remaining pixels until the entire pixel chain is processed, where the mean square error is... As shown in formula (6), （6）； Step C2: According to Helmholtz's principle, remove the shortest line segments from the first set of line segments to obtain the second set of line segments. Let the minimum length of the line segment be... Then, according to Helmholtz's principle, the minimum length The calculation process is shown in formula (7). （7） in, Indicates the side length of the image. This represents the probability that the gradient directions are consistent.

5. The visual inertial odometry method based on point-line feature fusion according to claim 4, characterized in that: Step D involves using the False Alarm Number (NFA) to verify the salience of the line ends in the second set of line segments and removing false line segments from the second set to obtain the third set of line segments. The specific steps are as follows. Step D1: Introduce the false alarm number (NFA) as the judgment criterion. Let the length be... In line segment A, at least one If the gradient direction of each pixel is consistent with the direction of the line segment, then the calculation process of the false alarm number NFA of line segment A is as shown in formula (8). （8） in, This indicates the number of potential line segments in the image. To indicate from Select from pixels One way; Step D2, set the NFA threshold If the false alarm number (NFA) of line segment A is not greater than the NFA threshold If the false alarm rate (NFA) of line segment A is significant and has a high confidence level, then the line segment is retained; if the false alarm rate (NFA) of line segment A is less than the NFA threshold, then the line segment is retained. If so, then the pseudo-line segment is removed.

6. The visual inertial odometry method based on point-line feature fusion according to claim 5, characterized in that: Step E involves performing line segment matching and merging on the line segments in the third set of line segments, and removing isolated line segments to obtain the fourth set of line segments. The specific steps are as follows. Step E1, let the initial set of line segments extracted after line segment detection and verification be... and each line segment Represented by the starting point and the ending point as According to the length threshold Set of line segments The set of long line segments is divided by Euclidean distance calculation. and short line segment set As shown in formula (9), （9）； Step E2: Calculate the initial set of merged line segments. ,in Specifically, it refers to the set of short line segments. Each short line segment in Calculation and Long Line Segment Set Each long line segment in Given the direction and distance between them, find the long line segment that satisfies both the direction and position conditions. The specific steps are as follows: Step E21: Find the long line segment that satisfies the direction condition. The specific steps are as follows: Step E211, the angle between the short line segment and the long line segment. Perform the calculations as shown in formula (10). （10） Among them, the direction vector of the short line segment directional vector of long line segment ; Step E212: Determine the angle between the shorter line segment and the longer line segment. Is it less than the angle threshold? As shown in formula (11), （11） in, This represents a small tolerance value; if formula (11) holds true, it means that the short line segment and the long line segment are close in direction; Step E22: Find the long line segment that satisfies the positional conditions, specifically by calculating the points. to line segment The shortest distance is found, and the specific steps are as follows. Step E221: Calculate the direction vector and magnitude of the line segment, as shown in formulas (12) and (13). （12） （13） Where the coordinates of the starting point s are The coordinates of the endpoint e are V represents the displacement vector from the starting point s to the ending point e; Step E222: Calculate the projection of the point onto the line; specifically, calculate the point... The projection point on the straight line Normalized position parameters As shown in formula (14), （14） Among them, the molecular part is The projected inner product along the straight line, where the projected inner product represents a point. The projection length along the straight line, where the denominator represents the square of the magnitude of the line segment direction vector, and the square of the magnitude is used to normalize the projection scale; Step E223, if When, then the projection point Located inside the line segment as shown in formula (15), and the shortest distance from the point to the line. As shown in formula (16), （15） （16）； Step E224, if When the projection point is located on the extension line of the starting point of the line segment, the shortest distance is the length from the point to the starting point of the line segment, as shown in formula (17). （17） Step E225, if When the projection point is located on the extension line of the endpoint of the line segment, the shortest distance is the length from the point to the endpoint of the line segment, as shown in formula (18). （18） Step E226, calculate the short line segment starting point and the end point Reaching the long segment The distance is calculated using the following formula. （19） in, It represents the shortest distance from the shortest line segment to the longest line segment; Step E227: Determine the shortest distance from the shorter line segment to the longer line segment. Is it less than the distance threshold? As shown in formula (20), if formula (20) is satisfied, it means that the short line segment and the long line segment are close in position. If not satisfied, the short line segment is regarded as an isolated line segment and eliminated. （20）； Step E23, if a long line segment exists If both the direction and position conditions are met, then the short line segment will be... Integrate into long line segments Next, set the maximum number of iterations for merging line segments and remove the line segments involved in the merging from the third set of line segments. Update the endpoints of the merged long line segments as shown in formula (21). （21） in, These are the coordinates of the endpoints of the merged long line segment.

7. The visual inertial odometry method based on point-line feature fusion according to claim 6, characterized in that: Step F involves extracting corner points from the processed image using the Shi-Tomasi corner detection method and using them as the first feature point set. Then, the first feature point set is further processed by tracking, removing, filtering, and supplementing feature points to obtain the second feature point set. The specific steps are as follows. Step F1: The corner points in the image are extracted using the Shi-Tomasi corner detection algorithm and the feature points are tracked using the KLT sparse optical flow method. The scale invariance of the point features is enhanced by using the image pyramid structure. In the process of feature tracking, if the tracking fails or the feature points exceed the image boundary, invalid points are removed using feature tracking state markers. Step F2 involves using a basic matrix to remove outliers and improve the accuracy of feature matching. Then, the extracted corner points are filtered and the tracking points are sorted according to the number of tracking attempts, thereby optimizing the feature point distribution. Step F3 involves homogenizing the densely concentrated feature areas and then dynamically adding newly extracted corner points.

8. The visual inertial odometry method based on point-line feature fusion according to claim 7, characterized in that: Step G involves using discrete-time integration to perform recursive integration calculations on the inertial measurement values ​​between consecutive image frames and obtaining IMU pre-integration. Specifically, by accelerometer and gyroscope data between adjacent keyframes, the relative motion parameters of the sensor at adjacent image acquisition times can be calculated. The relative motion parameters include displacement, velocity, and attitude changes.

9. The visual inertial odometry method based on point-line feature fusion according to claim 8, characterized in that: Step H involves using the fourth line segment set, the second feature point set, and IMU pre-integration as observation inputs to the backend optimization module and completing the visual inertial odometry calculation. The specific implementation of the backend optimization module adopts the VINS-Mono algorithm.

10. A visual inertial odometry system based on the Fast-EDLines algorithm and fusion of point and line features, wherein the specific calculation process of the visual inertial odometry system is based on the visual inertial odometry method according to any one of claims 1-9, characterized in that: It includes a gradient calculation module, an edge drawing module, a line segment extraction module, a line segment verification module, a line segment fusion module, a feature point extraction module, an IMU pre-integration acquisition module, and an observation optimization module. The gradient calculation module is used to perform Gaussian filtering smoothing on the grayscale acquisition image and obtain the processed image, and then perform gradient calculation on the processed image to obtain the gradient magnitude. The edge drawing module is used to filter pixels with significant gradient changes based on the gradient magnitude and obtain edge points, and then draw edges based on the edge points to obtain edge pixel chains; The line segment extraction module is used to extract the edge pixel chain using the linear least squares fitting method and obtain a first set of line segments. Then, according to the Helmholtz principle, short line segments in the first set of line segments are removed to obtain a second set of line segments. The line segment verification module is used to verify the salience of the line ends in the second set of line segments using the false alarm number NFA and to remove the false line segments in the second set of line segments to obtain the third set of line segments. The line segment fusion module is used to perform line segment matching and fusion on the line segments in the third line segment set and remove isolated line segments to obtain the fourth line segment set. The feature point extraction module is used to extract corner points in the processed image using the Shi-Tomasi corner detection method and use them as the first feature point set. Then, the first feature point set is used to track, remove, filter and supplement feature points to obtain the second feature point set. The IMU pre-integration acquisition module is used to perform recursive integration calculations on the inertial measurement values ​​between consecutive image frames using discrete-time integration and obtain the IMU pre-integration. The observation optimization module is used to take the fourth line segment set, the second feature point set, and the IMU pre-integration as the observation input back-end optimization module and complete the visual inertial odometry calculation.

Images