A 3D reconstruction system for mountainous pine forest environment

Abstract

The application provides a 3D reconstruction system for mountain pine forest environment, and relates to the technical field of unmanned aerial vehicle (UAV) simultaneous localization and mapping (SLAM). The system comprises a hardware platform and a software system, which are composed of a UAV, a laser radar, an inertial measurement unit (IMU) and a global navigation satellite system (GNSS). In view of the problems of sparse features in forest areas, easy drift and high redundancy caused by repetition during mapping, the software system adopts three modules to build a closed loop: 1) a data alignment and system initialization module, which realizes optimal robust alignment of laser and GNSS trajectories by embedding weighted Procrustes analysis into a RANSAC framework; 2) a key point cloud frame selection module, which scores the current frame by comprehensively considering geometric, density and structural characteristics, and dynamically adjusts the space-time threshold to select key frames in combination with historical feedback; and 3) a graph optimization module with multi-dimensional factor constraints, which fuses odometry, GNSS and loop factors to build a factor graph, and globally optimizes the mapping algorithm by using incremental smoothing. The application effectively suppresses cumulative errors and improves the accuracy and efficiency of forest reconstruction.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) synchronous positioning and mapping technology, and more specifically, to a system that integrates solid-state lidar, inertial measurement unit, and global navigation satellite system. Background Technology

[0002] Resource surveys and ecological environment monitoring in mountainous pine forests urgently require high-precision and high-efficiency three-dimensional digital models as a data foundation. Existing Simultaneous Positioning and Mapping (SLAM) systems face the following problems when operating for extended periods in large-scale environments with repetitive structures and sparse features in pine forests: (1) The coordinates of laser inertial odometry and GNSS odometry are difficult to match accurately, and the generated maps cannot be directly aligned with geographic information systems. Figure 1 (1) Slow convergence; (2) High computational cost and storage overhead due to simple key frame selection strategy, resulting in loss or redundancy of map information; (3) Lack of multi-factor constraints and real-time map optimization, pure laser inertial odometry often drifts severely, resulting in map deformation and poor global consistency. Summary of the Invention

[0003] Addressing the challenges existing Simultaneous Localization and Mapping (SLAM) systems face when operating for extended periods in pine forest environments. Figure 1 To address the issues of slow convergence, map information loss or redundancy, and poor global consistency, this invention provides a 3D reconstruction system for mountainous pine forest environments. This system, through multi-sensor fusion and the introduction of data alignment and system initialization modules, environmental feature-driven key point cloud frame selection modules, and multi-dimensional factor-constrained graph optimization modules, achieves high-precision, high-efficiency, and globally consistent 3D reconstruction in large-scale, low-texture environments such as mountainous pine forests. It resolves the two core contradictions of accumulated error and data processing efficiency during long-term system operation.

[0004] The present invention provides a specific system for a 3D reconstruction system for mountainous pine forest environments. The system is configured to operate for a long time in large-scale, weakly textured environments such as mountainous pine forests, where structures are repetitive, features are sparse, and GNSS signals are easily blocked and reflected. The system includes a hardware platform and a software system.

[0005] The hardware platform includes a multi-rotor drone, a sensor suite mounted on the upper part of the multi-rotor drone, and a computing unit installed inside the multi-rotor drone; the sensor suite includes a lidar, an inertial measurement unit (IMU), and a GNSS receiver;

[0006] The software system runs in the computing unit and includes:

[0007] Data Alignment and System Initialization Module: During the startup phase of the 3D reconstruction system for mountainous pine forest environments, GNSS data, lidar scan data, and IMU data are recorded simultaneously. A GNSS odometry is constructed based on the GNSS data, and a laser inertial odometry is constructed based on the lidar scan data and IMU data. The laser inertial odometry provides an estimated laser trajectory in the local coordinate system, and the GNSS odometry provides a corresponding GNSS trajectory in the global coordinate system. Coordinate alignment between the lidar local coordinate system and the global coordinate system is achieved through the laser trajectory and GNSS trajectory. To address the abnormal observations caused by GNSS multipath effects in mountainous pine forest environments, the alignment process employs weighted Protodyakonov analysis embedded in the RANSAC framework for robust estimation, providing denoised continuous point cloud frames for subsequent processing.

[0008] Key point cloud frame selection module: continuously receives the continuous point cloud frames and the relative motion information between the current continuous point cloud frame and historical key point cloud frames; continuously filters continuous point cloud frames through an adaptive key frame selection mechanism; the adaptive mechanism addresses the problem of key frame redundancy or loss caused by repetitive pine forest structure and sparse features by comprehensively scoring three dimensions of geometric complexity, density distribution, and structural characteristics, and dynamically adjusting the threshold in combination with historical selection quality feedback, marking continuous point cloud frames that meet preset conditions as key point cloud frames;

[0009] The multi-dimensional factor-constrained graph optimization module continuously receives the key point cloud frames and optimizes them using a nonlinear method. The optimization is tailored to the cumulative drift in the large-scale environment of the forest area. It integrates laser inertial odometry factors, loop closure factors, and GNSS factors to construct a factor graph and uses the optimized key point cloud frames to update the global three-dimensional point cloud map in real time.

[0010] Preferably, the coordinate alignment process between the local coordinate system and the global coordinate system of the lidar is as follows:

[0011] Let the robot trajectory estimated by the laser inertial odometry in the local coordinate system L be a pose sequence. ,in Indicates the first The pose of a series of point cloud frames, including rotation and translation components; This refers to the total number of consecutive point cloud frames involved in the initialization; GNSS provides the corresponding position observation sequence in the global coordinate system G. That is, the position coordinates of the GNSS trajectory, where Using ENU coordinates, the goal of initial pose estimation is to solve for the optimal rigid body transformation. ,in Let be a rotation matrix. Given a translation vector, construct a weighted least squares problem:

[0012] ;

[0013] in, It is the first in the estimated robot trajectory Translation components of the pose of a continuous point cloud frame For the first Signal quality weights for each GNSS observation.

[0014] Specifically, the coordinate alignment is solved through weighted Protodyakonov analysis, as follows:

[0015] First, calculate the weighted centroids of the radar and GNSS trajectories. , :

[0016] ,

[0017] ;

[0018] Further construct the weighted covariance matrix :

[0019] ;

[0020] right Perform singular value decomposition: The rotation matrix can be obtained. Translation vector :

[0021] ,

[0022] ,

[0023] Based on rotation matrix Translation vector Achieve coordinate alignment.

[0024] Preferably, during the coordinate alignment process, outlier removal is performed on the corresponding position observations provided by the GNSS in the global coordinate system G. The removal method involves embedding weighted Protodyakonov analysis into the RANSAC framework, specifically:

[0025] RANSAC estimates the transformation by iteratively sampling the minimum point set and selects the optimal model based on the number of interior points; interior points The judgment criteria are:

[0026] ,

[0027] Among them, the error threshold Adjustable based on GNSS signal quality;

[0028] Maximum number of iterations Based on the expected percentage of points and the preset expected probability Sure:

[0029] .

[0030] Preferably, the adaptive keyframe selection mechanism includes a feature evaluation step and a threshold decision step; the feature evaluation step scores the current continuous point cloud frame; the threshold decision step dynamically adjusts the keyframe insertion threshold according to the score, and when the relative motion information between the current continuous point cloud frame and the previous key point cloud frame satisfies the triggering condition corresponding to the keyframe insertion threshold, the current point cloud frame is determined to be a key point cloud frame.

[0031] Specifically, the feature evaluation step is as follows: assuming each consecutive point cloud frame has M point clouds, the current i-th... A series of point cloud frames In this context, a point cloud can be represented as , midpoint , For the index of the intra-frame point cloud of a continuous point cloud, i.e. For the first The first of the consecutive point cloud frames A point cloud; the scoring mechanism includes an environmental feature score for the current point cloud frame, specifically: a geometric complexity score. Density distribution score and structural characteristic score The feature evaluation step obtains the environmental feature score of the current continuous point cloud frame. , and The environmental characteristic scores were normalized to... scope;

[0032] The geometric complexity score Based on the definition of eigenvalues, the geometric complexity score of the current continuous point cloud frame. for:

[0033] For point Within its radius Built-in local point set Calculate the covariance matrix:

[0034] ,

[0035] in For local centroid, ;

[0036] right Perform eigenvalue decomposition:

[0037] ,

[0038] in For eigenvalues; The corresponding feature vector;

[0039] The geometric complexity score for the current consecutive point cloud frames is:

[0040] ;

[0041] Flatness Curvature change ;

[0042] The density change score Defined as:

[0043] ;

[0044] in The number of point clouds after voxelization;

[0045] The aforementioned structural characteristic score Combining the changes in normal vectors and the anisotropy index, it is defined as:

[0046] ;

[0047] in For the change of normal vector, Calculated using the variance of the local normal vector direction:

[0048] ;

[0049] in, For point The estimated normal vector; This is the average direction vector of the normal vectors of all points in the current point cloud frame;

[0050] For anisotropy, Based on eigenvalue calculation:

[0051] .

[0052] Specifically, the threshold decision step includes a historical selection quality feedback quantity. With three decoupled threshold adjusters , and ,in For rotation threshold, Distance threshold The time threshold is used; historical selection of quality feedback volume. By maintaining a fixed length A sliding window that stores the most recently successfully inserted... The comprehensive information score for each key point cloud frame is as follows: ,in , where parameters , and For the preset weights, , and It is the normalized environmental feature score of each keypoint cloud frame, where m is the index of the keypoint cloud frame within the sliding window; feedback quantity Calculated using the exponential moving average, specifically:

[0053] ,

[0054] in, The score for the last inserted keyframe. As the attenuation factor, ;

[0055] The rotation threshold The specific calculation method is as follows:

[0056] ;

[0057] in, The base value for the rotation threshold. The environmental response coefficient represents the structural characteristic score. The strength of the influence on the threshold This is the feedback adjustment coefficient for the rotation threshold;

[0058] The distance threshold The specific calculation method is as follows:

[0059] ;

[0060] Among them, parameters This is the base value for the distance threshold. The environmental response coefficient represents the geometric complexity score. The strength of the influence on the threshold This is the feedback adjustment coefficient for the distance threshold;

[0061] The time threshold The specific calculation method is as follows:

[0062] ;

[0063] Among them, parameters This is the base value for the time threshold. This is the feedback adjustment coefficient for the time threshold;

[0064] Feedback adjustment coefficient , and Indicates the quality feedback volume of historical selections. The adjustment strength of the keyframe insertion threshold, when When the threshold is low, the keyframe insertion threshold is reduced.

[0065] Preferably, the method of dynamically adjusting the keyframe insertion threshold based on the score is as follows:

[0066] Obtain the relative motion information between the current frame and the previous keyframe, including translation. Rotation amount and time difference The final insertion decision is generated based on the following logic:

[0067] ;

[0068] Among them, Insert Keyframe is the insertion decision;

[0069] If the current frame is inserted as a keyframe, update the history scoring window and recalculate. And then according to Adjust the keyframe insertion threshold.

[0070] Preferably, the specific method for optimizing the key point cloud frame using a nonlinear method is as follows:

[0071] Suppose that the sequence of key point cloud frames after keyframe filtering is a series of key point cloud frame poses. ,for Key point cloud frame pairs selected by the keyframe selection mechanism Based on laser odometry factor Circulation factor and GNSS factor Constructing a factor graph model The key point cloud frame is optimized and expressed as the maximum a posteriori probability estimate:

[0072] ,

[0073] in, , and These represent the sets of indices for the laser inertial odometry factor, the cloze factor, and the GNSS factor, respectively. For the number of key point cloud frames, The optimized key point cloud frame sequence is obtained through incremental smoothing and mapping algorithms.

[0074] Specifically, factor graph model The optimization method is as follows:

[0075] The factor graph model The conditional probability distribution can be expressed as:

[0076] ;

[0077] in Represents all observed data; prior factors The initial pose is fixed at the origin of the world coordinate system; , and These represent the sets of indices for the laser inertial odometry factor, the loop closure factor, and the GNSS factor, respectively; each factor corresponds to a specific sensor observation constraint.

[0078] The laser inertial odometry factor utilizes a tightly coupled framework based on an error-iterative Kalman filter algorithm. For each laser scan cycle, motion prediction is provided through IMU pre-integration, and measurements are updated using the original point cloud. For keyframes selected by the keyframe selection mechanism... Laser odometry factor in factor graph model The following is represented as inter-frame factor constraint:

[0079] ;

[0080] The loop closure detection factor is mathematically expressed as a residual function based on pose constraints:

[0081] ;

[0082] in Here is the noise covariance matrix; Each of the keyframes Pose matrix; It is the adaptive noise covariance matrix; logarithmic operation Map the elements of the Lie group to the tangent space; To obtain the selected keyframe pair through point cloud registration calculation The relative poses between them;

[0083] Point cloud registration is achieved using the ICP algorithm, which seeks an optimal rigid body transformation matrix to minimize the objective function.

[0084] ;

[0085] in and Point clouds for the current frame and historical frames, respectively; Let be the relative transformation matrix to be optimized; It is the index of points in the point cloud;

[0086] The determination of whether to start loopback detection is as follows:

[0087] ;

[0088] Among them, uniqueness check For the current keyframe Cannot be in the set of detected loops Medium; Spatial proximity check To be at the current frame position neighborhood radius Within it, at least one historical keyframe is required. Time separation test Eliminate loops caused by small-scale reciprocating motions at adjacent moments;

[0089] The GNSS factor provides absolute position constraints, and its mathematical form is:

[0090] ;

[0091] in Keyframe The translation vector, These are GNSS measurements, all representing positions in the local ENU coordinate system. It is the GNSS noise covariance, extracted from the receiver navigation data;

[0092] Coordinate transformation involves a complete chain of mathematical derivations; the formula for transforming from WGS84 coordinates to ENU coordinates is:

[0093] ;

[0094] in The radius of curvature of the zonal circle varies with latitude; , and These represent latitude, longitude, and elevation, respectively, forming a point description in the WGS84 coordinate system.

[0095] The transformation from ECEF coordinates to local ENU coordinates is achieved using a rotation matrix:

[0096] .

[0097] The 3D reconstruction system proposed in this invention, through its three core collaborative working modules, has multi-layered beneficial effects in solving the problem of large-scale, weak-texture environment reconstruction in mountainous pine forests.

[0098] First, in terms of reconstruction accuracy and global consistency, the data alignment and initialization module ensures high-precision and robust initial alignment between the local coordinate system and the global coordinate system of the LiDAR, establishing an accurate global spatial benchmark for the entire reconstruction task, enhancing the global consistency of the map, and avoiding slow convergence speed and decreased map accuracy during back-end optimization due to initial system deviations.

[0099] Based on this, the environmental feature-driven keyframe selection module expands the evaluation dimensions from the traditional single spatiotemporal threshold to multi-dimensional environmental feature perception such as geometric complexity, point cloud density distribution, and structural characteristics. In feature-rich areas, it retains multiple highly representative key point cloud frames to avoid map loss, while in feature-single areas, it avoids map redundancy, thus ensuring the information completeness and structural integrity of the constructed fine point cloud map.

[0100] Finally, the multi-dimensional factor-constrained graph optimization module unifies the processing of laser inertial odometer factors, GNSS odometer factors, and loop closure detection factors, effectively suppressing the cumulative drift caused by long-term, large-scale operation, and meeting the needs of forestry surveys, ecological monitoring, and other applications for high-precision three-dimensional spatial data.

[0101] In summary, this invention provides a complete solution to the problems of accumulated errors and low efficiency in long-term mapping in complex forest environments through a closed-loop technology system of highly robust initialization, intelligent frame filtering, and global optimization. Attached Figure Description

[0102] Figure 1 This is a flowchart illustrating the software system architecture and data processing.

[0103] Figure 2 A schematic diagram illustrating the working principle of the data alignment and system initialization module;

[0104] Figure 3 Flowchart for keyframe selection decision based on environmental features. Detailed Implementation

[0105] The following description, in conjunction with the accompanying drawings and specific embodiments, illustrates a 3D reconstruction system for mountainous pine forest environments provided by the present invention.

[0106] The SLAM system proposed in this invention adopts a layered architecture and modular data processing, including a hardware platform and a software system, combined with... Figure 1The overall process is described below. The hardware platform includes a multi-rotor drone, a sensor suite mounted on the upper part of the multi-rotor drone, and a computing unit installed inside the multi-rotor drone; wherein, the sensor suite includes a lidar, an inertial measurement unit (IMU), and a GNSS receiver; the software system runs in the computing unit and includes:

[0107] The module includes data alignment and system initialization, key point cloud frame selection, and graph optimization with multi-dimensional factor constraints.

[0108] First, in the data alignment module, the system takes raw LiDAR point cloud, IMU data, and GNSS observations as input. Motion compensation and state estimation are performed by a front-end laser inertial odometry system based on tightly coupled iterative Kalman filtering, outputting the high-frequency robot pose and distortion-free point cloud in real time. Simultaneously, the GNSS odometry module fuses the laser inertial trajectory, completing coordinate alignment and system initialization.

[0109] Preferably, combined with Figure 2 The data alignment and system initialization module is responsible for resolving the precise alignment problem between the local coordinate system (L) and the global coordinate system (G, ENU) of the LiDAR. Let the robot trajectory estimated by the laser inertial odometry in the local coordinate system L be a pose sequence. ,in Indicates the first The pose of a series of point cloud frames; The number of consecutive point cloud frames participating in the initialization; GNSS provides the corresponding position observation in the global coordinate system G, i.e., the position coordinates of the GNSS trajectory. ,in Using ENU coordinates, the goal of initial pose estimation is to solve for the optimal rigid body transformation. ,in Let be a rotation matrix. Let be the translation vector. The transformed laser trajectory and the GNSS trajectory are optimally aligned in the weighted least squares sense:

[0110] ;

[0111] in, It is the first in the laser trajectory Translation components of each pose For the first Each GNSS signal quality weight.

[0112] Preferably, weighted Protodyakonov analysis is selected as the core mathematical tool for solving the above optimal transformation. First, the weighted centroids of the radar and GNSS trajectories are calculated. , :

[0113] ,

[0114] ;

[0115] Then construct the weighted covariance matrix. :

[0116] ;

[0117] right Perform singular value decomposition: The rotation matrix can be obtained. Translation vector :

[0118] ,

[0119] ,

[0120] Based on rotation matrix Translation vector Achieve coordinate alignment.

[0121] Preferably, to address the outlier problem in GNSS observations, the aforementioned weighted Protodyakonov analysis is embedded within the RANSAC framework. RANSAC estimates the transformation by iteratively sampling the minimum point set and selects the optimal model based on the number of inliers. The judgment criteria are:

[0122] ,

[0123] Among them, the error threshold Adjustable based on GNSS signal quality;

[0124] Maximum number of iterations Based on the expected percentage of points and the preset expected probability Confirm, take :

[0125] .

[0126] The algorithm effectively removes outlier observations using the RANSAC framework and applies weighted Protodyakonov analysis on a clean subset of data, exhibiting both robustness and optimality, thus providing a reliable initial transformation for subsequent fusion. After initialization, the keyframe selection and map management module dynamically adjusts thresholds based on point cloud geometric features to filter keyframes, passing their poses to the backend for optimization, while the point cloud participates in constructing the global map.

[0127] In addition, the system also includes a keyframe selection decision module. Combined with... Figure 3The keyframe selection module includes an adaptive keyframe selection mechanism based on multi-dimensional feature perception, which consists of two steps: environmental feature evaluation and threshold decision.

[0128] Preferably, the environmental feature assessment extends the assessment to three dimensions: geometric complexity, density distribution, and structural characteristics. Assume each consecutive point cloud frame contains M point clouds, and the current frame contains... A series of point cloud frames In this context, a point cloud can be represented as , midpoint , For the index of the intra-frame point cloud of a continuous point cloud, i.e. For the first The first of the consecutive point cloud frames A point cloud; the scoring mechanism includes an environmental feature score for the current point cloud frame, specifically: a geometric complexity score. Density distribution score and structural characteristic score The feature evaluation step obtains the environmental feature score of the current continuous point cloud frame. , and The environmental characteristic scores were normalized to... scope;

[0129] The geometric complexity score Based on the definition of eigenvalues, the geometric complexity score of the current continuous point cloud frame. for:

[0130] For point Within its radius Built-in local point set Calculate the covariance matrix:

[0131] ,

[0132] in For local centroid, ;

[0133] right Perform eigenvalue decomposition:

[0134] ,

[0135] in For eigenvalues; The corresponding feature vector;

[0136] The geometric complexity score for the current consecutive point cloud frames is:

[0137] ;

[0138] Flatness Curvature change ;

[0139] The density change score Defined as:

[0140] ;

[0141] in The number of point clouds after voxelization;

[0142] The aforementioned structural characteristic score Combining the changes in normal vectors and the anisotropy index, it is defined as:

[0143] ;

[0144] in For the change of normal vector, Calculated using the variance of the local normal vector direction:

[0145] ;

[0146] in, For point The estimated normal vector; This is the average direction vector of the normal vectors of all points in the current point cloud frame;

[0147] For anisotropy, Based on eigenvalue calculation:

[0148] .

[0149] The environmental feature score of the current continuous point cloud frame is obtained by combining the evaluation results from the three dimensions. , and The environmental characteristic scores were normalized to... scope.

[0150] Furthermore, the threshold decision module includes a historical selection quality feedback quantity. With three decoupled spatiotemporal thresholds , and ,in For rotation threshold, Distance threshold These, acting as time thresholds, collectively form a closed-loop adjustment system capable of perceiving historical performance. Historical selection quality feedback quantity. By maintaining a fixed length A sliding window that stores the most recently successfully inserted... The comprehensive information score for each key point cloud frame is as follows: ,in ; , and It is the normalized environmental feature score of each keypoint cloud frame, where m is the index of the keypoint cloud frame within the sliding window; feedback quantity Calculated using an exponential moving average, which places greater emphasis on recent performance, specifically:

[0151] ,

[0152] in, The score for the last inserted keyframe. As the attenuation factor, ;

[0153] The rotation threshold The specific calculation method is as follows:

[0154] ;

[0155] in, The base value for the rotation threshold. For environmental response coefficient, This is the feedback adjustment coefficient;

[0156] The distance threshold The specific calculation method is as follows:

[0157] ;

[0158] Among them, parameters This is the base value for the distance threshold. For environmental response coefficient, This is the feedback adjustment coefficient for the distance threshold;

[0159] The time threshold The specific calculation method is as follows:

[0160] ;

[0161] Among them, parameters This is the base value for the time threshold. This is the feedback adjustment coefficient for the time threshold.

[0162] Preferably, the method of dynamically adjusting the keyframe insertion threshold based on the score specifically involves: obtaining the relative motion information between the current frame and the previous keyframe, including the translation amount. Rotation amount and time difference The final insertion decision is generated based on the following logic:

[0163] ;

[0164] Among them, Insert Keyframe is the insertion decision;

[0165] If the current frame is inserted as a keyframe, update the history scoring window and recalculate. .

[0166] The system also includes a multi-factor constraint graph optimization module. The optimized keyframe pose is used to update the global point cloud map and fed back to the front-end odometry module to achieve closed-loop correction of motion estimation.

[0167] Preferably, combined with Figure 1 The factor graph model constructed in this invention aims to fuse multi-source sensor information, including laser inertial odometry, loop closure detection, and GNSS observations, through a front-end and back-end architecture. It assumes that the key point cloud frame sequence after key frame filtering is a series of key point cloud frame poses. ,for Key point cloud frame pairs selected by the keyframe selection mechanism Based on laser odometry factor Circulation factor and GNSS factor Constructing a factor graph model The key point cloud frame is optimized and expressed as the maximum a posteriori probability estimate:

[0168] ;

[0169] The solution is obtained through incremental smoothing and graph construction algorithms.

[0170] Furthermore, the conditional probability distribution of the factor graph model can be expressed as:

[0171] ;

[0172] in Represents all observed data; prior factors The initial pose is fixed at the origin of the world coordinate system; , and These represent the sets of indices for the laser inertial odometry factor, the loop closure factor, and the GNSS factor, respectively; each factor corresponds to a specific sensor observation constraint.

[0173] The laser inertial odometry factor utilizes a tightly coupled framework based on an error-iterative Kalman filter algorithm. For each laser scan cycle, motion prediction is provided through IMU pre-integration, and measurements are updated using the original point cloud. For keyframe pairs selected by the keyframe selection mechanism... Laser odometry factor in factor graph model The following is represented as inter-frame factor constraint:

[0174] ;

[0175] in Here is the noise covariance matrix; To obtain the selected keyframe pair through point cloud registration calculation The relative poses between them are derived from the global pose difference transformation of the front-end state estimation result of the Kalman filter algorithm based on error iteration, which is obtained by accumulating the transformations of intermediate consecutive frames;

[0176] The loop closure detection factor establishes constraints between discontinuous keyframes by identifying revisit locations in the environment. When a loop closure occurs, it corrects accumulated errors through strong constraints. The mathematical foundation of the loop closure detection factor is based on Lie group and Lie algebra theory. Its core mathematical expression is a residual function based on pose constraints:

[0177] ;

[0178] in Each of the keyframes Pose matrix; It is the adaptive noise covariance matrix; logarithmic operation Map the elements of the Lie group to the tangent space; To obtain the selected keyframe pair through point cloud registration calculation Relative poses between them; logarithmic operations Mapping Lie group elements to the tangent space facilitates optimization in the vector space;

[0179] Point cloud registration is achieved using the ICP algorithm, which seeks an optimal rigid body transformation matrix to minimize the objective function.

[0180] ;

[0181] in and Point clouds for the current frame and historical frames, respectively; Let be the relative transformation matrix to be optimized; It is the index of points in the point cloud;

[0182] The determination of whether to start loop closure detection needs to consider the uniqueness, spatial proximity, and temporal separation of keyframe detection:

[0183] ;

[0184] Uniqueness check It is necessary to ensure the current keyframe Cannot be in the set of detected loops Medium; Spatial proximity check This is achieved through radius search using a KD-tree at the current frame position. neighborhood radius Within it, at least one historical keyframe is required. Time separation test This ensures that the loop is not caused by small-scale reciprocating motion at adjacent moments;

[0185] GNSS factors provide absolute position constraints, and their mathematical form is:

[0186] ;

[0187] in Keyframe The translation vector, These are GNSS measurements, all representing positions in the local ENU coordinate system. It is the GNSS noise covariance, extracted from the receiver navigation data;

[0188] Coordinate transformation involves a complete chain of mathematical derivations; the formula for transforming from WGS84 coordinates to ENU coordinates is:

[0189] ;

[0190] in The radius of curvature of the zonal circle varies with latitude; , and These represent latitude, longitude, and elevation, respectively, forming a point description in the WGS84 coordinate system.

[0191] The transformation from ECEF coordinates to local ENU coordinates is achieved using a rotation matrix:

[0192] .

[0193] The optimization result is expressed as pose correction. The information is fed back to the laser inertial odometry in real time to correct its pose front end, forming a closed-loop optimization mechanism.

[0194] ,

[0195] in, The pose before laser inertial odometry correction. The pose is corrected by the laser inertial odometry. This is the pose correction amount output by the graph optimization module.

[0196] The scope of this invention is not limited to the above-described embodiments. A combination of one or more specific embodiments can also achieve the purpose of the invention and falls within the protection scope of this invention.

Claims

1. A 3D reconstruction system for mountainous pine forest environments, characterized in that, The system includes a hardware platform and a software system; The hardware platform includes a multi-rotor drone, a sensor suite mounted on the upper part of the multi-rotor drone, and a computing unit installed inside the multi-rotor drone; the sensor suite includes a lidar, an inertial measurement unit (IMU), and a GNSS receiver; The software system runs in the computing unit and includes: Data alignment and system initialization module: During the startup phase of the 3D reconstruction system for a mountainous pine forest environment, GNSS data, lidar scan data, and IMU data are recorded simultaneously; a GNSS odometry is constructed based on the GNSS data, and a laser inertial odometry is constructed based on the lidar scan data and IMU data; the laser inertial odometry provides an estimated laser trajectory in the local coordinate system, and the GNSS odometry provides a corresponding GNSS trajectory in the global coordinate system; the local coordinate system and the global coordinate system of the lidar are aligned through the laser trajectory and the GNSS trajectory; the data alignment and system initialization module provides denoised continuous point cloud frames for subsequent processing; Key point cloud frame selection module: continuously receives the continuous point cloud frames and the relative motion information between the current continuous point cloud frames and historical key point cloud frames; continuously filters continuous point cloud frames through an adaptive key frame selection mechanism, and marks continuous point cloud frames that meet preset conditions as key point cloud frames. The multi-factor constrained graph optimization module continuously receives the key point cloud frames and optimizes them using a nonlinear method, then uses the optimized key point cloud frames to update the global 3D point cloud map in real time.

2. The 3D reconstruction system for mountainous pine forest environments according to claim 1, characterized in that, The coordinate alignment process between the local coordinate system and the global coordinate system of the lidar is as follows: Let the robot trajectory estimated by the laser inertial odometry in the local coordinate system L be a pose sequence. ,in Indicates the first The pose of a series of point cloud frames, including rotation and translation components; This refers to the total number of consecutive point cloud frames involved in the initialization; GNSS provides the corresponding position observation sequence in the global coordinate system G. That is, the position coordinates of the GNSS trajectory, where Using ENU coordinates, the goal of initial pose estimation is to solve for the optimal rigid body transformation. ,in Let be a rotation matrix. Given a translation vector, construct a weighted least squares problem: ； in, It is the first in the estimated robot trajectory Translation components of the pose of a continuous point cloud frame For the first Signal quality weights for each GNSS observation.

3. The 3D reconstruction system for mountainous pine forest environments according to claim 2, characterized in that, The coordinate alignment is solved using weighted Protodyakonov analysis, specifically as follows: First, calculate the weighted centroids of the radar and GNSS trajectories. , : ， ； Further construct the weighted covariance matrix : ； right Perform singular value decomposition: The rotation matrix can be obtained. Translation vector : ， ， Based on rotation matrix Translation vector Achieve coordinate alignment.

4. The 3D reconstruction system for mountainous pine forest environments according to claim 3, characterized in that, In the process of solving the coordinate alignment, outlier removal is also performed on the corresponding position observations provided by the GNSS in the global coordinate system G. The removal method is to embed weighted Protodyakonov analysis into the RANSAC framework, specifically: RANSAC estimates the transformation by iteratively sampling the minimum point set and selects the optimal model based on the number of interior points; interior points The judgment criteria are: ， Among them, the error threshold Adjustable based on GNSS signal quality; Maximum number of iterations Based on the expected percentage of points and the preset expected probability Sure: 。 5. A 3D reconstruction system for mountainous pine forest environments according to claim 1, characterized in that, The adaptive keyframe selection mechanism includes a feature evaluation step and a threshold decision step. The feature evaluation step scores the current continuous point cloud frame; the threshold decision step dynamically adjusts the key frame insertion threshold based on the score; when the relative motion information between the current continuous point cloud frame and the previous key point cloud frame satisfies the triggering condition corresponding to the key frame insertion threshold, the current point cloud frame is determined to be a key point cloud frame.

6. A 3D reconstruction system for mountainous pine forest environments according to claim 5, characterized in that, The feature evaluation step is specifically as follows: assuming each consecutive point cloud frame has M point clouds, the current i-th... A series of point cloud frames In this context, a point cloud can be represented as , midpoint , For the index of the intra-frame point cloud of a continuous point cloud, i.e. For the first The first of the consecutive point cloud frames A point cloud; the scoring mechanism includes an environmental feature score for the current point cloud frame, specifically: a geometric complexity score. Density distribution score and structural characteristic score The feature evaluation step obtains the environmental feature score of the current continuous point cloud frame. , and The environmental characteristic scores were normalized to... scope; The geometric complexity score Based on the definition of eigenvalues, the geometric complexity score of the current continuous point cloud frame. for: For point Within its radius Built-in local point set Calculate the covariance matrix: , in For local centroid, ; right Perform eigenvalue decomposition: ， in For eigenvalues; The corresponding feature vector; The geometric complexity score for the current consecutive point cloud frames is: ； Flatness Curvature change ; The density change score Defined as: ； in The number of point clouds after voxelization; The aforementioned structural characteristic score Combining the changes in normal vectors and the anisotropy index, it is defined as: ； in For the change of normal vector, Calculated using the variance of the local normal vector direction: ； in, For point The estimated normal vector; This is the average direction vector of the normal vectors of all points in the current point cloud frame; For anisotropy, Based on eigenvalue calculation: 。 7. A 3D reconstruction system for mountainous pine forest environments according to claim 5, characterized in that, The threshold decision-making step includes a historical selection quality feedback quantity. With three decoupled threshold adjusters , and ,in For rotation threshold, Distance threshold The time threshold is used; historical selection of quality feedback volume. By maintaining a fixed length A sliding window that stores the most recently successfully inserted... The comprehensive information score for each key point cloud frame is as follows: ,in , where parameters , and For the preset weights, , and It is the normalized environmental feature score of each keypoint cloud frame, where m is the index of the keypoint cloud frame within the sliding window; feedback quantity Calculated using the exponential moving average, specifically: ， in, The score for the last inserted keyframe. As the attenuation factor, ; The rotation threshold The specific calculation method is as follows: ； in, The base value for the rotation threshold. The environmental response coefficient represents the structural characteristic score. The strength of the influence on the threshold This is the feedback adjustment coefficient for the rotation threshold; The distance threshold The specific calculation method is as follows: ； Among them, parameters This is the base value for the distance threshold. The environmental response coefficient represents the geometric complexity score. The strength of the influence on the threshold This is the feedback adjustment coefficient for the distance threshold; The time threshold The specific calculation method is as follows: ； Among them, parameters This is the base value for the time threshold. This is the feedback adjustment coefficient for the time threshold; Feedback adjustment coefficient , and Indicates the quality feedback volume of historical selections. The adjustment strength of the keyframe insertion threshold, when When the threshold is low, the keyframe insertion threshold is reduced.

8. A 3D reconstruction system for mountainous pine forest environments according to claim 5, characterized in that, The specific method for dynamically adjusting the keyframe insertion threshold based on the aforementioned score is as follows: Obtain the relative motion information between the current frame and the previous keyframe, including translation. Rotation amount and time difference The final insertion decision is generated based on the following logic: ； Among them, Insert Keyframe is the insertion decision; If the current frame is inserted as a keyframe, update the history scoring window and recalculate. And then according to Adjust the keyframe insertion threshold.

9. A 3D reconstruction system for mountainous pine forest environments according to claim 1, characterized in that, The specific method for optimizing the key point cloud frame using a nonlinear approach is as follows: Suppose that the sequence of key point cloud frames after keyframe filtering is a series of key point cloud frame poses. ,for Key point cloud frame pairs selected by the keyframe selection mechanism Based on laser odometry factor Circulation factor and GNSS factor Constructing a factor graph model The key point cloud frame is optimized and expressed as the maximum a posteriori probability estimate: ， in, , and These represent the sets of indices for the laser inertial odometry factor, the cloning factor, and the GNSS factor, respectively. For the number of key point cloud frames, The optimized key point cloud frame sequence is obtained through incremental smoothing and mapping algorithms.

10. A 3D reconstruction system for mountainous pine forest environments according to claim 9, characterized in that, The factor graph model The optimization method is as follows: Factor graph model is The conditional probability distribution can be expressed as: ； in Represents all observed data; prior factors The initial pose is fixed at the origin of the world coordinate system; , and These represent the sets of indices for the laser inertial odometry factor, the loop closure factor, and the GNSS factor, respectively; each factor corresponds to a specific sensor observation constraint. The laser inertial odometry factor utilizes a tightly coupled framework based on an error-iterative Kalman filter algorithm. For each laser scan cycle, motion prediction is provided through IMU pre-integration, and measurements are updated using the original point cloud. For keyframes selected by the keyframe selection mechanism... Laser odometry factor in factor graph model The following is represented as inter-frame factor constraint: ； The loop closure detection factor is mathematically expressed as a residual function based on pose constraints: ； in Here is the noise covariance matrix; Each of the keyframes Pose matrix; It is the adaptive noise covariance matrix; logarithmic operation Map the elements of the Lie group to the tangent space; To obtain the selected keyframe pair through point cloud registration calculation The relative poses between them; Point cloud registration is achieved using the ICP algorithm, which seeks an optimal rigid body transformation matrix to minimize the objective function. ； in and Point clouds for the current frame and historical frames, respectively; Let be the relative transformation matrix to be optimized; It is the index of points in the point cloud; The determination of whether to start loopback detection is as follows: ； Among them, uniqueness check For the current keyframe Cannot be in the set of detected loops Medium; Spatial proximity check To be at the current frame position neighborhood radius Within it, at least one historical keyframe is required. Time separation test Eliminate loops caused by small-scale reciprocating motions at adjacent moments; The GNSS factor provides absolute position constraints, and its mathematical form is: ； in Keyframe The translation vector, These are GNSS measurements, all representing positions in the local ENU coordinate system. It is the GNSS noise covariance, extracted from the receiver navigation data; Coordinate transformation involves a complete chain of mathematical derivations; the formula for transforming from WGS84 coordinates to ENU coordinates is: ； in The radius of curvature of the zonal circle varies with latitude; , and These are latitude, longitude, and elevation, respectively, forming a point description in the WGS84 coordinate system; The transformation from ECEF coordinates to local ENU coordinates is achieved using a rotation matrix: 。

Images