Laser mapping method and system for isam2

Abstract

The application relates to the technical field of laser SLAM, and discloses a laser mapping method and system based on ISAM2, which comprises the following steps: acquiring lidar, inertial measurement unit and wheel odometer data, obtaining a point cloud sequence through tight coupling pre-integration motion distortion correction, adaptive voxel down-sampling and space-time feature fusion dynamic object marking; extracting track lines, tunnel profiles and column features from the point cloud sequence to construct a structured line feature descriptor sequence; calculating continuous frame relative poses and uncertainty covariance by using a multi-constraint fusion registration cost function; performing loop detection by using multi-level feature fusion scene descriptor and sequence consistency analysis multi-hypothesis tracking; eliminating accumulated errors by using ISAM2 incremental optimization and gradual loop factor activation; and outputting a three-dimensional point cloud map and an optimized trajectory by fusing point cloud confidence weighting. The application can realize quasi-real-time and high-precision laser mapping in a highly repetitive subway tunnel environment.

Description

Technical Field

[0001] This invention relates to the field of laser SLAM technology, and more specifically, to a laser mapping method and system for ISAM2. Background Technology

[0002] Subway tunnels are typical long and narrow structured environments with highly repetitive geometric features, such as parallel track lines, periodically arranged columns, and similar tunnel cross-sections. With the rapid development of urban rail transit, the need for high-precision 3D mapping of subway tunnels to support intelligent operation and maintenance applications such as structural health monitoring, track parameter measurement, and equipment asset management is becoming increasingly urgent. Laser SLAM technology, with its high precision and robustness, has become an important technical approach for subway tunnel mapping.

[0003] Existing laser SLAM methods face multiple technical challenges in subway tunnel scenarios. Firstly, the highly repetitive environment of subway tunnels makes it difficult for traditional loop closure detection methods based on point cloud geometric features to distinguish similar scenes at different locations, easily leading to erroneous loop closures or missed detections, resulting in global map distortion or inconsistencies. Secondly, subway tunnels are typically several kilometers or even tens of kilometers long, causing pose estimation errors to accumulate continuously during long-distance mapping. Traditional filter methods cannot effectively eliminate these accumulated errors, while batch optimization methods struggle to meet real-time requirements due to the rapidly increasing computational load with map size. Furthermore, the presence of dynamic objects such as moving trains and maintenance personnel within subway tunnels can interfere with the registration and mapping process; existing methods lack effective mechanisms for handling dynamic objects in subway scenes.

[0004] In summary, in the highly repetitive environment of subway tunnels, existing laser SLAM methods cannot simultaneously guarantee the accuracy of loop closure detection, the precision of long-distance mapping, and the real-time nature of the mapping process, and therefore cannot meet the actual needs of near real-time, high-precision laser mapping for large-scale subway tunnels. Summary of the Invention

[0005] This invention provides a laser mapping method and system for ISAM2, which solves the technical problems in related technologies such as insufficient accuracy of loop closure detection in highly repetitive environments such as subway tunnels, difficulty in eliminating long-distance cumulative errors, and difficulty in balancing real-time mapping and accuracy.

[0006] This invention provides a laser mapping method for ISAM2, comprising the following steps: S1: Acquire the raw scanning data stream of the lidar, the data stream of the inertial measurement unit, and the data stream of the wheel odometer. After preprocessing, obtain the point cloud sequence and quality evaluation indicators. S2, using structural feature extraction methods to identify structured geometric elements of the subway environment from point cloud sequences, and obtaining structured line feature descriptor subsequences; S3, acquire point cloud sequence and line feature descriptor sequence, calculate relative motion between consecutive frames and estimate pose uncertainty, and obtain the sequence of relative pose transformation matrix of consecutive frames, the sequence of uncertainty covariance matrix, cumulative odometry trajectory and registration quality mark. S4. Based on the line feature description subsequence, cumulative odometry trajectory and registration quality marker, identify events in which the robot revisits historical locations, and obtain the loop closure set and loop closure confidence covariance. S5: Obtain the sequence of relative pose transformation matrix, uncertainty covariance matrix, registration quality marker, loop closure set and loop closure confidence covariance of continuous frames. Eliminate the accumulated error through incremental optimization and constrained smoothing methods to obtain the global pose estimation sequence and pose covariance estimation. S6 receives the global pose estimation sequence, pose covariance estimation, point cloud sequence, and quality evaluation index, converts the point cloud into a unified coordinate system, and merges overlapping areas to obtain a 3D point cloud map and optimized trajectory.

[0007] In a preferred embodiment, S1 includes: A tightly coupled pre-integration method using an inertial measurement unit and a wheeled odometer is adopted. Taking the frame start time as the reference time, the relative pose transformation of each point in the point cloud is queried according to its scanning timestamp. The corresponding point is then transformed to the sensor coordinate system of the reference time to complete motion distortion correction. The point cloud space is divided into a three-dimensional voxel mesh. The voxel size is adaptively determined based on the local density. For voxels with local density higher than the average density, a larger voxel size is used to re-divide and select representative points. For voxels with local density lower than the average density, more points are retained. A point cloud segmentation method based on spatiotemporal feature fusion is used to label dynamic object point clouds. A sliding window is maintained to cache point clouds of multiple consecutive frames. Temporal correlation analysis is performed on the features of corresponding spatial locations in consecutive frames. Points whose position changes or feature changes exceed a preset dynamic judgment threshold are marked as dynamic candidate points. Cluster analysis and classification are performed on dynamic candidate points, and dynamic object probability and category label are assigned to each point.

[0008] In a preferred embodiment, S2 includes: Adaptive ground segmentation is performed on the preprocessed point cloud of the current frame. The vertical height distribution histogram of the point cloud is statistically analyzed, and the lowest peak value is identified as the ground height estimate. The multi-plane RANSAC algorithm is used to fit the main ground plane on the ground candidate point cloud. Points that are less than a preset distance threshold from the main ground plane are classified as ground point sets, and the remaining points are classified as non-ground point sets. Track line features are extracted from ground point clouds. Density spatial clustering is performed on the ground point clouds to identify track region clusters. For each track region cluster, Hough transform and RANSAC iterative fitting are combined to extract the track centerline. The parallelism of the two straight lines is verified and the track spacing is calculated. The confidence level of the track line features is calculated based on the number of inliers, fitting residuals, parallelism deviation and track gauge deviation. Tunnel contour features are extracted from non-ground point clouds. Cross-sectional point cloud slices perpendicular to the direction of travel are extracted at fixed intervals along the centerline of the track. For the two-dimensional contour point set of each slice, circular, elliptical and multi-segment circular arc models are tried for shape fitting. The model with the smallest residual is selected as the tunnel contour description.

[0009] In a preferred embodiment, S2 further includes: A column detection and sequence completion algorithm integrating geometric constraints and machine learning is adopted. Non-terrestrial point clouds are divided into multiple connected clusters using a 3D clustering algorithm. Point cloud clusters with a height-to-cross-sectional dimension ratio greater than a preset threshold are selected as candidate columns. Multidimensional feature vectors of geometric, shape, and contextual features are extracted from the candidate column clusters and input into a classifier for column identification. Candidate clusters with confidence scores higher than a preset confidence threshold are confirmed as detected columns. Spectral analysis is performed on the position sequences of detected columns to extract dominant periods. The locations of missing columns are predicted based on the dominant periods and marked as virtual columns. The track line parameters, tunnel contour parameters, column position sequence, and confidence scores of each feature are combined into a high-dimensional feature vector as the line feature descriptor for the corresponding frame. For features with low confidence or missing features, a temporal filtering method is used to complete and smooth them using feature values ​​from adjacent frames. The overall feature extraction quality score is calculated by comprehensively evaluating the confidence scores of each feature.

[0010] In a preferred embodiment, S3 includes: The initial registration values ​​are calculated by extracting the track centerline direction vector and the tunnel center position from the line feature descriptor subsequence. The initial rotation value is calculated by the rotation matrix of the track direction vectors in two frames, and the initial translation value is calculated by the displacement vector of the tunnel center position in two frames. A multi-constraint fusion registration cost function is constructed, which includes point-to-point distance, point-to-surface distance, track line direction consistency, and tunnel center alignment. Each constraint is dynamically weighted according to the confidence of its corresponding feature. A nonlinear optimization algorithm is used to iteratively solve the relative pose, and a robust kernel function is used to reduce the weight of corresponding points whose residuals exceed a preset residual threshold. The uncertainty covariance matrix of relative pose is calculated by analyzing the Hessian matrix of the cost function; the registration quality score is calculated by combining the registration convergence, final residual and inlier ratio, and the registration is marked as a low reliability observation if the quality score is lower than the preset quality threshold; the cumulative odometry trajectory is obtained by accumulating the relative pose transformation between consecutive frames.

[0011] In a preferred embodiment, S4 includes: A hierarchical historical feature database is constructed, and a dual index structure of time index and spatial index is established. The time index stores the line feature descriptor sequence in the order of frame timestamps, and the spatial index organizes the spatial location of historical frames using a KD tree structure based on the cumulative odometer trajectory. A coarse search is performed using a scene descriptor with multi-level feature fusion. Global structural features and local detail features are extracted from the current frame. Global structural features include the direction vector of the track centerline, the shape parameters of the tunnel cross section, and the distribution pattern of the pillars. Local detail features include the point cloud density distribution, the reflection intensity distribution, and the geometric shape descriptor. The global structural features and local detail features are adaptively weighted and fused according to stability and discriminability to construct a comprehensive scene descriptor. In the historical database, the global structural features are used for coarse screening, and the local detail features are used for fine screening. The frames with the highest similarity are selected as the candidate loopback frame set, while historical frames whose timestamps are less than the preset time interval threshold are excluded. Geometric consistency verification is performed on the candidate loopback frame set. The track spacing, tunnel radius and column spacing are compared item by item. Candidate frames whose differences in each parameter are less than the corresponding preset consistency threshold are verified. The similarity of column position sequences is compared using the dynamic time warping algorithm.

[0012] In a preferred embodiment, S4 further includes: For candidate frames that pass the precision verification, the ICP registration algorithm with line feature constraints is used to perform multiple optimizations starting from multiple initial poses. The result with the smallest residual is selected as the best registration. The single confidence of the candidate loop is calculated based on the registration residual, the number of inliers and the convergence. Loop candidates with confidence exceeding the preset loop confidence threshold are retained. The spatiotemporal consistency constraint is introduced to verify the loop closure. The path length is calculated from the current frame back along the cumulative odometry trajectory to the candidate frame. The spatial straight-line distance between the current frame and the candidate frame is calculated. The ratio of the path length to the straight-line distance is calculated. Only loop closures with a ratio within the preset range pass the spatiotemporal consistency check. At the same time, the spatial relationship consistency between the current candidate loop closure and the confirmed historical loop closure is checked. For ambiguous loops with multiple candidate frames, a multi-hypothesis tracking strategy based on sequence consistency analysis is adopted. In subsequent consecutive frames, the similarity between each hypothesis and the scene descriptor sequence of the corresponding historical region, the matching degree of the pillar position sequence, and the consistency of the tunnel contour parameter change trend are continuously calculated. An attention weight mechanism is introduced to give higher weight to the discriminative local features. The hypothesis with the highest overall consistency score and the difference between it and the second highest score is greater than the preset ambiguity judgment threshold is selected as the correct loop. If they cannot be distinguished, all hypotheses are rejected.

[0013] In a preferred embodiment, S5 includes: Initialize the factor graph and ISAM2 optimizer. The factor graph consists of variable nodes and factor nodes. The variable nodes represent the robot's 6-DOF pose at different times. Add prior factors to the pose variable nodes at the initial time to fix the initial pose. Incrementally add pose nodes and odometry factors. Add odometry factors between the new pose node and the previous pose node. The measurement value of the odometry factor is the corresponding relative pose transformation matrix in the sequence of relative pose transformation matrices of consecutive frames. The noise model is the corresponding uncertainty covariance matrix. The noise covariance of the odometry factor is dynamically adjusted according to the registration quality mark. The ISAM2 algorithm maintains the Bayesian tree representation of the factor graph. When a new factor is added, only the affected subset of variables is locally eliminated and updated. A progressive activation strategy is adopted to add closure factors. The closure factors connect the pose nodes at the current time step with the pose nodes at historical time steps. The newly added closure factors are initially given a large noise covariance. In subsequent optimization iterations, the noise covariance is gradually reduced so that the closure constraints are smoothly propagated to the global level. When the deviation between the linearized point of the variable node and the current estimated value exceeds the preset relinearization threshold, the corresponding node is relinearized.

[0014] In a preferred embodiment, S6 includes: The optimized pose is compared with the original pose. Pose nodes whose position or attitude changes exceed the preset pose change threshold are marked as needing to be updated. For point cloud frames that need to be updated, the robot body coordinate system is transformed to the global coordinate system according to the optimized pose. During the transformation, the position uncertainty of each point is calculated based on the pose covariance estimation. The global coordinate space is divided into voxels of fixed size to construct a spatial index structure. A confidence-weighted fusion strategy is used to process the point cloud in the overlapping region. For each point, a comprehensive confidence score is calculated based on distance confidence, angle confidence, frame quality confidence, and pose confidence. Multiple observation points within a voxel are weighted and averaged based on the comprehensive confidence score. Remove point clouds marked as dynamic objects from the global map, perform semantic segmentation based on the geometric features and location information of the point clouds, and output a 3D point cloud map and optimized trajectory.

[0015] In a preferred embodiment, an ISAM2 laser mapping system is used to perform the steps in the aforementioned ISAM2 laser mapping method, including: The preprocessing module is used to acquire the raw scanning data stream of the lidar, the data stream of the inertial measurement unit, and the data stream of the wheel odometer. After preprocessing, the point cloud sequence and quality evaluation index are obtained. The structural feature extraction module is used to identify the structured geometric elements of the subway environment from the point cloud sequence using structural feature extraction methods, and obtain a structured line feature descriptor sequence. The inter-frame registration module is used to acquire point cloud sequences and line feature descriptor sequences, calculate the relative motion between consecutive frames and estimate pose uncertainty, and obtain the sequence of relative pose transformation matrices, the sequence of uncertainty covariance matrices, the cumulative odometry trajectory and the registration quality markers for consecutive frames. The loop closure detection module is used to identify events in which the robot revisits historical locations based on line feature description subsequences, cumulative odometry trajectories, and registration quality markers, and to obtain the loop closure set and loop closure confidence covariance. The incremental optimization module is used to obtain the sequence of relative pose transformation matrix, uncertainty covariance matrix, registration quality marker, loop closure set and loop closure confidence covariance of continuous frames. It eliminates the cumulative error through incremental optimization and constrained smoothing methods to obtain the global pose estimation sequence and pose covariance estimate. The map fusion module receives global pose estimation sequences, pose covariance estimates, point cloud sequences, and quality assessment metrics. It then transforms the point clouds into a unified coordinate system and merges overlapping areas to obtain a 3D point cloud map and an optimized trajectory.

[0016] The beneficial effects of this invention are as follows: To address the highly repetitive environment of subway tunnels, a multi-level scene descriptor integrating global structural features and local detailed features is proposed. Combined with a multi-hypothesis tracking mechanism based on sequence consistency analysis, this effectively improves the accuracy of loop closure detection and reduces the false loop closure rate. Furthermore, a progressive loop closure factor activation strategy ensures that loop closure constraints are smoothly propagated globally, avoiding trajectory distortion during the optimization process. This effectively eliminates accumulated errors in long-distance subway tunnel mapping, keeping trajectory closure errors at a low level. By employing a dynamic object detection method that integrates spatiotemporal features, dynamic interference from moving trains and maintenance personnel within subway tunnels is effectively filtered out, ensuring the purity of the mapping data. Combined with the ISAM2 incremental factor graph optimization framework, the system only performs local updates on a subset of affected variables when new data arrives, reducing the computational complexity of optimization from a high-order batch method to near-linear. While maintaining centimeter-level map accuracy, this significantly shortens the overall mapping time, achieving near real-time high-precision laser mapping of large-scale subway tunnel environments. Attached Figure Description

[0017] Fig. 1 This is a flowchart of a laser mapping method for ISAM2 according to the present invention; Fig. 2 This is a flowchart of a laser mapping method for ISAM2 according to the present invention. Detailed Implementation

[0018] The subject matter described herein will now be discussed with reference to exemplary embodiments. It should be understood that these embodiments are discussed only to enable those skilled in the art to better understand and implement the subject matter described herein, and changes may be made to the function and arrangement of the elements discussed without departing from the scope of this specification. Various processes or components may be omitted, substituted, or added as needed in the examples. Furthermore, some features described in the examples may be combined in other examples.

[0019] At least one embodiment of the present invention discloses a laser mapping method for ISAM2, such as... Figs. 1-2 As shown, it includes the following steps: S1: Acquire the raw scanning data stream of the lidar, the data stream of the inertial measurement unit, and the data stream of the wheel odometer. After preprocessing, obtain the point cloud sequence and quality evaluation indicators. The lidar rotates continuously during the scanning process, collecting approximately 64,000 3D points per scan cycle. Each point contains spatial coordinates, reflection intensity, and a precise timestamp. Because the robot moves continuously within the scan cycle, the points collected at different times are in different spatial positions. Direct use of these points would cause point cloud distortion, necessitating motion distortion correction.

[0020] S1.1 Receive the raw point cloud frame output by the lidar at the current moment, record the start and end timestamps of the frame, and simultaneously acquire the acceleration and angular velocity measurement sequences output by the inertial measurement unit (IMU) and the velocity measurement sequence output by the wheel odometer within this time period. Perform time alignment and interpolation processing on the IMU data to ensure that each point cloud point has corresponding inertial measurement data at the scanning time.

[0021] S1.2, Establish a robot motion model for motion distortion correction. A tightly coupled pre-integration method using an inertial measurement unit (IMU) and a wheeled odometer is employed to estimate the robot's trajectory within the scan cycle. Using the frame start time as a reference time, the angular velocity is integrated to obtain the attitude change. Acceleration is rotated to the world coordinate system and then integrated to obtain the velocity and position changes. Combined with the velocity constraint from the wheeled odometer, an extended Kalman filter is used to fuse the IMU and wheeled odometer data to estimate the relative pose transformation between the reference time and the scan time of each point. For each point in the point cloud, the corresponding relative pose transformation is queried based on its scan timestamp. The point is then transformed from the sensor coordinate system at its scan time to the sensor coordinate system at the reference time, completing the motion distortion correction. After correction, all points in the point cloud are projected onto a unified reference time coordinate system, eliminating geometric deformations caused by motion.

[0022] S1.3, Adaptive voxel downsampling is performed to homogenize the point cloud density. The point cloud space is divided into a 3D voxel grid, and the voxel size is adaptively determined based on the local density of the point cloud. The 3D bounding box and the number of points in the point cloud are calculated, the average point density is estimated, and the space is divided into an initial voxel grid. For each voxel, the number of points within it is counted, and the local density is calculated. High-density voxels with a number of points higher than the average density are re-divided using a larger voxel size, and representative points are selected within these voxels using the centroid method or random sampling method; voxels with a number of points close to or lower than the average density use the initial voxel size or a smaller voxel size to retain more points and maintain details. Adaptive voxel size adjustment ensures that the downsampled point cloud maintains a relatively uniform density distribution at different distances and viewpoints, reducing the amount of data while preserving key geometric features.

[0023] S1.4 employs adaptive noise filtering based on local density statistics. For each downsampled point, a KD-tree data structure is used to quickly query its K nearest neighbors, and the average distance from that point to its K neighbors is calculated. The average distance distribution of all points is statistically analyzed, and the mean and standard deviation of the distribution are calculated. Points whose average distance exceeds the mean plus a certain multiple of the standard deviation are identified as outliers and removed. This method adaptively determines the noise threshold based on the local density characteristics of the point cloud, using a smaller distance threshold in high-density areas and a larger distance threshold in low-density areas, avoiding the inconsistent performance of fixed-threshold methods in different density regions.

[0024] S1.5, a spatiotemporal feature fusion point cloud segmentation method is used to label dynamic object point clouds. In subway tunnel environments, there may be moving trains, walking maintenance personnel, or temporarily placed equipment, etc. The point clouds of these objects can interfere with the mapping process, requiring identification and labeling. Considering the diverse shapes, frequent occlusion, and complex motion patterns of dynamic objects in subway scenes, a spatiotemporal feature fusion point cloud segmentation method is used for dynamic object detection. The system maintains a sliding window to cache the point cloud data of the most recent frames, such as 5 to 10 frames. The input to this method is a sequence of consecutive point cloud frames in the cache, each frame containing spatial coordinates, reflection intensity, and timestamp information. Spatial geometric features are extracted from each frame of point cloud. A multi-scale neighborhood analysis strategy is used to statistically analyze the local geometric features of the point cloud within different radii, including normal vectors, curvature, and density, capturing multi-level information from fine texture to overall shape. Temporal correlation analysis is performed on the features corresponding to the spatial positions in consecutive frames. By calculating the temporal changes in features, moving objects are identified. This step can distinguish between subtle jitter in a static background and real object movement. For each point in the current frame, its corresponding points in the spatial neighborhood are searched in the previous few frames. The positional and feature changes of the corresponding points are calculated, and points with significant positional or feature changes are marked as dynamic candidate points. Cluster analysis is performed on the dynamic candidate points, aggregating spatially continuous dynamic candidate points into dynamic object clusters. Based on the geometry, size, and motion pattern of the dynamic object clusters, they are classified as trains, people, temporary equipment, or static backgrounds. The classification process comprehensively considers features such as the height, width, length, speed, and direction of motion of the object clusters, using classifiers such as decision trees or support vector machines for discrimination. The dynamic object point cloud output by the classifier is combined with reflection intensity and spatial continuity for post-processing verification. Dynamic points with abnormal reflection intensity or spatial isolation are re-confirmed to avoid false detections. A dynamic object probability and category label are assigned to each point. Points with probabilities higher than a set threshold are excluded or given very low weights in subsequent processing. The category label is used to distinguish the processing strategies for different types of dynamic objects. This method, through the organic fusion of spatiotemporal features, reduces computational complexity while ensuring detection accuracy, meeting the needs of real-time mapping.

[0025] S1.6 transforms the processed point cloud to the robot's body coordinate system. Based on the installation position and attitude of the LiDAR relative to the robot body, i.e., the extrinsic parameter calibration matrix, the point cloud is transformed from the LiDAR coordinate system to the robot's body coordinate system. The extrinsic parameter calibration matrix is ​​obtained offline before system deployment using a calibration board method or a hand-eye calibration method and stored in the system configuration file.

[0026] S1.7, Evaluate the quality of the current frame's point cloud. A comprehensive quality score is calculated, taking into account factors such as the number of points, density uniformity, proportion of noisy points, proportion of dynamic object points, and interpolation error in motion distortion correction. Point clouds with sufficient point count, uniform density, low noise, few dynamic objects, and high motion distortion correction accuracy receive high-quality scores. The quality score is recorded and passed to subsequent steps for registration weight allocation and map fusion weight calculation.

[0027] After the above processing, a preprocessed point cloud sequence is obtained. Each frame of the point cloud contains corrected 3D coordinates, reflection intensity, dynamic object markers, and quality assessment scores, which serve as inputs for subsequent feature extraction and pose estimation.

[0028] S2, using structural feature extraction methods to identify structured geometric elements of the subway environment from point cloud sequences, and obtaining structured line feature descriptor subsequences; Subway tunnel environments exhibit distinct structured features, including parallel track lines on the ground, periodically arranged pillars, and regular tunnel cross-sections. These structured features provide strong geometric constraints, which can be used for pose estimation and loop closure detection. However, real-world environments present challenges such as track occlusion, missing pillars, and tunnel deformation, necessitating the design of robust feature extraction algorithms.

[0029] S2.1, Adaptive ground segmentation is performed on the preprocessed point cloud of the current frame. A histogram of the point cloud's height distribution along the vertical direction (Z-axis) is generated, with peaks corresponding to major horizontal planes such as the ground and platforms. The lowest peak is identified as the ground height estimate. A height tolerance range is set around this height, and points falling within this range are initially marked as ground candidate points. The multi-plane RANSAC algorithm is used to accurately fit the ground candidate point cloud. This algorithm allows fitting multiple planes to handle multiple horizontal structures such as the ground and platform edges. The RANSAC algorithm iteratively samples the minimum set of points randomly from the candidate points to fit a plane model, calculates the distance from other points to this plane, and counts the number of interior points. After multiple iterations, the plane with the most interior points is selected as the primary ground plane, and its plane equation parameters, normal vector, and distance are recorded. Points with a distance less than a distance threshold from the primary ground plane are classified as ground points, and the remaining points are classified as non-ground points. In the case of multiple horizontal planes, after extracting the primary ground plane, repeat the RANSAC process to extract secondary planes for the remaining candidate points until no valid planes can be extracted or the number of extracted planes reaches the upper limit.

[0030] S2.2 Robust extraction of track line features from ground point cloud. Subway tracks consist of two parallel rails, represented in the point cloud as two parallel straight line segments. Density spatial clustering is performed on the ground point cloud using the DBSCAN algorithm or a similar method, dividing the ground points into multiple clusters based on spatial proximity. Track areas typically form dense point cloud clusters due to laser reflection from the rails, while the point cloud in the track bed area between tracks is sparser. Possible track area clusters are identified by analyzing the spatial distribution and density of each cluster. For each track area cluster, an improved straight line detection algorithm is applied to extract the track centerline. This algorithm combines Hough transform and RANSAC iterative fitting. Hough transform accumulates straight line evidence in the parameter space to find possible straight line directions and positions, and then the RANSAC algorithm accurately fits the straight line parameters to the original point cloud. The RANSAC process effectively handles partial occlusion and outliers; even if debris on the track causes partial loss of rail point cloud data, the algorithm can still fit the correct straight line from the remaining points. The centerlines of the left and right tracks are extracted separately, obtaining the direction vectors of the two lines and the coordinates of the points they pass through. To verify the parallelism of two straight lines, calculate the angle between their direction vectors. If the angle is less than a parallelism threshold, the line is considered a valid track pair. Calculate the perpendicular distance between the two parallel lines as the track spacing; this distance should be close to the standard track gauge. Calculate the line connecting the midpoints of the two track centerlines as the track centerline; the direction vector of the track centerline is the estimated direction of the robot's movement. Based on factors such as the number of inliers involved in the line fitting, the fitting residual, parallelism deviation, and track gauge deviation, calculate the confidence level of the track line features. The confidence level is between 0 and 1; high confidence indicates reliable track line extraction, while low confidence indicates potential occlusion or inaccurate detection.

[0031] S2.3 Extracts tunnel contour features from non-ground point cloud. Tunnel cross-sections are typically circular, elliptical, or horseshoe-shaped; extracting cross-sectional contour parameters aids in environmental description and loop closure detection. Using the track centerline direction extracted in S2.2 as the robot's travel direction, cross-sectional point cloud slices perpendicular to the travel direction are extracted from the non-ground point cloud at fixed intervals (e.g., 1 meter) along this direction. For each slice, a local coordinate system is established, with the slice center as the origin, the travel direction as the Z-axis, the vertically upward Y-axis as the Y-axis, and the horizontal X-axis as the X-axis. The slice point cloud is projected onto the XY plane to obtain a two-dimensional contour point set. Shape fitting is performed on the two-dimensional contour point set, attempting various geometric models including circular, elliptical, and multi-segment circular arc models. The circular model uses a least-squares circle fitting algorithm to solve for the center coordinates and radius, minimizing the sum of the squared radial distances from all points to the circle. The elliptical model uses least-squares elliptical fitting to solve for the ellipse center, major axis, minor axis, and rotation angle. The multi-segment circular arc model divides the contour into upper and lower parts, fitting circular arcs to each part separately. Calculate the fitting residuals for each model, and select the model with the smallest residual as the tunnel contour description for that slice. Record the model type and parameters. For a circular model, the parameters are the center coordinates and radius; for an elliptical model, the parameters are the center coordinates, major axis, minor axis, and rotation angle. Statistically analyze the contour parameters of multiple slices, and calculate the average tunnel radius or major and minor axes as the tunnel contour features for that frame. Calculate the confidence level of the tunnel contour features based on the fitting residuals and the completeness of the slice point cloud.

[0032] S2.4 employs a column detection and sequence completion algorithm that integrates geometric constraints and machine learning. Within subway tunnels, column support structures are periodically arranged along the track direction. In the point cloud, these columns appear as vertically extending clusters of points with small cross-sectional areas. However, in real-world scenarios, columns exhibit various shapes such as cylindrical, square, and I-beam shapes, and are often obstructed by pipelines and equipment, resulting in incomplete point clouds. To address this challenge, a detection strategy combining geometric rules and machine learning is adopted. A 3D clustering algorithm is used for non-ground point clouds, dividing them into multiple connected clusters using Euclidean distance. For each point cloud cluster, its 3D bounding box is calculated, and the length-width-height ratio of the bounding box is analyzed. Typical characteristics of columns include a large height (large Z-axis dimension), a small cross-section (small XY-axis dimension), and a height-to-cross-section ratio exceeding a threshold, such as greater than 5. Point cloud clusters satisfying these geometric characteristics are selected as candidate columns. Multidimensional feature vectors are extracted from candidate column clusters. These features include geometric features such as height, cross-sectional area, aspect ratio, and verticality; shape features such as principal component analysis results of point cloud distribution and cross-sectional shape descriptors; and contextual features such as distance from the track centerline and spacing from adjacent candidate columns. The extracted feature vectors are input into a classifier for column identification. The classifier employs machine learning methods such as support vector machines or random forests, outputting column confidence and column type. The classifier learns diverse morphological features of columns through offline training, effectively distinguishing real columns from similarly shaped obstructions such as pipelines and equipment supports. It can still correctly identify columns with severe occlusion and incomplete point clouds. Candidate clusters with confidence scores higher than a set threshold are confirmed as detected columns, and their 3D center coordinates are recorded. The column center coordinates are projected onto the track centerline to obtain the column position sequence along the track direction. The spacing between adjacent columns is calculated, along with the average column spacing and standard deviation. Since columns may be completely occluded, leading to missed detections, a column sequence completion algorithm based on periodic pattern analysis is employed. This algorithm analyzes the detected column position and spacing sequences to identify periodic patterns and local variations in column distribution. Spectral analysis is performed on the column spacing sequence to extract the dominant period, and the theoretical column positions are predicted along the track centerline based on this dominant period. The deviation between the predicted and actual detected positions is corrected using Kalman filtering or particle filtering to adapt to complex situations such as uneven column spacing and varying spacing in curved areas. Predicted missing column positions are marked as virtual columns, and their detection is prioritized in subsequent frames. If no column is detected for several consecutive frames, the confidence level of the virtual column is reduced. The current frame combines the detected actual column positions and the predicted virtual column positions to form a complete column position sequence, with each column accompanied by a detection confidence level and type label. The overall confidence level of the column features is calculated based on factors such as the number of detected columns, spacing consistency, geometric shape matching, and classifier output confidence.

[0033] S2.5, Constructing a Comprehensive Line Feature Descriptor by Integrating Multimodal Features. Track line parameters, including the track centerline direction vector, track spacing, tunnel contour parameters (including tunnel radius or major and minor axes), pillar position sequence, and the confidence level of each feature, are combined into a high-dimensional feature vector as the line feature descriptor for that frame. The feature vector contains continuous values ​​such as direction, distance, and radius, and discrete values ​​such as pillar position indices. Continuous value features are normalized to ensure comparability of features with different dimensions. For frames with low feature extraction quality, i.e., those with low or missing feature confidence, temporal filtering is used for feature completion and smoothing. Using the feature values ​​of adjacent frames, temporal smoothing is performed through weighted averaging or Kalman filtering to estimate the missing or unreliable feature values ​​of the current frame. The weights of the temporal filter are determined based on the temporal distance and feature confidence of adjacent frames; frames that are closer in time and have higher confidence have higher weights. After temporal filtering, the continuity and stability of the feature sequence are enhanced, allowing for reasonable estimation even if feature extraction fails in individual frames, based on information from adjacent frames.

[0034] S2.6, Mark and output the feature extraction quality. The confidence levels of track lines, tunnel contours, and pillar features are comprehensively evaluated to calculate the overall feature extraction quality score for the current frame. The quality score reflects the reliability and completeness of the features in that frame. Features from high-quality frames can be used for high-confidence registration and loop closure detection, while features from low-quality frames are used with reduced weight in subsequent processing. Output a sequence of structured line feature descriptors. Each descriptor contains a feature parameter vector including the track centerline direction vector, track spacing, tunnel radius or major and minor axes, pillar position sequence, confidence levels for each feature, and an overall quality label for subsequent point cloud registration and loop closure detection.

[0035] S3, acquire point cloud sequence and line feature descriptor sequence, calculate relative motion between consecutive frames and estimate pose uncertainty, and obtain the sequence of relative pose transformation matrix of consecutive frames, the sequence of uncertainty covariance matrix, cumulative odometry trajectory and registration quality mark. Inter-frame pose estimation is fundamental to laser mapping. By registering consecutive point cloud frames and calculating relative motion, the robot's trajectory is accumulated. In the subway environment, the geometric constraints provided by line features can improve the accuracy and robustness of registration.

[0036] S3.1 Select the preprocessed point clouds and corresponding line feature descriptors of the current and previous frames. Extract the track centerline direction vector and tunnel center position from the line feature descriptors to calculate the initial registration value. The initial rotation value from the previous frame to the current frame is calculated using the rotation matrix of the track direction vectors of the two frames, and the initial translation value is calculated using the displacement vector of the track center position or tunnel center position of the two frames. This geometric initial value utilizes structured features and, compared to zero initial value or constant velocity model initial value, is closer to the true relative pose, which can accelerate the convergence speed of subsequent optimization and reduce the risk of getting trapped in local optima.

[0037] S3.2, Constructing a multi-constraint fusion registration cost function. Traditional point cloud registration algorithms such as ICP mainly rely on minimizing the geometric distance between points or between points and surfaces. This embodiment introduces a line feature constraint term to construct a unified optimization framework. The cost function includes four terms: point-to-point distance, point-to-surface distance, track line orientation consistency, and tunnel center alignment. The point-to-point distance term targets edge points with large curvature, extracting edge feature points in the current frame point cloud and finding the nearest neighbor in the previous frame point cloud, minimizing the Euclidean distance between corresponding point pairs. The point-to-surface distance term targets planar points with small curvature, extracting planar feature points in the current frame point cloud and fitting a local plane in the previous frame point cloud, minimizing the vertical distance from the point to the plane. The track line orientation consistency term constrains the track orientation vectors of the two frames to remain consistent after rotation transformation, penalizing track orientation deviations caused by rotation estimation. The tunnel center alignment term constrains the tunnel center positions of the two frames to be aligned after pose transformation, penalizing tunnel center offsets caused by translation estimation. Each constraint term is dynamically weighted based on the confidence level of its corresponding feature. A higher confidence level for the track line results in a heavier weight for the track direction constraint, a higher confidence level for the tunnel profile results in a heavier weight for the tunnel center constraint, and higher point cloud registration quality results in a heavier weight for the geometric distance constraint. Through dynamic weight allocation, the system can adaptively utilize reliable constraints and reduce the weight of unreliable constraints, thereby improving registration robustness.

[0038] Step S3.3 employs a nonlinear optimization algorithm to iteratively solve for the relative pose. The relative pose includes six degrees of freedom: rotation and translation. Rotation is represented using rotation matrices or quaternions, and translation using three-dimensional vectors. The optimization algorithm starts with initial geometric values ​​and iteratively updates the pose estimate to minimize the cost function. In each iteration, a point cloud correspondence is established based on the current pose estimate. For feature points in the current frame, the nearest neighbor point or nearest neighbor plane is quickly found in the previous frame using a KD-tree to establish point-to-point or point-to-plane correspondences. The residuals and Jacobian matrices for each constraint term are calculated. The residuals represent the constraint errors under the current pose, and the Jacobian matrix represents the partial derivatives of the residuals with respect to the pose parameters. The Gauss-Newton method or the Levenberg-Marquardt method is used to solve for the pose increment and update the pose estimate. During the iteration process, a robust kernel function is used to handle outliers. The residuals of each corresponding point are calculated, and the Hubel kernel function or Cauchy kernel function is used to reduce the weight of corresponding points with large residuals, avoiding excessive influence of outliers on the optimization results. The iteration terminates when the pose increment is less than the convergence threshold or the number of iterations reaches the upper limit. After optimization convergence, the final relative pose transformation matrix is ​​output.

[0039] S3.4, Estimate the covariance matrix of relative pose uncertainty. Registration uncertainty reflects the accuracy of pose estimation; uncertainties may differ across degrees of freedom. The Hessian matrix, which is the second derivative matrix of the cost function, is used to calculate the covariance of the parameter estimates. The inverse of the Hessian matrix is ​​the covariance matrix of the parameter estimates. The diagonal elements of the covariance matrix represent the variance of each degree of freedom, and the off-diagonal elements represent the correlation between different degrees of freedom. In linearized least squares problems, the Hessian matrix can be approximated as the product of the transpose of the Jacobian matrix and the Jacobian matrix. The calculated covariance matrix is ​​a 6x6 symmetric positive definite matrix, describing the uncertainty distribution of the six degrees of freedom of rotation and translation. In a subway environment, the track line and tunnel profile provide strong constraints; the lateral and vertical pose uncertainties are typically smaller than the forward uncertainties, and the covariance matrix reflects this anisotropic characteristic.

[0040] S3.5, Evaluate and label the quality of the current registration. Calculate the registration quality score by comprehensively considering factors such as registration convergence, final residual, inlier ratio, and feature constraint satisfaction. Registrations with fast iteration convergence, small residuals, high inlier ratio, and well-satisfied feature constraints are assigned a high-quality score; otherwise, they are assigned a low-quality score. Registrations with quality scores below a threshold are marked as low-reliability observations, and their weight is reduced by increasing the noise covariance in subsequent factor graph optimization. Registration failures, such as non-convergence of iterations or excessively large residuals, are skipped, or a constant-rate model is used to estimate a coarse pose, awaiting correction in subsequent global optimization.

[0041] S3.6, Accumulate relative poses to obtain the odometry trajectory. The relative pose transformations between consecutive frames are accumulated sequentially to obtain the accumulated pose estimates from the starting position to each time step, forming the odometry trajectory. The odometry trajectory is based only on local registration and has not undergone global optimization; therefore, errors accumulate as distance increases. However, it provides a coarse pose estimate for subsequent loop closure detection and global optimization.

[0042] S3.7 Output odometry observation data. The relative pose transformation matrix, uncertainty covariance matrix, and registration quality markers of each pair of consecutive frames are organized into an odometry observation sequence. At the same time, the cumulative odometry trajectory is output and passed to the loop closure detection module and the factor graph optimization module as the pose prior and odometry factor input for loop closure detection.

[0043] S4. Based on the line feature description subsequence, cumulative odometry trajectory and registration quality marker, identify events in which the robot revisits historical locations, and obtain the loop closure set and loop closure confidence covariance. Loop closure detection is used to identify events where a robot revisits a historical location. By establishing constraints between the current and historical locations, accumulated trajectory errors can be eliminated, achieving global consistency. The high repetitiveness of subway tunnels poses a significant challenge to loop closure detection. Similar track lines, tunnel contours, and periodic columns make it difficult for traditional geometric features to distinguish different locations, necessitating the design of multi-level verification mechanisms to eliminate erroneous loops.

[0044] S4.1, Construct a hierarchical historical feature database. Efficient retrieval of similar historical frames requires a dual-index structure: a temporal index and a spatial index. The temporal index stores line feature descriptors in frame timestamp order, recording the sequence number and timestamp of each frame. The spatial index organizes the spatial locations of historical frames using an octree or KD-tree structure based on the coarse location provided by the odometry trajectory, supporting fast spatial nearest neighbor queries. Whenever a new frame is processed, its line feature descriptors and odometry pose are added to the historical database, and the temporal and spatial indexes are updated.

[0045] S4.2 employs a multi-level feature fusion scene descriptor for coarse retrieval and screening of candidate loopback frames. Addressing the insufficient discriminative power of traditional geometric features due to the high repetition of subway tunnels, a scene descriptor fusing global structural features and local detail features is constructed. For the preprocessed point cloud of the current frame, global structural features are extracted, including the track centerline direction vector, tunnel cross-sectional shape parameters, and column distribution patterns. These features describe the overall geometric structure of the scene. Local detail features are extracted by dividing the point cloud into multiple local regions along the track direction and extracting feature descriptors for each region. Local features include point cloud density distribution, reflection intensity distribution, geometric shape descriptors such as fast point feature histograms, and obvious features such as pipeline joint locations, equipment marker locations, and wall texture variations. These local detail features have stronger discriminative power in repetitive environments because even with similar overall structures, local details often differ. The global structural features and local detail features are then weighted and fused to construct a comprehensive scene descriptor. The fusion weights are adaptively determined based on the stability and discriminative power of the features, assigning larger weights to highly discriminative local detail features and moderate weights to highly stable global structural features. Scene descriptors are represented by compact vectors with dimensions kept within a reasonable range to support efficient retrieval. The most similar candidate frames are retrieved from the historical database, which employs a hierarchical index structure. Coarse filtering is performed based on global structural features, followed by fine filtering based on local detail features, resulting in sublinear retrieval time complexity. The similarity between the current frame descriptor and historical frame descriptors is calculated, considering both global feature distance and local feature matching. The frames with the highest similarity are selected as the candidate loop closure frame set. Simultaneously, a time interval constraint is applied to exclude historical frames with timestamps too close to the current frame, avoiding misclassification of short-term continuous motion as loop closure. Traditional line feature descriptors are used for auxiliary filtering, extracting geometric parameters such as the track centerline direction vector, track spacing, and tunnel radius of the current frame. Geometric feature distances between candidate frames are calculated, and candidate frames with excessively large geometric feature differences are given lower priority. This multi-level feature fusion and hierarchical retrieval strategy leverages the strong discriminative power of local detail features while preserving the robustness and interpretability of global structural features.

[0046] S4.3, Perform a fine-grained geometric consistency check. For each frame in the candidate loopback frame set, a detailed geometric parameter comparison is performed. Numerical parameters such as track spacing, tunnel radius, and column spacing are compared item by item, and the absolute or relative differences of each parameter are calculated. Consistency thresholds are set for each parameter; for example, the difference in track spacing must be less than 0.05 meters, the difference in tunnel radius less than 0.1 meters, and the difference in column spacing less than 0.2 meters. Only candidate frames whose differences in all key parameters are less than the corresponding thresholds pass the geometric consistency check. Pattern matching is performed on the column position sequences, using a dynamic time warping algorithm to compare the column sequences of the current frame and candidate frames. Dynamic time warping allows for non-linear scaling of sequences on the time axis, handling omissions or redundancies in column detection, and calculating the similarity score between the two sequences. Candidate frames with column sequence similarity higher than the threshold are retained. After fine-grained verification, the number of candidate frames is significantly reduced, typically leaving only a few high-confidence candidates.

[0047] S4.4 Perform final confirmation and verification of point cloud registration. For candidate frames that have passed the fine verification, extract the original preprocessed point clouds of the current frame and the candidate frames, and use the ICP registration algorithm with line feature constraints to calculate the relative pose transformation between the two frames. The registration process is similar to S3, but due to the long time interval between the candidate frame and the current frame, the odometry cumulative error is large, and the initial pose estimation is inaccurate, requiring a larger search range and more iterations. The registration algorithm starts from multiple different initial poses, performs multiple optimizations, and selects the result with the smallest final residual as the best registration. The single-step confidence of the candidate loop is calculated based on the registration residual, the number of inliers, and the registration convergence. Loops with small residuals, many inliers, and good convergence receive high confidence, while those with low confidence receive low confidence. Only loop candidates with confidence exceeding the threshold are retained for the next verification step.

[0048] S4.5 introduces spatiotemporal consistency constraints to verify loop closures. High confidence from a single registration does not guarantee the correctness of the loop closure; in extremely similar environments, incorrect loop closure registration may still yield low residuals. A spatiotemporal consistency check is introduced, utilizing constraints provided by confirmed historical loop closures and odometry trajectories to verify the consistency of the current candidate loop closure. Tracing back from the current frame along the odometry trajectory to the candidate frame, the path length provided by the odometry is accumulated; this path length represents the actual distance traveled by the robot. Based on the odometry poses of the current and candidate frames, the straight-line distance between them in space is calculated. When the loop closure is correct, the path length should be greater than the straight-line distance, because the robot travels along tunnel curves or turns, and the travel path is longer than the straight-line distance in space. The ratio of path length to straight-line distance is calculated. If the ratio is close to 1, it indicates that the two are almost at the same position and should not constitute a loop closure; if the ratio is too large, it indicates that the path is too tortuous, possibly indicating an incorrect loop closure or excessive odometry error. A reasonable ratio range is set; only loop closures with ratios within this range pass the spatiotemporal consistency check. If other loopbacks have been confirmed historically, check the consistency of the spatial relationship between the current candidate loopback and the confirmed loopbacks. The odometry path can estimate the relative positional relationship between the current frame and each historical loopback frame. These relationships should be consistent with the implicit spatial relationship of the candidate loopback. If there is a contradiction, the candidate loopback is rejected.

[0049] S4.6, applying sequence consistency analysis with multiple hypothesis tracking to handle ambiguous loops. In highly repetitive straight tunnel sections, multiple candidate frames may simultaneously pass all the aforementioned verifications. These frames exhibit high feature similarity, almost identical geometric parameters, and very low registration residuals, and spatiotemporal consistency cannot be explicitly ruled out. In this case, a sequence consistency analysis with multiple hypothesis tracking strategy is employed to temporarily retain these ambiguous loop candidates, not immediately adding them to the factor graph, but continuously monitoring and verifying them in subsequent frames. For each ambiguous loop hypothesis, its sequence consistency score with the corresponding historical region is continuously calculated in several consecutive subsequent frames, accumulating consistency evidence. The sequence consistency analysis method is as follows: The current sequence, consisting of the current frame and several subsequent frames, extracts features such as scene descriptors, pillar position sequences, and tunnel contour parameters from each frame to form the current feature sequence. For each hypothesis's corresponding historical region, features from the corresponding historical frame sequence are extracted to form the historical feature sequence. The similarity between the current feature sequence and the historical feature sequence is calculated, taking into account multiple dimensions. The similarity of scene descriptor sequences is calculated by aligning two sequences using a dynamic time warping algorithm and calculating the distance between descriptors in each frame after alignment; a smaller distance indicates higher scene similarity. The matching degree of the pillar position sequences is calculated by comparing the relative positional relationships and spacing patterns of the pillars in the two sequences; a high matching degree indicates consistent pillar distribution. The consistency of the changing trends of the tunnel contour parameter sequences is calculated by analyzing the changing trends of the tunnel radius or major and minor axes in the two sequences; consistent trends indicate similar tunnel shape changes. An attention weighting mechanism is introduced, assigning different weights to features at different locations in the sequences. Discriminating local features, such as specific pillar absences, local tunnel deformations, and equipment markings, are given higher weights, as these features contribute more to hypothesis verification. Attention weights are automatically determined by analyzing the variance and uniqueness of the features; features with large variance and high uniqueness have higher weights. Combining the above multi-dimensional similarity, a comprehensive consistency score for each hypothesis is calculated. After accumulating several frames, the hypothesis with the highest consistency score, higher than other hypotheses, is selected as the correct loop closure. If the difference between the highest-scoring hypothesis and the second-highest-scoring hypothesis is less than a set threshold, it indicates that ambiguity still exists, and the multiple-hypothesis state continues, awaiting more frames of information. If the loops remain indistinguishable across multiple consecutive frames, all hypotheses are rejected to avoid introducing erroneous loops. This strategy effectively improves the accuracy of identifying ambiguous loops through sequence-level consistency analysis and an attention weighting mechanism.

[0050] S4.7 Calculate the confidence covariance and consistency score for the confirmed loop closures. For all validated confirmed loop closures, calculate the uncertainty covariance matrix of the loop closure relative pose based on the registration residuals and the Hessian matrix during the registration process. This covariance matrix describes the accuracy of the loop closure measurement. A comprehensive consistency score is calculated by combining the geometric consistency score, pillar sequence similarity, and spatiotemporal consistency verification results. A higher score indicates a more reliable loop closure. The confidence covariance and consistency score will be used to set the weights of the loop closure factor in factor graph optimization.

[0051] S4.8 outputs a highly reliable loop closure set. All confirmed loop closures are organized into a loop closure set, with each loop containing the current frame index, historical frame index, relative pose transformation matrix, confidence covariance matrix, and consistency score. This set is then passed to the factor graph optimization module as input to the loop closure factor.

[0052] S5: Obtain the sequence of relative pose transformation matrix, uncertainty covariance matrix, registration quality marker, loop closure set and loop closure confidence covariance of continuous frames. Eliminate the accumulated error through incremental optimization and constrained smoothing methods to obtain the global pose estimation sequence and pose covariance estimation. A factor graph is a graphical model used to represent the factorization of probability distributions, and in SLAM, it is used to represent the pose estimation problem. The ISAM2 algorithm uses a Bayesian tree data structure to incrementally optimize the factor graph, only updating the affected variables locally when new data arrives, thus improving computational efficiency.

[0053] S5.1 Initialize the factor graph and ISAM2 optimizer. The factor graph consists of two types of nodes: variable nodes and factor nodes. Variable nodes represent the state variables to be estimated; in this embodiment, they represent the robot's 6-DOF poses at different times. Each pose includes 3D position and 3D attitude, represented using rotation matrices or quaternions. Factor nodes represent observation constraints, connecting one or more variable nodes and encoding the observation equations and noise models. At the initial time step, the first pose variable node is created, and prior factors are added to it. The prior factors specify the initial estimate and high-confidence covariance matrix of the pose. Typically, the initial position is set to the origin, the attitude to unit rotation, and the covariance to a minimum value to fix the initial pose. The ISAM2 optimizer is initialized, setting optimization parameters such as the relinearization threshold, partial relinearization strategy, and factorization method.

[0054] In step S5.2, pose nodes and odometry factors are added incrementally. As the robot moves and the point cloud is registered, new odometry observations are acquired sequentially. At each new time step, a new pose variable node is added to the factor graph. The initial estimate of the node is obtained by accumulating the pose and odometry relative pose from the previous time step. An odometry factor is added between the new pose node and the previous pose node. The odometry factor is a binary factor connecting two adjacent pose nodes, and its measurement value is the relative pose transformation matrix output from S3. The noise model is the corresponding uncertainty covariance matrix. Based on the registration quality label in S3, the noise covariance of the odometry factor is dynamically adjusted. High-quality registration corresponds to a small covariance (high weight), while low-quality registration corresponds to a large covariance (low weight). Adaptive weight allocation enables the system to automatically reduce the impact of unreliable observations and improve the robustness of the global estimation.

[0055] S5.3, ISAM2 Incremental Optimization Updates Pose Estimation. The ISAM2 algorithm maintains a Bayesian tree representation of the factor graph. A Bayesian tree is a tree-like data structure where variable elimination transforms the factor graph into a tree, with nodes corresponding to variables and edges corresponding to conditional probabilities. When a new odometry factor is added, the ISAM2 algorithm performs an incremental update, identifying the subset of variables affected by the new factor and performing local elimination and updates only on these variables. The addition of a new factor affects the variable nodes directly connected to it and their ancestor nodes in the Bayesian tree, requiring these nodes to be re-eliminated and factorized. The ISAM2 algorithm updates the estimates and covariances of these nodes through local tree reconstruction, while unaffected variable nodes remain unchanged and do not require recalculation. This incremental update mechanism reduces the computational complexity from quadratic or cubic in batch optimization to near linear, enabling real-time optimization. After each incremental update, ISAM2 outputs the latest estimates and covariance estimates for all current pose variables.

[0056] S5.4 introduces a progressive addition of closure factors to smooth the optimization process. When a new closure is detected in S4, its confidence and consistency score are evaluated. Only closures with high confidence and passing consistency verification are added to the factor graph. The closure factor is a binary factor connecting the pose node at the current moment to the pose node at a historical moment. The measured value is the relative pose transformation matrix of the closure output by S4, and the noise model is the corresponding confidence covariance matrix. The addition of the closure factor breaks the chain structure of the factor graph, forming a loop and providing global constraints. To avoid optimization oscillations caused by the sudden addition of closure factors, a progressive activation strategy is adopted. Newly added closure factors are initially assigned a large noise covariance, i.e., a low weight. In subsequent optimization iterations, the noise covariance is gradually reduced and the weight is increased, so that the influence of the closure constraint is smoothly propagated globally, avoiding trajectory distortion or optimization instability caused by sudden large adjustments. The progressive process can be designed to decay linearly or exponentially. After several iterations, such as 5 to 10, the weight of the closure factor reaches the value corresponding to its true confidence.

[0057] S5.5, ISAM2 global optimization propagates loop closure constraints. After adding the loop closure factor, the ISAM2 algorithm identifies a subset of variables affected by loops. Since loops connect two poses that are far apart in time, the impact typically involves all intermediate poses between the historical pose and the current pose. ISAM2 propagates the loop closure constraints to these variables through Bayesian tree re-elimination, adjusting their estimates to satisfy the loop closure constraints. The propagation process is essentially solving a large-scale nonlinear least squares problem, which ISAM2 solves efficiently through incremental factorization and reverse substitution. After optimization, the historical trajectory undergoes global adjustment, accumulated odometry errors are distributed and corrected, and the global consistency of the trajectory is improved.

[0058] S5.6, Relinearization handles accumulated linearization errors. Nonlinear optimization typically employs iterative linearization methods, linearizing the nonlinear problem at the current estimation point in each iteration and solving the linearized problem to obtain the updated direction. When the estimated value is far from the linearized point, linearization errors accumulate, affecting optimization accuracy. ISAM2 internally maintains the linearized point and current estimated value for each variable node and calculates the deviation between them. When the deviation exceeds a set relinearization threshold, the node is marked as needing relinearization. The relinearization process recalculates the Jacobian matrix at the node and updates the factors related to that node in the Bayesian tree, improving linearization accuracy. Relinearization is a computationally expensive operation; it is only performed on nodes with significant accumulated errors to balance computational efficiency and optimization accuracy.

[0059] S5.7 outputs the optimized global pose estimate and covariance. The ISAM2 algorithm outputs the optimal estimates and covariance estimates for all pose variables after each update. The pose estimation sequence constitutes the robot's global motion trajectory. The trajectory after loop closure optimization has global consistency, and accumulated errors are effectively suppressed. The pose covariance reflects the uncertainty of each pose estimate; after loop closure constraints, the uncertainty of historical poses is usually reduced. Convergence metrics of the optimization process are recorded, such as the number of optimization iterations, the final residual, and the satisfaction of loop closure constraints, to evaluate the optimization quality. The optimized pose sequence and covariance are then passed to the map fusion module for point cloud coordinate transformation and map construction.

[0060] S6 receives the global pose estimation sequence, pose covariance estimation, point cloud sequence and quality evaluation index, converts the point cloud into a unified coordinate system and merges overlapping areas to obtain a 3D point cloud map and optimized trajectory. A global map is a stitched image of all point cloud frames in a unified coordinate system, providing a data foundation for applications such as tunnel deformation monitoring and track measurement. After ISAM2 optimizes and updates the pose, the point cloud needs to be converted to the new pose to rebuild the map. The incremental map update strategy avoids the high cost of global reconstruction.

[0061] S6.1 maintains pose change records and identifies local map regions that need updating. After ISAM2 optimization, it outputs estimates of all current poses. The optimized poses are compared with the original poses, and the change in each pose is calculated, including position and orientation changes. Position changes are measured by the Euclidean distance between the two positions, and orientation changes are measured by rotation angle or quaternion distance. A pose change threshold is set; pose nodes with position changes exceeding 0.1 meters or orientation changes exceeding 1 degree are marked as having significant changes. For pose nodes with significant changes, their corresponding point cloud frames require re-coordinate transformation and map fusion. For pose nodes without significant changes, their corresponding local maps remain unchanged, requiring no reprocessing, thus achieving incremental updates.

[0062] S6.2 performs coordinate transformation on the point cloud frames that need updating. For each point cloud frame marked as needing updating, its optimized pose (position and orientation) is obtained. Point cloud data for the corresponding frame is extracted from the preprocessed point cloud sequence output from S1, and each point in that frame's point cloud is transformed from the robot's body coordinate system to the global coordinate system. The transformation formula is: global coordinates equal to the rotation matrix multiplied by the body coordinates plus the translation vector. The pose uncertainty covariance is considered during the transformation, and the position uncertainty for each point is calculated. The global position uncertainty of a point is determined by the point's position in the body coordinate system, the covariance of its pose, and the Jacobian matrix of the transformation, and is calculated using the error propagation formula. Points with high position uncertainty have low confidence in subsequent fusion.

[0063] S6.3, Constructing the Spatial Index Structure of the Global Point Cloud Map. Efficiently handling point cloud fusion in overlapping areas requires organizing the global map using a 3D voxel mesh or octree structure. The global coordinate space is divided into voxels of fixed size, each storing the point cloud data falling within it. The voxel size is set according to the required map accuracy, such as 0.05 meters or 0.1 meters. After a new point cloud frame is transformed to the global coordinate system, its points are inserted into the corresponding voxels. Voxels already containing points undergo point cloud fusion operations.

[0064] S6.4 employs a confidence-weighted fusion strategy to process point clouds in overlapping regions. The same spatial location in an overlapping region may be observed from multiple frames of point clouds, with varying quality and reliability. Weighted fusion is necessary to generate a high-quality map. A comprehensive confidence score is calculated for each point, determined by the following factors: Distance confidence score is calculated based on the distance from the point to the sensor. LiDAR measurement accuracy is higher at close range than at long range, and distance confidence score decreases with increasing distance, typically using an exponential decay model or a piecewise linear model. Angle confidence score is calculated based on the laser incident angle, which is the angle between the laser beam direction and the point's surface normal vector. Perpendicular incidence results in the highest angle confidence score, decreasing as the incident angle increases due to increased uncertainty in point location caused by oblique incidence. The surface normal vector is estimated by fitting the point's local neighborhood plane. Frame quality confidence score is derived from the point cloud quality evaluation index in S1; points in high-quality frames receive high confidence scores. Pose confidence score is derived from the pose covariance estimation in S5; points in frames with low pose uncertainty receive high confidence scores. The overall confidence score is calculated by weighted product or weighted summation of the above components. Multiple observation points within a voxel are weighted averaged based on their respective overall confidence scores. The location coordinates of the fused point are the weighted average of the locations of all observation points, and the color or reflectance intensity of the fused point is the weighted average of the attributes of all observation points. Weighted fusion ensures that high-quality observations dominate the fusion result, while low-quality observations are appropriately suppressed, resulting in a smooth and continuous map surface that avoids point cloud jumps and inconsistencies.

[0065] S6.5 performs post-processing optimization on the global map. Voxel filtering is applied to reduce map data volume. For fusion points within each voxel, if the number of points is still large, the voxel center point or weighted center point is used as the representative point to reduce redundant point clouds. Point clouds marked as dynamic objects are removed; these point clouds are marked in S1 and excluded during map construction to prevent dynamic objects from polluting the static map. Map semantic segmentation is performed, classifying map points into different semantic categories such as ground, tracks, pillars, tunnel walls, ceilings, and equipment based on the geometric features and location information of the point clouds. Semantic segmentation can use rule-based methods or machine learning methods to provide semantic information for subsequent applications. Holes or incomplete areas in the map are detected and repaired, using interpolation or surface reconstruction methods to fill in missing areas and improve map integrity.

[0066] Section S6.6 evaluates global map quality. Multiple quality metrics are calculated to assess map accuracy and usability. Trajectory closure error assesses the effectiveness of loop closure optimization. For detected loops, the distance between trajectory points passing the same location twice before and after the loop is calculated; a smaller distance indicates a smaller closure error and better trajectory consistency. The closure errors of all loops are statistically analyzed, and the average and maximum values ​​are calculated. Map overlap area consistency assesses map fusion quality. For overlapping areas observed across multiple frames, the point cloud distances between different observations before fusion are calculated; smaller distances indicate better observation consistency and higher fusion quality. Map integrity assesses map coverage by calculating the spatial distribution of point clouds in the map, checking for large-area gaps, and calculating the coverage metric. Map resolution and density assess map detail by statistically analyzing the average density and minimum resolvable feature size of point clouds in the map. High density and high resolution indicate that the map can capture detailed features. Based on these metrics, a map quality assessment report is generated, providing users with a quantitative basis for map reliability.

[0067] S6.7 Outputs the final mapping results. A globally consistent, high-precision 3D point cloud map is stored in a standard format such as PCD, PLY, or LAS. The point cloud includes attributes such as the 3D coordinates, color or reflection intensity, and semantic labels for each point. The optimized robot motion trajectory is output, recording the pose at each moment in a time-series format, such as TUM trajectory format, for easy subsequent analysis and visualization. Pose uncertainty estimation is output, recording the covariance matrix of each pose to provide a basis for trajectory reliability assessment. A map quality assessment report is output, presenting various quality indicators in text or tabular form to help users understand the map's accuracy and applicability. The mapping results can be used for subsequent tunnel deformation monitoring, detecting changes in tunnel structure by comparing maps from different periods; for track geometry parameter measurement, extracting track lines from the map and calculating parameters such as track gauge and direction; and for equipment asset management, marking equipment locations and statuses on the map to establish a digital asset ledger.

[0068] In one embodiment of the invention, a practical application test was conducted in a tunnel section of Metro Line 5 in a certain city. This tunnel is a double-track tunnel, approximately 4.8 kilometers long, containing a curve with a large curvature and several small curvature undulations. Periodic columns are arranged within the tunnel, with a spacing of approximately 15 meters between them. A track inspection robot equipped with a 64-line LiDAR was used for data collection. The robot traveled along the track at a variable speed of 1.2 to 1.8 meters per second, completing a single scan in approximately 50 minutes, collecting approximately 30,000 point cloud frames, with a raw point cloud data volume of approximately 180 gigabytes.

[0069] Application Scenario Introduction: This invention focuses on the application of intelligent operation and maintenance management of urban rail transit infrastructure. As a key component of rail transit, subway tunnels require regular monitoring and assessment of their structural safety status. Traditional manual inspection methods rely on personnel carrying instruments and walking or riding in engineering vehicles within the tunnel to conduct measurements, which suffers from low efficiency, high cost, discontinuous data, and significant personnel safety risks. The ISAM2 laser mapping method proposed in this invention enables autonomous inspection robots to rapidly collect high-precision 3D point cloud data of the entire tunnel area, constructing a digital twin map. This allows for functions such as tunnel deformation monitoring, automatic measurement of track geometric parameters, and recording of equipment and facility locations, providing data support for intelligent subway operation and maintenance.

[0070] The point cloud data acquired during the data acquisition phase is processed using the method of this invention. The point cloud preprocessing phase completes motion distortion correction and downsampling, significantly reducing the amount of point cloud data after processing. In the dynamic object detection phase, the spatiotemporal feature fusion method successfully identified and filtered multiple train passing events and maintenance personnel appearance events, demonstrating good dynamic object detection performance and meeting real-time requirements. In the structured feature extraction phase, track line features were successfully extracted, and the track spacing measurement value of 1.435 meters conforms to the standard track gauge. The tunnel cross-section fitting results show that the tunnel radius is between 5.2 and 5.5 meters. In the column detection phase, the detection algorithm fusion of geometric constraints and machine learning detected approximately 320 columns, detecting more partially occluded columns compared to traditional geometric methods, improving the column detection recall rate, and achieving an average column spacing of 14.9 meters. In the inter-frame registration phase, the average registration time is 0.15 seconds per frame, with a high registration success rate. A few frames failed registration due to poor point cloud quality or interference from dynamic objects and were skipped using a constant velocity model. In the loop closure detection phase, the scene descriptor method based on multi-level feature fusion detected 12 valid loops throughout the tunnel scanning process, mainly distributed in tunnel curves and undulating areas, where the environmental features are relatively unique. In highly repetitive straight sections, ambiguous loops were encountered. The multi-hypothesis tracking method based on sequence consistency analysis successfully identified the correct loops from multiple candidates, achieving a higher accuracy rate for ambiguous loop discrimination than traditional methods. The overall accuracy of loop closure detection was high, with a low false loop rate. In the ISAM2 optimization phase, global optimization was triggered after loop detection. Each global optimization took an average of 2.3 seconds, and the trajectory closure error decreased from a maximum of 4.2 meters before optimization to 0.28 meters after optimization. The map fusion phase employed an incremental update strategy, reconstructing only the local map areas affected by loops. The map update time was approximately 1.5 times the global optimization time, making the overall mapping process nearly real-time.

[0071] The final generated 3D point cloud map covers the entire 4.8-kilometer tunnel, with a point cloud density of approximately 2,000 points per square meter and a map file size of approximately 12 gigabytes. Track geometry parameters were automatically extracted from the map, and the measured parameters such as track gauge, alignment, and elevation showed good consistency with results from traditional measurement methods. By comparing maps collected at different times, millimeter-level settlement deformation was detected at certain locations in the tunnel, providing quantitative evidence for tunnel health assessment. All pillars, lighting equipment, ventilation equipment, signaling equipment, etc., within the tunnel were marked on the map, establishing a digital ledger of equipment assets with centimeter-level location accuracy.

[0072] To verify the performance advantages of the method of this invention, comparative experiments were conducted with traditional batch optimization methods and filtering methods. As shown in Tables 1 and 2, the performance indicators of different methods are compared.

[0073] Table 1. Performance Comparison of Different Mapping Methods

[0074] Table 2. Success rate of feature extraction in different segments

[0075] The results show that the method of this invention improves computational efficiency and loop closure detection accuracy while maintaining centimeter-level map accuracy, with trajectory closure error controlled within 0.3 meters, outperforming traditional batch optimization and filtering methods. The multi-level loop closure detection mechanism effectively improves loop closure detection accuracy, avoiding optimization failures caused by erroneous loops, especially in highly repetitive straight tunnel sections, where its ability to distinguish ambiguous loops is superior to traditional methods. The spatiotemporal feature fusion method achieves good dynamic object detection results, effectively filtering dynamic interference such as trains and personnel, avoiding the impact of dynamic objects on mapping accuracy. The column detection algorithm, which integrates geometric constraints and machine learning, improves column detection recall, especially for incomplete columns obscured by pipelines and equipment. By learning the diverse morphological features of columns, it can correctly identify them even with incomplete point clouds. The scene descriptor, through multi-level feature fusion, extracts global structural features and local detail features, exhibiting superior discrimination ability in repetitive environments compared to traditional geometric features. The feature extraction algorithm maintains a high success rate in different tunnel sections, and even in complex sections with occlusion, the overall feature quality still meets mapping requirements. The ISAM2 incremental optimization framework reduces the time for a single optimization cycle compared to batch optimization, significantly shortening the overall mapping time and achieving near real-time mapping. The generated high-precision 3D maps have been successfully applied to tunnel deformation monitoring and track parameter measurement, validating the practical value of the method presented in this invention.

[0076] This invention, through systematic design, combines the ISAM2 incremental optimization framework with the structured feature extraction of the subway environment. It innovatively introduces methods such as spatiotemporal feature fusion, multi-level feature fusion, and sequence consistency analysis to solve key challenges such as dynamic object detection, column recognition, and loop closure detection. For the highly repetitive scenarios in subway tunnels, it designs a scene descriptor that integrates global structure and local details, as well as a multi-hypothesis tracking mechanism for sequence consistency analysis, which improves the accuracy and robustness of loop closure detection. It solves the key technical problem of laser mapping in long-distance and narrow environments, providing an efficient and reliable technical solution for intelligent subway inspection and asset management, and has certain engineering application value and innovation.

[0077] The embodiments of the present invention have been described above. However, the embodiments are not limited to the specific implementation methods described above. The specific implementation methods described above are merely illustrative and not restrictive. Those skilled in the art can make more equivalent embodiments under the guidance of the present embodiments, and all of them are within the protection scope of the present embodiments.

Claims

1. A laser mapping method for ISAM2, characterized in that, Includes the following steps: S1: Acquire the raw scanning data stream of the lidar, the data stream of the inertial measurement unit, and the data stream of the wheel odometer. After preprocessing, obtain the point cloud sequence and quality evaluation indicators. S2, using structural feature extraction methods to identify structured geometric elements of the subway environment from point cloud sequences, and obtaining structured line feature descriptor subsequences; S3, acquire point cloud sequence and line feature descriptor sequence, calculate relative motion between consecutive frames and estimate pose uncertainty, and obtain the sequence of relative pose transformation matrix of consecutive frames, the sequence of uncertainty covariance matrix, cumulative odometry trajectory and registration quality mark. S4. Based on the line feature description subsequence, cumulative odometry trajectory and registration quality marker, identify events in which the robot revisits historical locations, and obtain the loop closure set and loop closure confidence covariance. S5: Obtain the sequence of relative pose transformation matrix, uncertainty covariance matrix, registration quality marker, loop closure set and loop closure confidence covariance of continuous frames. Eliminate the accumulated error through incremental optimization and constrained smoothing methods to obtain the global pose estimation sequence and pose covariance estimation. S6 receives the global pose estimation sequence, pose covariance estimation, point cloud sequence, and quality evaluation index, converts the point cloud into a unified coordinate system, and merges overlapping areas to obtain a 3D point cloud map and optimized trajectory.

2. The laser mapping method for ISAM2 according to claim 1, characterized in that, S1 includes: A tightly coupled pre-integration method using an inertial measurement unit and a wheeled odometer is adopted. Taking the frame start time as the reference time, the relative pose transformation of each point in the point cloud is queried according to its scanning timestamp. The corresponding point is then transformed to the sensor coordinate system of the reference time to complete motion distortion correction. The point cloud space is divided into a three-dimensional voxel mesh. The voxel size is adaptively determined based on the local density. For voxels with local density higher than the average density, a larger voxel size is used to re-divide and select representative points. For voxels with local density lower than the average density, more points are retained. A point cloud segmentation method based on spatiotemporal feature fusion is used to label dynamic object point clouds. A sliding window is maintained to cache point clouds of multiple consecutive frames. Temporal correlation analysis is performed on the features of corresponding spatial locations in consecutive frames. Points whose position changes or feature changes exceed a preset dynamic judgment threshold are marked as dynamic candidate points. Cluster analysis and classification are performed on dynamic candidate points, and dynamic object probability and category label are assigned to each point.

3. The laser mapping method for ISAM2 according to claim 1, characterized in that, S2 includes: Adaptive ground segmentation is performed on the preprocessed point cloud of the current frame. The vertical height distribution histogram of the point cloud is statistically analyzed, and the lowest peak value is identified as the ground height estimate. The multi-plane RANSAC algorithm is used to fit the main ground plane on the ground candidate point cloud. Points that are less than a preset distance threshold from the main ground plane are classified as ground point sets, and the remaining points are classified as non-ground point sets. Track line features are extracted from ground point clouds. Density spatial clustering is performed on the ground point clouds to identify track region clusters. For each track region cluster, Hough transform and RANSAC iterative fitting are combined to extract the track centerline. The parallelism of the two straight lines is verified and the track spacing is calculated. The confidence level of the track line features is calculated based on the number of inliers, fitting residuals, parallelism deviation and track gauge deviation. Tunnel contour features are extracted from non-ground point clouds. Cross-sectional point cloud slices perpendicular to the direction of travel are extracted at fixed intervals along the centerline of the track. For the two-dimensional contour point set of each slice, circular, elliptical and multi-segment circular arc models are tried for shape fitting. The model with the smallest residual is selected as the tunnel contour description.

4. The laser mapping method for ISAM2 according to claim 3, characterized in that, S2 further includes: A column detection and sequence completion algorithm integrating geometric constraints and machine learning is adopted. Non-terrestrial point clouds are divided into multiple connected clusters using a 3D clustering algorithm. Point cloud clusters with a height-to-cross-sectional dimension ratio greater than a preset threshold are selected as candidate columns. Multidimensional feature vectors of geometric, shape, and contextual features are extracted from the candidate column clusters and input into a classifier for column identification. Candidate clusters with confidence scores higher than a preset confidence threshold are confirmed as detected columns. Spectral analysis is performed on the position sequences of detected columns to extract dominant periods. The locations of missing columns are predicted based on the dominant periods and marked as virtual columns. The track line parameters, tunnel contour parameters, column position sequence, and confidence scores of each feature are combined into a high-dimensional feature vector as the line feature descriptor for the corresponding frame. For features with low confidence or missing features, a temporal filtering method is used to complete and smooth them using feature values ​​from adjacent frames. The overall feature extraction quality score is calculated by comprehensively evaluating the confidence scores of each feature.

5. The laser mapping method for ISAM2 according to claim 1, characterized in that, S3 includes: The initial registration values ​​are calculated by extracting the track centerline direction vector and the tunnel center position from the line feature descriptor subsequence. The initial rotation value is calculated by the rotation matrix of the track direction vectors in two frames, and the initial translation value is calculated by the displacement vector of the tunnel center position in two frames. A multi-constraint fusion registration cost function is constructed, which includes point-to-point distance, point-to-surface distance, track line direction consistency, and tunnel center alignment. Each constraint is dynamically weighted according to the confidence of its corresponding feature. A nonlinear optimization algorithm is used to iteratively solve the relative pose, and a robust kernel function is used to reduce the weight of corresponding points whose residuals exceed a preset residual threshold. The uncertainty covariance matrix of relative pose is calculated by analyzing the Hessian matrix of the cost function; the registration quality score is calculated by combining the registration convergence, final residual and inlier ratio, and the registration is marked as a low reliability observation if the quality score is lower than the preset quality threshold; the cumulative odometry trajectory is obtained by accumulating the relative pose transformation between consecutive frames.

6. The laser mapping method for ISAM2 according to claim 1, characterized in that, S4 includes: A hierarchical historical feature database is constructed, and a dual index structure of time index and spatial index is established. The time index stores the line feature descriptor sequence in the order of frame timestamps, and the spatial index organizes the spatial location of historical frames using a KD tree structure based on the cumulative odometer trajectory. A coarse search is performed using a scene descriptor with multi-level feature fusion. Global structural features and local detail features are extracted from the current frame. Global structural features include the direction vector of the track centerline, the shape parameters of the tunnel cross section, and the distribution pattern of the pillars. Local detail features include the point cloud density distribution, the reflection intensity distribution, and the geometric shape descriptor. The global structural features and local detail features are adaptively weighted and fused according to stability and discriminability to construct a comprehensive scene descriptor. In the historical database, the global structural features are used for coarse screening, and the local detail features are used for fine screening. The frames with the highest similarity are selected as the candidate loopback frame set, while historical frames whose timestamps are less than the preset time interval threshold are excluded. Geometric consistency verification is performed on the candidate loopback frame set. The track spacing, tunnel radius and column spacing are compared item by item. Candidate frames whose differences in each parameter are less than the corresponding preset consistency threshold are verified. The similarity of column position sequences is compared using the dynamic time warping algorithm.

7. The laser mapping method for ISAM2 according to claim 6, characterized in that, S4 further includes: For candidate frames that pass the precision verification, the ICP registration algorithm with line feature constraints is used to perform multiple optimizations starting from multiple initial poses. The result with the smallest residual is selected as the best registration. The single confidence of the candidate loop is calculated based on the registration residual, the number of inliers and the convergence. Loop candidates with confidence exceeding the preset loop confidence threshold are retained. The spatiotemporal consistency constraint is introduced to verify the loop closure. The path length is calculated from the current frame back along the cumulative odometry trajectory to the candidate frame. The spatial straight-line distance between the current frame and the candidate frame is calculated. The ratio of the path length to the straight-line distance is calculated. Only loop closures with a ratio within the preset range pass the spatiotemporal consistency check. At the same time, the spatial relationship consistency between the current candidate loop closure and the confirmed historical loop closure is checked. For ambiguous loops with multiple candidate frames, a multi-hypothesis tracking strategy based on sequence consistency analysis is adopted. In subsequent consecutive frames, the similarity between each hypothesis and the scene descriptor sequence of the corresponding historical region, the matching degree of the pillar position sequence, and the consistency of the tunnel contour parameter change trend are continuously calculated. An attention weight mechanism is introduced to give higher weight to the discriminative local features. The hypothesis with the highest overall consistency score and the difference between it and the second highest score is greater than the preset ambiguity judgment threshold is selected as the correct loop. If they cannot be distinguished, all hypotheses are rejected.

8. The laser mapping method for ISAM2 according to claim 1, characterized in that, S5 includes: Initialize the factor graph and ISAM2 optimizer. The factor graph consists of variable nodes and factor nodes. The variable nodes represent the robot's 6-DOF pose at different times. Add prior factors to the pose variable nodes at the initial time to fix the initial pose. Incrementally add pose nodes and odometry factors. Add odometry factors between the new pose node and the previous pose node. The measurement value of the odometry factor is the corresponding relative pose transformation matrix in the sequence of relative pose transformation matrices of consecutive frames. The noise model is the corresponding uncertainty covariance matrix. The noise covariance of the odometry factor is dynamically adjusted according to the registration quality mark. The ISAM2 algorithm maintains the Bayesian tree representation of the factor graph. When a new factor is added, only the affected subset of variables is locally eliminated and updated. A progressive activation strategy is adopted to add closure factors. The closure factors connect the pose nodes at the current time step with the pose nodes at historical time steps. The newly added closure factors are initially given a large noise covariance. In subsequent optimization iterations, the noise covariance is gradually reduced so that the closure constraints are smoothly propagated to the global level. When the deviation between the linearized point of the variable node and the current estimated value exceeds the preset relinearization threshold, the corresponding node is relinearized.

9. The laser mapping method for ISAM2 according to claim 1, characterized in that, S6 includes: The optimized pose is compared with the original pose. Pose nodes whose position or attitude changes exceed the preset pose change threshold are marked as needing to be updated. For point cloud frames that need to be updated, the robot body coordinate system is transformed to the global coordinate system according to the optimized pose. During the transformation, the position uncertainty of each point is calculated based on the pose covariance estimation. The global coordinate space is divided into voxels of fixed size to construct a spatial index structure. A confidence-weighted fusion strategy is used to process the point cloud in the overlapping region. For each point, a comprehensive confidence score is calculated based on distance confidence, angle confidence, frame quality confidence, and pose confidence. Multiple observation points within a voxel are weighted and averaged based on the comprehensive confidence score. Remove point clouds marked as dynamic objects from the global map, perform semantic segmentation based on the geometric features and location information of the point clouds, and output a 3D point cloud map and optimized trajectory.

10. A laser mapping system for ISAM2, used to perform the steps of the laser mapping method for ISAM2 as described in any one of claims 1-9, characterized in that, include: The preprocessing module is used to acquire the raw scanning data stream of the lidar, the data stream of the inertial measurement unit, and the data stream of the wheel odometer. After preprocessing, the point cloud sequence and quality evaluation index are obtained. The structural feature extraction module is used to identify the structured geometric elements of the subway environment from the point cloud sequence using structural feature extraction methods, and obtain a structured line feature descriptor sequence. The inter-frame registration module is used to acquire point cloud sequences and line feature descriptor sequences, calculate the relative motion between consecutive frames and estimate pose uncertainty, and obtain the sequence of relative pose transformation matrices, the sequence of uncertainty covariance matrices, the cumulative odometry trajectory and the registration quality markers for consecutive frames. The loop closure detection module is used to identify events in which the robot revisits historical locations based on line feature description subsequences, cumulative odometry trajectories, and registration quality markers, and to obtain the loop closure set and loop closure confidence covariance. The incremental optimization module is used to obtain the sequence of relative pose transformation matrix, uncertainty covariance matrix, registration quality marker, loop closure set and loop closure confidence covariance of continuous frames. It eliminates the cumulative error through incremental optimization and constrained smoothing methods to obtain the global pose estimation sequence and pose covariance estimate. The map fusion module receives global pose estimation sequences, pose covariance estimates, point cloud sequences, and quality assessment metrics. It then transforms the point clouds into a unified coordinate system and merges overlapping areas to obtain a 3D point cloud map and an optimized trajectory.

Images