An improved LVI-SAM method integrating multi-line joint curvature calculation and adaptive thresholding
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-06-10
- Publication Date
- 2026-08-14
AI Technical Summary
[0004]针对相似纹理环境下SLAM定位难题的三大主流改进方法，虽在一定程度上缓解了定位漂移、跟踪失效等核心问题，提升了系统在弱纹理场景下的基本适配能力，但仍存在技术瓶颈：普遍忽视三维几何感知能力提升，缺乏自适应曲率阈值调控机制，难以兼顾远距离与近距离特征的有效检测，导致前端特征辨识度不足、后端误差抑制不彻底的问题未能得到根本解决
[0045]1、本发明提出多线联合曲率计算方法，突破传统单线曲率计算的局限性，对IMU运动补偿后的去畸变激光点云，选取每个特征点所在扫描线及相邻多条扫描线的点云构建局部三维平面，通过主成分分析法计算点云初始曲率，为进一步降低噪声干扰、提升曲率计算的可靠性，在初始曲率计算完成后引入曲率平滑处理环节，通过高斯滤波对初始曲率进行平滑优化，有效抑制孤立噪声点对曲率计算结果的影响。充分利用点云的空间几何分布特征，弥补垂直扫描线方向几何变化感知不足的缺陷，显著提升相似纹理环境下点云特征的辨识度与唯一性，提升特征点提取的稳定性。
Smart Images
Figure CN122568537A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of Simultaneous Localization and Mapping (SLAM) technology based on multi-sensor fusion, specifically involving an improved LVI-SAM method that integrates multi-line joint curvature calculation and adaptive threshold. Background Technology
[0002] Simultaneous Localization and Mapping (SLAM) technology is a core support for autonomous navigation systems to achieve environmental perception, pose estimation, and spatial modeling. It has been widely applied in various engineering scenarios such as indoor service robots, autonomous inspection, park autonomous driving, and UAV aerial surveying. However, in complex scenarios such as tree obstruction, narrow corridors, and underground spaces, the positioning accuracy of SLAM is easily affected. Therefore, multi-sensor fusion SLAM is a core technology to ensure high positioning accuracy and strong robustness. In the multi-sensor fusion SLAM system, LiDAR (Light Detection and Ranging) has become the core sensor for environmental geometry modeling due to its advantages of high ranging accuracy, strong anti-dynamic interference capability, and good environmental adaptability. Combined with the high-frequency motion information of the Inertial Measurement Unit (IMU), it forms Lidar Inertial Odometry Smoothing and Mapping (LIO-SAM) technology. Through tight coupling optimization, it achieves high-precision pose estimation and exhibits good performance in environments with rich textures and significant geometric features. However, single laser-inertial fusion systems are prone to problems such as point cloud matching difficulties and pose estimation drift in scenes with regular walls and sparse geometric features. Cameras, on the other hand, can capture rich texture and semantic information of the environment, making them suitable for scene recognition and feature association, effectively compensating for the limitations of LiDAR in scenes with weak geometric features. Against this backdrop, Shan et al. proposed the Lidar-Visual-Inertial Odometry and Mapping via Smoothing and Mapping (LVI-SAM) algorithm. This algorithm integrates the geometric accuracy of LiDAR, the rich texture of visual cameras, and the high-frequency motion estimation capabilities of IMUs, constructing a tightly coupled fusion framework based on graph optimization. This has become a mainstream solution for improving the robustness and positioning accuracy of SLAM systems in complex scenes. The LVI-SAM algorithm decouples the system into two independent yet collaborative subsystems: laser inertial and visual inertial. By jointly optimizing laser point cloud constraints, visual reprojection constraints, and IMU pre-integration constraints, it alleviates the localization limitations of single sensors or loosely coupled systems in scenarios with weak textures, drastic lighting changes, rapid motion, and missing geometric features, laying the foundation for multi-sensor fusion SLAM.Despite the significant advantages of LVI-SAM in multimodal information fusion, it still faces unavoidable core technical challenges in environments with similar textures and repetitive structures: First, the feature extraction process of LiDAR point clouds uses the traditional single-line curvature calculation method, which only calculates curvature along a single laser scanning line. This fails to capture the three-dimensional geometric changes in the direction perpendicular to the scanning line, making it difficult to distinguish subtle geometric features in similar texture areas, resulting in insufficient discriminativeness and uniqueness of the extracted features. Second, the classification of corner points and planar points uses a fixed curvature threshold, which does not consider the inherent characteristics of LiDAR point clouds. Concave and convex features of the same physical size have smaller angle changes when projected at a distance, resulting in lower calculated curvature values. Furthermore, sparse point clouds have fewer local neighborhood points, leading to poor stability in covariance estimation. This can easily cause nearby feature points to be over-extracted, while equally important effective feature points at a distance are ignored due to curvature distortion. [7] The aforementioned problems directly lead to the failure of point cloud feature matching and fluctuations in local pose estimation, which in turn cause the accumulation of global trajectory drift errors, ultimately severely affecting the system's positioning accuracy in similar texture environments.
[0003] To address the challenge of localization in SLAM systems within similar textured environments, scholars both domestically and internationally have conducted extensive targeted research. The research focus primarily revolves around three core directions: multimodal information enhancement, feature extraction optimization, and robust estimation mechanism construction. Regarding multimodal information enhancement, many researchers leverage the complementary advantages of multi-source sensing—laser, vision, and inertial—to overcome the limitations of single sensors and improve the completeness of perception in similar textured environments. Chen et al., addressing the insufficient perception capability in weakly textured scenes, proposed a multimodal depth association fusion method. This method reconstructs dense depth representations by stacking multiple frames of laser point clouds and bidirectionally couples visual pixel-level depth information. This effectively compensates for the sparseness and limited geometric constraints of single-frame laser point clouds, strengthens the stability of cross-modal feature association matching, and alleviates the problem of incomplete perception in similar textured environments. However, these methods only integrate raw sensor data and do not specifically adapt to the perceptual differences between near and far regions. When faced with extremely similar texture scenes, the modal complementarity gain is limited, making it difficult to weaken invalid interference information at the source. In terms of feature extraction optimization, Zhang et al. introduced line features with stronger structural stability and constructed a point-line joint extraction framework. They supplemented the discrete point feature constraints with global line features, enriched the effective feature volume in similar texture scenes, significantly reduced the probability of feature matching failure, and effectively improved the robustness of the front end. In terms of robust estimation mechanism construction, Wang et al. based their approach on the LVI-SAM framework and embedded a Super Point deep learning high-dimensional feature extraction network to strengthen the weak feature representation capability of similar textures. Combined with a global factor graph joint optimization strategy, they dual-constrained pose deviations, effectively improving the back-end localization stability in complex scenes. Existing mainstream optimization schemes highly rely on the quality of the front-end basic features and do not specifically compensate for the shortcomings of 3D geometric perception. The inherent defects in front-end feature extraction cannot be fully compensated by the back end, and the overall system robustness improvement has a clear upper limit. Summary of the Invention
[0004] While the three mainstream improvement methods for SLAM localization in similar texture environments have alleviated core issues such as localization drift and tracking failure to some extent and improved the system's basic adaptability in weak texture scenes, they still have technical bottlenecks: they generally neglect the improvement of 3D geometric perception capabilities, lack adaptive curvature threshold control mechanisms, and struggle to effectively detect both long-distance and short-distance features, resulting in insufficient front-end feature recognition and incomplete back-end error suppression. This invention addresses the shortcomings of the LVI-SAM algorithm in similar texture environments by optimizing a laser inertial subsystem as the primary component and a visual inertial subsystem as a secondary component. It retains the advantages of the original algorithm's tightly coupled fusion framework and multi-sensor collaborative operation, proposing targeted improvement strategies from two aspects: enhancing 3D geometric perception capabilities and achieving adaptive curvature thresholds. The aim is to improve the quality and quantity of effective feature point extraction in similar texture regions, providing reliable geometric constraints for subsequent feature matching and multi-sensor factor map joint optimization. Simultaneously, the visual inertial subsystem, by stably providing texture constraints and IMU motion priors, can provide auxiliary compensation for feature degradation in weak geometric regions of the laser point cloud, improving the overall stability of the system's pose estimation. The above improvements enhance the system's positioning accuracy and robustness.
[0005] An improved LVI-SAM method integrating multi-line joint curvature calculation and adaptive thresholding includes the following steps:
[0006] Step 1: Construct an improved multi-sensor fusion SLAM system framework based on graph optimization, with the laser inertial subsystem (LIO) as the core and the visual inertial subsystem (VIO) as auxiliary compensation. At the laser inertial front end, for the distortion-reduced laser point cloud, a multi-line joint curvature calculation method and a Gaussian weighted curvature smoothing strategy are introduced to extract the initial curvature with three-dimensional spatial geometric features and suppress noise. On this basis, an adaptive curvature threshold dynamic adjustment strategy based on spatial distance and local point cloud density is introduced to complete the adaptive and accurate determination and classification of corner points and planar points. Finally, the extracted feature points are jointly constructed with visual reprojection constraints and IMU pre-integration constraints to construct a factor graph for tight coupling optimization and closed-loop correction, thereby improving the positioning accuracy and robustness of the system in environments with similar textures and repetitive structures.
[0007] Step 2: To overcome the limitation of traditional single-line curvature calculation in failing to capture geometric changes in the direction of the vertical scan line, this invention proposes a multi-line joint curvature calculation method. This method directly applies to the distortion-free 3D laser point cloud, selecting the current scan line where the feature point is located and the point clouds of multiple adjacent scan lines to jointly construct a local 3D plane. Principal component analysis (PCA) is used to calculate the covariance matrix of the point cloud in the neighborhood and perform eigenvalue decomposition. The initial curvature of the point cloud is calculated using the physical mapping relationship between the minimum and maximum eigenvalues, thereby significantly enhancing the distinguishability and uniqueness of feature points in similar textures and geometrically sparse scenes.
[0008] Step 3: To suppress the interference of measurement noise in the original point cloud on the stability of feature point extraction, this invention proposes a curvature smoothing method. After obtaining the initial curvature of the point cloud, a Gaussian filtering smoothing optimization step is introduced. A Gaussian weighted function is constructed with the spatial distance from the target feature point to each point cloud in the neighborhood as the independent variable. The weighted average of the initial curvature of all point clouds in the neighborhood is used as the final smoothed curvature value of the target point. While preserving the local spatial geometric change trend, isolated noise points are effectively eliminated, providing a reliable basis for subsequent feature selection.
[0009] Step 4: To address the issue that fixed curvature thresholds are easily affected by the spatial distance and density unevenness of point clouds, leading to the loss of effective feature points at long distances, this invention proposes an adaptive curvature threshold adjustment strategy based on spatial distance and local density. The global curvature median of the entire effective point cloud is used as the basic threshold. A distance attenuation function is constructed based on the real-time spatial distance from each point to the sensor, and a density compensation function is constructed in combination with the number of effective local neighborhood points. An adaptive curvature threshold judgment standard for each point cloud is generated through dynamic modulation of two-factor product. Finally, the nonlinear classification judgment of feature points is achieved by combining the neighborhood density condition and the upper and lower bound conditions of the dynamic threshold.
[0010] Step 1 introduces a multi-line joint curvature calculation method and a Gaussian weighted curvature smoothing strategy at the laser inertial front end, and introduces an adaptive curvature threshold dynamic adjustment strategy based on spatial distance and local point cloud density to complete the adaptive and accurate determination and classification of corner points and planar points.
[0011] The specific steps are as follows:
[0012] Step 1-1: In the feature curvature extraction stage, the front end constructs a local spatial neighborhood for the 3D laser point cloud after distortion removal by the inertial measurement unit (IMU) using the current scan line and multiple vertically adjacent scan lines. This enables the network to simultaneously capture the complete 3D geometric structure change trend in both horizontal and vertical directions. To enhance feature discrimination and alleviate the interference of point cloud measurement noise on curvature calculation, principal component analysis and Gaussian weighted smoothing filtering are introduced in sequence to achieve noise suppression and smooth reconstruction of the initial curvature in two dimensions: spatial geometric mapping and neighborhood distance weight. This improves the stability of local curvature representation while maintaining efficient computation.
[0013] Steps 1-2: In the feature selection and judgment stage, considering that traditional fixed curvature thresholds are prone to curvature distortion and spatial oversmoothing in long-range radar scanning or sparse local point cloud regions, especially in distant object surfaces and geometrically sparse regions, effective feature points are easily missed or lost, this invention designs an adaptive curvature threshold dynamic adjustment framework based on dual-factor modulation. This framework consists of two parts: a distance attenuation mechanism and a density compensation mechanism. The distance attenuation mechanism enhances the feature expression of distant weak geometric regions by displaying the perspective changes at near and far distances. The density compensation mechanism performs fine dynamic calibration of the feature judgment scale of different dense and sparse neighborhoods at the point-level scale, thereby improving the consistency of corner point and planar point classification and spatial recognition ability of the whole frame point cloud at different spatial scales. The overall framework effectively improves the problems of discontinuous feature extraction and matching failure in complex similar texture scenes while maintaining computational efficiency.
[0014] Step 2: For the distortion-free laser point cloud after IMU motion compensation, a multi-line joint curvature calculation method is used for each laser point. The initial curvature of the point cloud is calculated by principal component analysis, which fully considers the three-dimensional geometric features of the point cloud under similar texture environment and makes up for the defect that the traditional single-line curvature cannot reflect the geometric changes in the vertical scan line direction.
[0015] The specific steps are as follows:
[0016] For the distortion-free laser point cloud after motion compensation by the inertial measurement unit, for each laser point, the left and right adjacent points on the laser scanning line are selected, and the point clouds on multiple adjacent laser scanning lines are introduced to jointly form a local three-dimensional planar neighborhood.
[0017] For the distortion-free laser point cloud after IMU motion compensation, a multi-line joint curvature calculation method is used for each laser point. Local curvature is estimated based on principal component analysis, and the curvature of the neighborhood is then calculated. The covariance matrix of all points within the matrix is calculated as follows:
[0018] (1)
[0019] In the formula, For the neighborhood centroid, the covariance matrix Eigenvalue decomposition is performed as follows:
[0020] (2)
[0021] The magnitude of the eigenvalue corresponds to the degree of dispersion of the point cloud along the direction of the corresponding eigenvector. The largest eigenvalue represents the main direction of the point cloud distribution and has the highest degree of dispersion. The smallest eigenvalue represents the normal direction of the point cloud distribution and has the lowest degree of dispersion. Based on the physical meaning of eigenvalues, eigenvalues... With point The degree of surface variation is defined as:
[0022] (3)
[0023] The range of values for this curvature value is: ,when When it approaches 1, ,illustrate When the value approaches 0, the local point cloud approximates a point distribution; the more drastic the surface change, the more likely the target point is an edge feature point. When it approaches 0, Approaching 0 indicates that the local point cloud is approximately planar in distribution, with gentle surface changes, and the target point is a planar feature point; when When taking the median value, the local point cloud is approximately linearly distributed, representing line feature points. Multi-line joint curvature calculation fully considers the three-dimensional geometric features of point clouds in similar texture environments, making up for the deficiency of traditional single-line curvature calculation in reflecting geometric changes in the direction of the vertical scan line, and significantly improving the ability to distinguish point cloud feature categories.
[0024] Step 3 aims to suppress the impact of noise on curvature calculation. After obtaining the initial curvature, the weighted average of the curvatures of all point clouds within the local neighborhood is used as the final curvature value for that point. This neighborhood curvature value is then used for subsequent feature point determination.
[0025] The specific steps are as follows:
[0026] For point Its smoothed curvature Defined as:
[0027] (4)
[0028] The contribution of different points in the current neighborhood to the curvature of the target point is determined by the distance between them; the closer the points are, the greater their weight, and vice versa.
[0029] In the formula, the weights Gaussian weighting is usually used.
[0030] (5)
[0031] In the formula, Using bandwidth parameters to control the distance decay rate, this smoothing strategy effectively eliminates the interference of isolated noise points on curvature calculation while preserving the local geometric change trend, ensuring the stability and reliability of curvature values and providing accurate judgment criteria for subsequent feature point selection.
[0032] Step 4 proposes an adaptive curvature threshold. Based on the spatial distance from each point cloud to the sensor and the number of local effective neighborhood points, the curvature judgment threshold is dynamically adjusted to replace the original fixed threshold to complete the accurate classification of edge points and planar points, and to select effective feature points with high geometric significance and high repeatability. This solves the problem of feature extraction deviation caused by the density and distance differences of point clouds in the traditional fixed threshold.
[0033] The specific steps are as follows:
[0034] (6)
[0035] In its formula, Based on the basic threshold, this paper statistically analyzes the curvature value distribution of the entire frame's effective point cloud and uses the global curvature median as the basic threshold for adaptive adjustment. For point Distance to the sensor, This represents the number of locally effective neighborhood points. and These are the distance decay function and the density compensation function, respectively, used to compensate for the problem of small curvature of point clouds at a distance and unstable estimation of point cloud curvature in sparse regions;
[0036] (7)
[0037] (8)
[0038] In the formula, To control the distance decay rate, and These are distance normalization parameters; For reference neighborhood points, Control the density compensation intensity. The greater the point cloud distance, the more... The smaller, The lower the value, the better to compensate for the curvature of sparse regions and achieve effective extraction of distant feature points;
[0039] Step 4-2, the improved feature point determination condition of this invention is:
[0040] (9)
[0041] (10)
[0042] (11)
[0043] In the formula, The number of effective neighborhood points participating in curvature calculation, The number of neighboring points when the distance is farthest. Using the base threshold The calculated maximum threshold. When the above three conditions are met, the point cloud can be determined to be a corner point; otherwise, the point cloud can be determined to be a planar point.
[0044] Beneficial effects of this invention:
[0045] 1. This invention proposes a multi-line joint curvature calculation method, overcoming the limitations of traditional single-line curvature calculation. For the distortion-free laser point cloud after IMU motion compensation, a local three-dimensional plane is constructed by selecting the scan line containing each feature point and the point clouds of multiple adjacent scan lines. The initial curvature of the point cloud is calculated using principal component analysis. To further reduce noise interference and improve the reliability of curvature calculation, a curvature smoothing process is introduced after the initial curvature calculation. Gaussian filtering is used to smooth and optimize the initial curvature, effectively suppressing the influence of isolated noise points on the curvature calculation results. This fully utilizes the spatial geometric distribution characteristics of the point cloud, compensating for the insufficient perception of geometric changes in the vertical scan line direction, significantly improving the recognizability and uniqueness of point cloud features in similar texture environments, and enhancing the stability of feature point extraction.
[0046] 2. This invention proposes an adaptive curvature threshold adjustment strategy based on spatial distance and local density. Unlike the traditional fixed curvature threshold, the adaptive curvature threshold optimizes the curvature judgment standard by adjusting the spatial distance from each point to the sensor and the number of effective local neighboring points through distance attenuation and density compensation. This solves the problem of feature extraction deviation caused by uneven point cloud density and distance differences, achieves fair detection of features at different distances, and effectively selects effective feature points with high geometric significance and repeatability. Attached Figure Description
[0047] Figure 1 is a flowchart of an improved LVI-SAM method that integrates multi-line joint curvature calculation and adaptive threshold according to the present invention.
[0048] Figure 2 is a detailed flowchart of step 1 of one embodiment of the present invention;
[0049] Figure 3 is a detailed flowchart of step 2 of one embodiment of the present invention;
[0050] Figure 4 is a detailed flowchart of step 3 of one embodiment of the present invention;
[0051] Figure 5 is a detailed flowchart of step 4 of one embodiment of the present invention;
[0052] Figure 6 is a comparison of the absolute pose error between the Street_04 and Walk datasets of the present invention.
[0053] Figure 7 is a comparison chart of the cumulative distribution function of APE according to the present invention. Detailed Implementation
[0054] An embodiment of the present invention will be further described below with reference to the accompanying drawings.
[0055] In this embodiment of the invention, a point cloud semantic segmentation method based on fusion feature enhancement and boundary awareness is used, such as... Figure 1 As shown, it includes the following steps:
[0056] Step 1 introduces a multi-line joint curvature calculation method and a Gaussian weighted curvature smoothing strategy to the laser inertial front end, and introduces an adaptive curvature threshold dynamic adjustment strategy based on spatial distance and local point cloud density to complete the adaptive and accurate determination and classification of corner points and planar points.
[0057] Step 1-1: In the feature curvature extraction stage, the front end constructs a local spatial neighborhood for the 3D laser point cloud after distortion removal by the inertial measurement unit (IMU) using the current scan line and multiple vertically adjacent scan lines. This enables the network to simultaneously capture the complete 3D geometric structure change trend in both horizontal and vertical directions. To enhance feature discrimination and alleviate the interference of point cloud measurement noise on curvature calculation, principal component analysis and Gaussian weighted smoothing filtering are introduced in sequence to achieve noise suppression and smooth reconstruction of the initial curvature in two dimensions: spatial geometric mapping and neighborhood distance weight. This improves the stability of local curvature representation while maintaining efficient computation.
[0058] Steps 1-2: In the feature selection and judgment stage, considering that traditional fixed curvature thresholds are prone to curvature distortion and spatial oversmoothing in long-range radar scanning or sparse local point cloud regions, especially in distant object surfaces and geometrically sparse regions, effective feature points are easily missed or lost, this invention designs an adaptive curvature threshold dynamic adjustment framework based on dual-factor modulation. This framework consists of two parts: a distance attenuation mechanism and a density compensation mechanism. The distance attenuation mechanism enhances the feature expression of distant weak geometric regions by displaying the perspective changes at near and far distances. The density compensation mechanism performs fine dynamic calibration of the feature judgment scale of different dense and sparse neighborhoods at the point-level scale, thereby improving the consistency of corner point and planar point classification and spatial recognition ability of the whole frame point cloud at different spatial scales. The overall framework effectively improves the problems of discontinuous feature extraction and matching failure in complex similar texture scenes while maintaining computational efficiency.
[0059] Step 2: For the distortion-free laser point cloud after IMU motion compensation, a multi-line joint curvature calculation method is used for each laser point. The initial curvature of the point cloud is calculated by principal component analysis, which fully considers the three-dimensional geometric features of the point cloud under similar texture environment and makes up for the defect that the traditional single-line curvature cannot reflect the geometric changes in the vertical scan line direction.
[0060] For the distortion-free laser point cloud after motion compensation by the inertial measurement unit, for each laser point, the left and right adjacent points on the laser scanning line are selected, and the point clouds on multiple adjacent laser scanning lines are introduced to jointly form a local three-dimensional planar neighborhood.
[0061] For the distortion-free laser point cloud after IMU motion compensation, a multi-line joint curvature calculation method is used for each laser point. Local curvature is estimated based on principal component analysis, and the curvature of the neighborhood is then calculated. The covariance matrix of all points within the matrix is calculated as follows:
[0062] (1)
[0063] In the formula, For the neighborhood centroid, the covariance matrix Eigenvalue decomposition is performed as follows:
[0064] (2)
[0065] The magnitude of the eigenvalue corresponds to the degree of dispersion of the point cloud along the direction of the corresponding eigenvector. The largest eigenvalue represents the main direction of the point cloud distribution and has the highest degree of dispersion. The smallest eigenvalue represents the normal direction of the point cloud distribution and has the lowest degree of dispersion. Based on the physical meaning of eigenvalues, eigenvalues... With point The degree of surface variation is defined as:
[0066] (3)
[0067] The range of values for this curvature value is: ,when When it approaches 1, ,illustrate When the value approaches 0, the local point cloud approximates a point distribution; the more drastic the surface change, the more likely the target point is an edge feature point. When it approaches 0, Approaching 0 indicates that the local point cloud is approximately planar in distribution, with gentle surface changes, and the target point is a planar feature point; when When taking the median value, the local point cloud is approximately linearly distributed, representing line feature points. Multi-line joint curvature calculation fully considers the three-dimensional geometric features of point clouds in similar texture environments, making up for the deficiency of traditional single-line curvature calculation in reflecting geometric changes in the direction of the vertical scan line, and significantly improving the ability to distinguish point cloud feature categories.
[0068] Step 3 aims to suppress the impact of noise on curvature calculation. After obtaining the initial curvature, the weighted average of the curvatures of all point clouds within the local neighborhood is used as the final curvature value for that point. This neighborhood curvature value is then used for subsequent feature point determination.
[0069] For point Its smoothed curvature Defined as:
[0070] (4)
[0071] The contribution of different points in the current neighborhood to the curvature of the target point is determined by the distance between them; the closer the points are, the greater their weight, and vice versa.
[0072] In the formula, the weights Gaussian weighting is usually used.
[0073] (5)
[0074] In the formula, Using bandwidth parameters to control the distance decay rate, this smoothing strategy effectively eliminates the interference of isolated noise points on curvature calculation while preserving the local geometric change trend, ensuring the stability and reliability of curvature values and providing accurate judgment criteria for subsequent feature point selection.
[0075] Step 4 proposes an adaptive curvature threshold. Based on the spatial distance from each point cloud to the sensor and the number of local effective neighborhood points, the curvature judgment threshold is dynamically adjusted to replace the original fixed threshold to complete the accurate classification of edge points and planar points, and to select effective feature points with high geometric significance and high repeatability. This solves the problem of feature extraction deviation caused by the density and distance differences of point clouds in the traditional fixed threshold.
[0076] The specific steps are as follows:
[0077] (6)
[0078] In its formula, Based on the basic threshold, this paper statistically analyzes the curvature value distribution of the entire frame's effective point cloud and uses the global curvature median as the basic threshold for adaptive adjustment. For point Distance to the sensor, This represents the number of locally effective neighborhood points. and These are the distance decay function and the density compensation function, respectively, used to compensate for the problem of small curvature of point clouds at a distance and unstable estimation of point cloud curvature in sparse regions;
[0079] (7)
[0080] (8)
[0081] In the formula, To control the distance decay rate, and These are distance normalization parameters; For reference neighborhood points, Control the density compensation intensity. The greater the point cloud distance, the more... The smaller, The lower the value, the better to compensate for the curvature of sparse regions and achieve effective extraction of distant feature points;
[0082] Step 4-2, the improved feature point determination condition of this invention is:
[0083] (9)
[0084] (10)
[0085] (11)
[0086] In the formula, The number of effective neighborhood points participating in curvature calculation, The number of neighboring points when the distance is farthest. Using the base threshold The calculated maximum threshold. When the above three conditions are met, the point cloud can be determined to be a corner point; otherwise, the point cloud can be determined to be a planar point.
[0087] like Figure 7As shown, the cumulative distribution function (CDF) is further used to statistically analyze the absolute pose error of different algorithms to evaluate the positioning accuracy and robustness of each algorithm from the perspective of overall probability distribution. The CDF curve reflects the cumulative probability that the system achieves the corresponding positioning accuracy under a given error threshold. Therefore, the more left the curve is and the steeper the rise, the smaller the positioning error and the more stable the error distribution of the algorithm for most of the time. In the Street_04 outdoor street scene dataset, all algorithms can complete the overall trajectory estimation. However, due to the presence of a large number of repetitive building structures and long straight road segments in the scene, traditional methods are easily affected by point cloud geometric degradation, leading to an increase in cumulative error. As can be seen from the figure, the CDF curve of the OURS algorithm is located on the far left and quickly reaches a high cumulative probability within a small error range, indicating that the method of this invention maintains a low APE error for most of the time. In comparison, the LIO-SAM and LVI-SAM curves shift to the right, indicating a more pronounced cumulative drift phenomenon in complex, repetitive structural environments. While FAST-LIVO2 shows improvement over the former two, its overall error distribution is still slightly inferior to the method described in this invention. In the Walk indoor office dataset, due to the presence of numerous weak-texture areas, repetitive walls, and enclosed corridor structures, the SLAM system is more prone to local matching degradation and cumulative error propagation. Experimental results show that the OURS algorithm maintains the optimal CDF distribution characteristics, and its curve growth rate is significantly faster than other comparative algorithms. This demonstrates that the multi-line joint curvature calculation and adaptive threshold feature selection strategy proposed in this invention can effectively enhance feature stability in weak-texture environments, improve the quality of local constraints, and thus reduce the overall localization error. Combining the CDF results of the two datasets, it can be found that the method described in this invention not only effectively reduces the average localization error but also exhibits superior performance in terms of error distribution consistency and system robustness, verifying the effectiveness of the proposed degradation-aware feature extraction strategy in complex geometrically degraded scenarios.
[0088] The above description is merely the most basic specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any substitutions that can be understood by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.
Claims
1. An improved LVI-SAM method integrating multi-line joint curvature calculation and adaptive thresholding, characterized in that, Includes the following steps: Step 1: Construct an improved multi-sensor fusion SLAM system framework based on graph optimization, with the laser inertial subsystem (LIO) as the core and the visual inertial subsystem (VIO) as auxiliary compensation. At the laser inertial front end, for the distortion-reduced laser point cloud, a multi-line joint curvature calculation method and a Gaussian weighted curvature smoothing strategy are introduced to extract the initial curvature with three-dimensional spatial geometric features and suppress noise. On this basis, an adaptive curvature threshold dynamic adjustment strategy based on spatial distance and local point cloud density is introduced to complete the adaptive and accurate determination and classification of corner points and planar points. Finally, the extracted feature points are jointly constructed with visual reprojection constraints and IMU pre-integration constraints to construct a factor graph for tight coupling optimization and closed-loop correction, thereby improving the positioning accuracy and robustness of the system in environments with similar textures and repetitive structures. Step 2: To overcome the limitation of traditional single-line curvature calculation in failing to capture geometric changes in the direction of the vertical scan line, this invention proposes a multi-line joint curvature calculation method. This method directly applies to the distortion-free 3D laser point cloud, selecting the current scan line where the feature point is located and the point clouds of multiple adjacent scan lines to jointly construct a local 3D plane. Principal component analysis (PCA) is used to calculate the covariance matrix of the point cloud in the neighborhood and perform eigenvalue decomposition. The initial curvature of the point cloud is calculated using the physical mapping relationship between the minimum and maximum eigenvalues, thereby significantly enhancing the distinguishability and uniqueness of feature points in similar textures and geometrically sparse scenes. Step 3: To suppress the interference of measurement noise in the original point cloud on the stability of feature point extraction, this invention proposes a curvature smoothing method. After obtaining the initial curvature of the point cloud, a Gaussian filtering smoothing optimization step is introduced. A Gaussian weighted function is constructed with the spatial distance from the target feature point to each point cloud in the neighborhood as the independent variable. The weighted average of the initial curvature of all point clouds in the neighborhood is used as the final smoothed curvature value of the target point. While preserving the local spatial geometric change trend, isolated noise points are effectively eliminated, providing a reliable basis for subsequent feature selection. Step 4: To address the issue that fixed curvature thresholds are easily affected by the spatial distance and density unevenness of point clouds, leading to the loss of effective feature points at long distances, this invention proposes an adaptive curvature threshold adjustment strategy based on spatial distance and local density. The global curvature median of the entire effective point cloud is used as the basic threshold. A distance attenuation function is constructed based on the real-time spatial distance from each point to the sensor, and a density compensation function is constructed in combination with the number of effective local neighborhood points. An adaptive curvature threshold judgment standard for each point cloud is generated through dynamic modulation of two-factor product. Finally, the nonlinear classification judgment of feature points is achieved by combining the neighborhood density condition and the upper and lower bound conditions of the dynamic threshold.
2. The LVI-SAM improved method according to claim 1, which integrates multi-line joint curvature calculation and adaptive threshold, is characterized in that... Step 1 introduces a multi-line joint curvature calculation method and a Gaussian weighted curvature smoothing strategy at the laser inertial front end, and introduces an adaptive curvature threshold dynamic adjustment strategy based on spatial distance and local point cloud density to complete the adaptive and accurate determination and classification of corner points and planar points. The specific steps are as follows: Step 1-1: In the feature curvature extraction stage, the front end constructs a local spatial neighborhood for the 3D laser point cloud after distortion removal by the inertial measurement unit (IMU) using the current scan line and multiple vertically adjacent scan lines. This enables the network to simultaneously capture the complete 3D geometric structure change trend in both horizontal and vertical directions. To enhance feature discrimination and alleviate the interference of point cloud measurement noise on curvature calculation, principal component analysis and Gaussian weighted smoothing filtering are introduced in sequence to achieve noise suppression and smooth reconstruction of the initial curvature in two dimensions: spatial geometric mapping and neighborhood distance weight. This improves the stability of local curvature representation while maintaining efficient computation. Steps 1-2: In the feature selection and judgment stage, considering that traditional fixed curvature thresholds are prone to curvature distortion and spatial oversmoothing in long-range radar scanning or sparse local point cloud regions, especially in distant object surfaces and geometrically sparse regions, effective feature points are easily missed or lost, this invention designs an adaptive curvature threshold dynamic adjustment framework based on dual-factor modulation. This framework consists of two parts: a distance attenuation mechanism and a density compensation mechanism. The distance attenuation mechanism enhances the feature expression of distant weak geometric regions by displaying the perspective changes at near and far distances. The density compensation mechanism performs fine dynamic calibration of the feature judgment scale of different dense and sparse neighborhoods at the point-level scale, thereby improving the consistency of corner point and planar point classification and spatial recognition ability of the whole frame point cloud at different spatial scales. The overall framework effectively improves the problems of discontinuous feature extraction and matching failure in complex similar texture scenes while maintaining computational efficiency.
3. The LVI-SAM improved method according to claim 1, which integrates multi-line joint curvature calculation and adaptive threshold, is characterized in that... Step 2: For the distortion-free laser point cloud after IMU motion compensation, a multi-line joint curvature calculation method is used for each laser point. The initial curvature of the point cloud is calculated by principal component analysis, which fully considers the three-dimensional geometric features of the point cloud under similar texture environment and makes up for the defect that the traditional single-line curvature cannot reflect the geometric changes in the vertical scan line direction. The specific steps are as follows: For the distortion-free laser point cloud after motion compensation by the inertial measurement unit, for each laser point, select the left and right adjacent points on the laser scanning line where it is located, and introduce the point clouds of multiple adjacent laser scanning lines to jointly form a local three-dimensional planar neighborhood. For the distortion-free laser point cloud after IMU motion compensation, a multi-line joint curvature calculation method is used for each laser point. Local curvature is estimated based on principal component analysis, and the curvature of the neighborhood is then calculated. The covariance matrix of all points within the matrix is calculated as follows: （1） In the formula, For the neighborhood centroid, with respect to the covariance matrix Eigenvalue decomposition is performed as follows: （2） The magnitude of the eigenvalue corresponds to the degree of dispersion of the point cloud along the direction of the corresponding eigenvector. The largest eigenvalue represents the main direction of the point cloud distribution and has the highest degree of dispersion. The minimum eigenvalue represents the normal direction of the point cloud distribution, exhibiting the lowest degree of dispersion. Based on the physical meaning of eigenvalues, eigenvalues... With point The degree of surface variation is defined as: （3） The range of values for this curvature value is: ,when When it approaches 1, ,illustrate When the value approaches 0, the local point cloud approximates a point distribution; the more drastic the surface change, the more likely the target point is an edge feature point. When it approaches 0, Approaching 0 indicates that the local point cloud is approximately planar in distribution, with gentle surface changes, and the target point is a planar feature point; when When taking the median value, the local point cloud is approximately distributed in a linear shape, which is a line feature point. The multi-line joint curvature calculation fully considers the three-dimensional geometric features of the point cloud in a similar texture environment, making up for the defect that the traditional single-line curvature calculation cannot reflect the geometric changes in the vertical scan line direction, and greatly improving the ability to distinguish point cloud feature categories.
4. The LVI-SAM improved method according to claim 1, which integrates multi-line joint curvature calculation and adaptive threshold, is characterized in that... Step 3 is to suppress the influence of noise on curvature calculation. After obtaining the initial curvature, the weighted average of the curvature of all point clouds in the local neighborhood is used as the final curvature value of the point. This neighborhood curvature value is used for subsequent feature point judgment. The specific steps are as follows: For point Its smoothed curvature Defined as: （4） The contribution of different points in the current neighborhood to the curvature of the target point is determined by the distance between them; the closer the points are, the greater their weight, and vice versa. In the formula, the weights Gaussian weighting is usually used. （5） In the formula, Using bandwidth parameters to control the distance decay rate, this smoothing strategy effectively eliminates the interference of isolated noise points on curvature calculation while preserving the local geometric change trend, ensuring the stability and reliability of curvature values and providing accurate judgment criteria for subsequent feature point selection.
5. The LVI-SAM improved method according to claim 1, which integrates multi-line joint curvature calculation and adaptive threshold, is characterized in that... Step 4 proposes an adaptive curvature threshold. Based on the spatial distance from each point cloud to the sensor and the number of local effective neighborhood points, the curvature judgment threshold is dynamically adjusted to replace the original fixed threshold to complete the accurate classification of edge points and planar points, and to select effective feature points with high geometric significance and high repeatability. This solves the problem of feature extraction deviation caused by the density and distance differences of point clouds in the traditional fixed threshold. The specific steps are as follows: Step 4-1, Adaptive Curvature Threshold （6） In its formula, Based on the basic threshold, this paper statistically analyzes the curvature value distribution of the entire frame's effective point cloud and uses the global curvature median as the basic threshold for adaptive adjustment. For point Distance to the sensor, This represents the number of locally effective neighborhood points. and These are the distance decay function and the density compensation function, respectively, used to compensate for the problem of small curvature of point clouds at a distance and unstable estimation of point cloud curvature in sparse regions; （7） （8） In the formula, To control the distance decay rate, and These are distance normalization parameters; For reference neighborhood points, To control the density compensation intensity, the greater the point cloud distance, The smaller, The lower the value, the better to compensate for the curvature of sparse regions and achieve effective extraction of distant feature points; Step 4-2, the improved feature point determination condition of this invention is: （9） （10） （11） In the formula, The number of effective neighborhood points participating in curvature calculation, The number of neighboring points when the distance is farthest. Using the base threshold The calculated maximum threshold can be used to determine that the point cloud is a corner point if the above three conditions are met; otherwise, the point cloud can be determined to be a planar point. This invention achieves the coupling of local geometric information and spatial distribution information through multi-factor modulation, and forms a multi-level constraint relationship in the classification and determination of points, including neighborhood accumulation effect, curvature dynamic adjustment and anomaly point identification conditions. This mechanism emphasizes the conditional dependence and nonlinear control process of local features of point cloud, rather than the direct determination of a single static parameter, which effectively improves the accuracy and noise resistance of point cloud category detection and is suitable for 3D point cloud recognition tasks in complex scenarios.