A multi-engine laser slam point cloud splicing method and system based on graph optimization

Abstract

The application discloses a multi-project laser SLAM point cloud splicing method and system based on graph optimization, wherein the multi-project laser SLAM point cloud splicing method comprises the following steps: performing single-project laser SLAM loop detection by taking submap point clouds as key frames to obtain single-project loop detection constraints; if there are overlapping point clouds between any two groups of projects, then the submap point clouds at the overlapping point cloud splicing positions are spliced to obtain multi-project splicing constraints; if there are GPS control points in the submap point clouds, then the GPS control point constraints are calculated according to the GPS control points associated with the submap point clouds; the single-project loop detection constraints, the multi-project splicing constraints and the GPS control point constraints are used to establish a graph optimization model of the submap point clouds; the graph optimization model is solved by using a Ceres nonlinear optimization library to obtain the pose correction amount of the submap point clouds, and the purpose of single-project SLAM point cloud non-rigid pose correction is achieved. The technical scheme of the application can solve the problem that, in the prior art, a single project is regarded as a rigid body when multi-project laser SLAM is used to solve point cloud splicing, and the whole cannot achieve the best splicing consistency.

Description

Technical Field

[0001] This invention relates to the field of lidar technology, and in particular to a graph-optimized multi-engineering laser SLAM point cloud stitching method and system. Background Technology

[0002] Handheld and backpack LiDAR scanners typically rely on SLAM (Simultaneous Localization and Mapping) algorithms for mapping. They generate dense LiDAR point clouds to describe the 3D physical features of objects, offering intuitiveness and convenience. Therefore, they are widely used in 3D building modeling, facade measurement, volume measurement, and cubic measurement. In practical applications, if the survey area is large, the limited size of the point cloud data collected per acquisition often necessitates separate data collection. Each collected point cloud is then individually processed using SLAM, and finally, a point cloud stitching algorithm is used to combine the point clouds from multiple projects to generate a complete point cloud. The point cloud stitching algorithm between multiple projects is crucial; it ensures that the point clouds from independently acquired projects, when stitched together, exhibit no layering between different projects.

[0003] The classic point cloud stitching algorithm is the rigid matching algorithm. The rigid matching algorithm assumes that the point cloud to be matched is a rigid body, and the relative pose between the two point clouds can be described by a translation parameter and a rotation parameter. Commonly used point cloud matching algorithms mainly include: (1) Direct matching method represented by ICP (Iterative Closest Point) algorithm and its variants. The cost function of the classic ICP algorithm iterative optimization is the distance value of the matching point pair in two frames of point clouds. The optimal pose transformation is obtained by minimizing the distance value of the matching point pair. The ICP algorithm is very sensitive to the accuracy of the initial value of the distance value. For the initial value with poor accuracy, its iteration may converge to the wrong result. Since each iteration needs to re-match and search for the nearest matching point, the time cost of this algorithm is large and it is a first-order convergence. (2) Matching method based on the mathematical characteristics of point clouds represented by NDT (Normal Distribution Transform) algorithm and its variants. The NDT algorithm utilizes the Gaussian distribution characteristics of point clouds to obtain the optimal transformation by maximizing the probability density function of matching point pairs. It does not require searching for a matching point for each individual point, thus improving computational speed and having lower requirements for initial value accuracy compared to the ICP algorithm. The Multilevel Normal Transform (ML-NDT) algorithm divides the point cloud into different hierarchical units and replaces the cost function with Mahalanobis distance, improving matching accuracy but increasing time overhead.

[0004] However, due to the fundamental principles of laser SLAM algorithms, the rigid matching algorithm described above is not suitable for stitching together point clouds calculated by two laser SLAM algorithms. Laser SLAM algorithms obtain relative poses between frames by performing relative matching on point clouds in a sequence, and finally accumulate these relative poses to obtain the global pose. This stage involves recursive matching using laser odometry, which leads to a continuous increase in accumulated error. When using this global pose to solve point clouds, due to the different matching accuracies between frames, the accuracy of the final SLAM-calculated point cloud varies between local areas, and it cannot be considered a rigid body. If only a translation parameter and a rotation parameter are used to describe the relative pose between the two sets of SLAM-calculated point clouds, the stitched point cloud will have good local consistency, but the overall stitching consistency will not achieve optimal consistency. Summary of the Invention

[0005] This invention provides a graph-optimized multi-engineering laser SLAM point cloud stitching scheme, which aims to solve the problem that the rigid matching algorithm provided by the existing technology is not suitable for stitching point clouds calculated by two laser SLAMs, and the overall point cloud cannot be regarded as a rigid body, resulting in the inability to achieve the best stitching consistency.

[0006] To achieve the above objectives, according to a first aspect of the present invention, a graph-optimized multi-engineering laser SLAM point cloud stitching method is proposed, comprising:

[0007] Multi-frame laser point cloud is stitched together to obtain submap point cloud. Submap point cloud is used as keyframe for single-engineer laser SLAM loop closure detection to obtain single-engineer loop closure detection constraints.

[0008] Determine whether there is an overlap between any two related sets of projects in a multi-project point cloud;

[0009] If it is determined that there is an overlapping point cloud between any two related projects, then the submap point cloud at the overlapping point cloud is spliced ​​according to the generalized iterative nearest point algorithm to obtain the multi-project splicing constraint.

[0010] Determine if a GPS control point exists in the submap point cloud;

[0011] If it is determined that there are GPS control points in the submap point cloud, then the GPS control point constraints are calculated based on the GPS control points associated with the submap point cloud.

[0012] A graph optimization model for the submap point cloud is established using single-project loop closure detection constraints, multi-project splicing constraints, and GPS control point constraints.

[0013] The Ceres nonlinear optimization library is used to solve the graph optimization model to obtain the pose correction amount of the submap point cloud. The pose correction amount is then used to correct the local pose of each single project in the multi-project point cloud.

[0014] Preferably, in the above multi-engineering laser SLAM point cloud stitching method, the step of performing single-engineering laser SLAM loop closure detection using the submap point cloud as keyframes to obtain single-engineering loop closure detection constraints includes:

[0015] The feature matching algorithm is used to match the inter-frame point clouds in a single-project point cloud to obtain the relative pose of the inter-frame point clouds.

[0016] A submap point cloud is obtained by stitching together a predetermined number of inter-frame point clouds based on their relative poses.

[0017] Based on the relative pose of the submap point cloud, the loop closure detection error of the submap point cloud is calculated using a loop closure detection algorithm, and the loop closure detection constraint for a single project is obtained.

[0018] Preferably, in the above multi-project laser SLAM point cloud stitching method, the step of using a feature matching algorithm to match the inter-frame point clouds in a single-project point cloud to obtain the relative pose of the inter-frame point clouds includes:

[0019] According to the point cloud computing formula:

[0020] P i =R·P i+1 +P

[0021] The target point cloud frame is calculated, where P i+1 For the point cloud frame to be matched, P i Let P be the target point cloud frame, and R be the translation parameter and R be the rotation parameter.

[0022] Using the point cloud frame to be matched and the target point cloud frame, according to the formula for the distance from a point to a plane:

[0023]

[0024] The formula for the distance from a point to a line:

[0025]

[0026] The distance from the point to the plane and the distance from the point to the line are calculated separately, where d p and d e These represent the distance from a point to a plane and the distance from a point to a line, respectively. For the feature points in the point cloud frame to be matched, and For feature points in the target point cloud frame;

[0027] The distance residual function is obtained by using the distance from a point to a plane and the distance from a point to a line;

[0028] The distance residual function is solved using the least squares method to obtain the relative pose of the point cloud between frames.

[0029] Preferably, in the above multi-project laser SLAM point cloud stitching method, the step of calculating the loop closure detection error of the submap point cloud using a loop closure detection algorithm based on the relative pose of the submap point cloud, and obtaining the single-project loop closure detection constraint, includes:

[0030] According to the loop closure detection error formula:

[0031]

[0032] The loop closure detection error is calculated, where T i and T j To determine the relative pose of two submap point clouds associated with loop closure detection. This is the error in loop closure detection;

[0033] Using the loop closure detection error, optimize the equation based on the loop closure detection constraint:

[0034]

[0035] The single-project closure detection constraints are calculated, where, Let F be the covariance matrix of the laparoscing constraint. l This is a constraint for single-project loopback detection.

[0036] Preferably, the above-mentioned multi-engineering laser SLAM point cloud stitching method, in the step of stitching the submap point cloud at the overlapping point cloud according to the generalized iterative nearest point algorithm to obtain the multi-engineering stitching constraints, includes:

[0037] According to the multi-project splicing error formula:

[0038]

[0039] The multi-project splicing error was calculated, where T i and T j The poses of the two submap point clouds associated with the target project and the project to be stitched, respectively. To account for splicing errors in multiple projects;

[0040] Using multi-project splicing errors, optimize the equations based on multi-project constraints:

[0041]

[0042] The multi-project splicing constraints are calculated, among which, For the multi-engineering constraint covariance matrix, F m Constraints for splicing multiple projects.

[0043] Preferably, in the above multi-project laser SLAM point cloud stitching method, the step of calculating GPS control point constraints based on the GPS control points associated with the submap point cloud includes:

[0044] According to the formula for calculating control point error:

[0045]

[0046] The GPS control point error is calculated, where P c and P i These represent the GPS control point location and the associated submap point cloud observation point location, respectively. This refers to GPS control point errors;

[0047] Based on the GPS control point constraint optimization equation:

[0048]

[0049] The GPS control point constraints are calculated, where, Let F be the covariance matrix of GPS control points. c For GPS control point constraints.

[0050] Preferably, in the above-mentioned multi-project laser SLAM point cloud stitching method, the steps for establishing a graph optimization model of the submap point cloud using single-project loop closure detection constraints, multi-project stitching constraints, and GPS control point constraints include:

[0051] Optimize equations using graphs:

[0052] F = F l +F m +F c

[0053] Establish a graph optimization model for the submap point cloud; where F l For single-project closure detection constraints, F m For multi-project splicing constraints, F c Given GPS control point constraints, F is the graph optimization model of the submap point cloud.

[0054] According to a second aspect of the present invention, the present invention also provides a graph-optimized multi-engineering laser SLAM point cloud stitching system, comprising:

[0055] The point cloud processing module is used to stitch together multiple frames of laser point cloud to obtain a submap point cloud, and use the submap point cloud as a key frame to perform single-engine laser SLAM loop closure detection to obtain single-engine loop closure detection constraints.

[0056] The point cloud judgment module is used to determine whether there are overlapping point clouds between any two related groups of projects in a multi-project point cloud.

[0057] The point cloud stitching module is used to stitch the submap point cloud at the overlapping point cloud according to the generalized iterative nearest point algorithm if the point cloud judgment module determines that there is an overlapping point cloud between any two related projects, so as to obtain the multi-project stitching constraint.

[0058] The control point determination module is used to determine whether GPS control points exist in the submap point cloud;

[0059] The constraint calculation module is used to calculate GPS control point constraints based on the GPS control points associated with the submap point cloud if the control point judgment module determines that there are GPS control points in the submap point cloud.

[0060] The model building module is used to build a graph optimization model of the submap point cloud using single-project loop closure detection constraints, multi-project splicing constraints, and GPS control point constraints.

[0061] The pose correction calculation module is used to solve the graph optimization model using the Ceres nonlinear optimization library to obtain the pose correction of the submap point cloud. The pose correction is then used to correct the local pose of each single project in the multi-project point cloud.

[0062] Preferably, in the above-mentioned multi-project laser SLAM point cloud stitching system, the point cloud solving module includes:

[0063] The point cloud matching submodule is used to match the inter-frame point clouds in a single project point cloud using a feature matching algorithm to obtain the relative pose of the inter-frame point clouds.

[0064] The point cloud stitching submodule is used to stitch together a predetermined number of inter-frame point clouds according to their relative poses to obtain a submap point cloud.

[0065] The error detection submodule is used to calculate the loop closure detection error of the submap point cloud based on the relative pose of the submap point cloud using a loop closure detection algorithm, and obtain the single-project loop closure detection constraint.

[0066] According to a third aspect of the present invention, the present invention also provides a graph-optimized multi-engineering laser SLAM point cloud stitching system, comprising:

[0067] The system includes a memory, a processor, and a graph-optimized multi-engineering laser SLAM point cloud stitching program stored in the memory and running on the processor. When the multi-engineering laser SLAM point cloud stitching program is executed by the processor, it implements the steps of the multi-engineering laser SLAM point cloud stitching method described in any of the above technical solutions.

[0068] In summary, the graph-optimized multi-project laser SLAM point cloud stitching scheme provided by the present invention addresses the issue that the laser SLAM point cloud of a single project is not a rigid point cloud. By stitching together multiple frames of laser point clouds to obtain a submap point cloud, which can be considered a rigid body, single-project laser SLAM loop closure detection can be performed using the submap point cloud as a keyframe, thus obtaining single-project loop closure detection constraints. For overlapping point clouds between any two related projects in a multi-project setup, the submap point cloud at the overlapping point cloud is stitched together using the generalized iterative nearest-point algorithm to obtain multi-project stitching constraints, achieving optimal matching of multiple non-square point clouds. If GPS control points exist, GPS control point constraints are calculated based on the GPS control points associated with the submap point cloud. The single-project loop closure detection constraints, multi-project stitching constraints, and GPS control point constraints are then incorporated into the graph optimization to establish a graph optimization model for the submap point cloud. The pose correction amount of the submap point cloud can then be obtained by solving the problem using the Ceres nonlinear optimization library. This pose correction amount is then used to correct the local pose of each single project, ensuring the consistency of the single-project point cloud while achieving the best match with the multi-project point cloud, and simultaneously achieving the best fit with the GPS control point positions. Attached Figure Description

[0069] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0070] Figure 1 This is a flowchart illustrating a graph-optimized multi-engineering laser SLAM point cloud stitching method provided in an embodiment of the present invention.

[0071] Figure 2 This is a flowchart illustrating a single-project laser SLAM loop closure detection method for submap point clouds provided in an embodiment of the present invention;

[0072] Figure 3 yes Figure 2 The illustrated embodiment provides a flowchart of a method for matching point clouds between frames in a single-project point cloud.

[0073] Figure 4 yes Figure 2 The illustrated embodiment provides a flowchart of a method for calculating loop closure detection error in a submap point cloud.

[0074] Figure 5 yes Figure 1The illustrated embodiment provides a flowchart of a submap point cloud stitching method;

[0075] Figure 6 yes Figure 1 The illustrated embodiment provides a flowchart of a method for calculating GPS control point constraints.

[0076] Figure 7 This is a schematic diagram of the structure of the first graph-optimized multi-engineering laser SLAM point cloud stitching system provided in this embodiment of the invention;

[0077] Figure 8 yes Figure 7 The illustrated embodiment provides a structural diagram of a point cloud solving module;

[0078] Figure 9 This is a schematic diagram of the structure of the second graph-optimized multi-engineering laser SLAM point cloud stitching system provided in the embodiments of the present invention.

[0079] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0080] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0081] The main technical problem solved by the embodiments of the present invention is:

[0082] Most current point cloud stitching algorithms are rigid matching algorithms. Rigid matching algorithms assume the point clouds to be matched are rigid bodies, and the relative pose between two point clouds can be described by a translation parameter and a rotation parameter. Commonly used point cloud matching algorithms include direct matching methods represented by the ICP algorithm and its variants, and matching methods based on the mathematical characteristics of point clouds represented by the NDT algorithm and its variants. However, due to the fundamental principles of laser SLAM algorithms, the above-mentioned rigid matching algorithms are not suitable for stitching point clouds solved by two laser SLAM algorithms. Laser SLAM algorithms obtain the relative pose between frames by performing relative matching on the point clouds of a sequence of frames, and finally accumulate the relative poses to obtain the global pose. This stage is a recursive matching using laser odometry. Recursive matching leads to a continuous increase in accumulated error. When using this global pose to solve the point cloud, due to the different matching accuracy between frames, the accuracy of the local parts of the final SLAM-solved point cloud varies, and it cannot be considered a rigid body, thus the above-mentioned rigid matching algorithm cannot be used. The above-mentioned rigid matching algorithms cannot treat the entire point cloud as a rigid body, and the overall stitching consistency requirement cannot be achieved.

[0083] To address the aforementioned issues and considering the characteristics of multi-project laser SLAM mapping, the following embodiments of the present invention provide a graph-optimized multi-project laser SLAM point cloud stitching scheme. By treating the point cloud obtained from a single-project laser SLAM as a non-rigid point cloud, the submap point cloud is used as the matching unit during stitching. Then, a large-scale graph optimization model is constructed using single-project loop closure constraints, multi-project stitching constraints, and GPS control point constraints. Finally, the graph optimization model is solved through nonlinear optimization to obtain the pose correction of the submap point cloud. This allows for optimal matching between multi-project laser SLAM point clouds and optimal fitting between the model and GPS control points while maintaining the overall consistency of the single-project point cloud.

[0084] To achieve the above objectives, please refer to Figure 1 , Figure 1 This is a flowchart illustrating a graph-optimized multi-engineering laser SLAM point cloud stitching method provided in an embodiment of the present invention. Figure 1 As shown, this graph-optimized multi-engineering laser SLAM point cloud stitching method includes:

[0085] S110: Multi-frame laser point cloud is stitched together to obtain submap point cloud. The submap point cloud is used as keyframes for single-engineer laser SLAM loop closure detection to obtain single-engineer loop closure detection constraints.

[0086] The above process constitutes single-engineering SLAM solution. Using a single-engineering loop closure detection algorithm, and with the submap point cloud as the basic unit, single-engineering SLAM solution is performed to obtain single-engineering loop closure detection constraints, thereby obtaining the global trajectory position and global point cloud. The aforementioned submap point cloud is obtained by adjusting the relative pose of the point clouds between frames and accumulating point clouds from multiple frames. Since the number of accumulated frames is relatively small (e.g., laser frame point clouds within 1 second), the accumulated error of the submap point cloud can be ignored and considered as a rigid body, thus serving as the basic unit for pose optimization using a rigid matching algorithm.

[0087] Specifically, as a preferred embodiment, such as Figure 2 As shown, in the above multi-engineering laser SLAM point cloud stitching method, step S110: using the submap point cloud as keyframes to perform single-engineering laser SLAM loop closure detection and obtain single-engineering loop closure detection constraints includes:

[0088] S111: Use a feature matching algorithm to match the inter-frame point clouds in a single-project point cloud to obtain the relative pose of the inter-frame point clouds. Single-project SLAM solution first uses a feature-based matching algorithm to match the inter-frame point clouds and obtain their relative poses.

[0089] Specifically, such as Figure 3As shown, the steps for using a feature matching algorithm to match inter-frame point clouds in a single-project point cloud to obtain the relative pose of the inter-frame point clouds include:

[0090] S1111: Assume there is a point cloud frame P to be matched. i+1 and target point cloud frame P i The relative pose T between two frames can be represented using a translation parameter P and a rotation parameter R, according to the point cloud computing formula:

[0091] P i =R·P i+1 +P

[0092] The target point cloud frame is calculated, where P i+1 For the point cloud frame to be matched, P i Let P be the target point cloud frame, and R be the translation parameter and R be the rotation parameter.

[0093] After obtaining the above target point cloud frame P i Then, using the formulas for the distance from a point to a plane and the distance from a point to a line, we can calculate the distance from a point to a plane and the distance from a point to a line, respectively. The specific steps are as follows:

[0094] S1112: Using the point cloud frame to be matched and the target point cloud frame, according to the point-to-plane distance formula:

[0095]

[0096] The formula for the distance from a point to a line:

[0097]

[0098] The distance from the point to the plane and the distance from the point to the line are calculated separately, where d p and d e These represent the distance from a point to a plane and the distance from a point to a line, respectively. For the feature points in the point cloud frame to be matched, and The feature points in the target point cloud frame.

[0099] S1113: Use the distance from a point to a plane and the distance from a point to a line to obtain the distance residual function;

[0100] S1114: Use the least squares method to solve the distance residual function to obtain the relative pose of the point cloud between frames.

[0101] Specifically, let f(x) denote the distance residual function, thus transforming the above line-surface residuals into a nonlinear least squares problem:

[0102]

[0103]

[0104] By solving the aforementioned least squares problem, the relative poses of the point clouds between frames can be obtained. This process is the rigid matching stage of the point clouds between frames.

[0105] After obtaining the relative poses of the point clouds between frames, Figure 2 The method provided in the illustrated embodiment further includes:

[0106] S112: Based on the relative pose of the inter-frame point clouds, stitch together a predetermined number of inter-frame point clouds to obtain the submap point cloud.

[0107] In this embodiment, accumulating a predetermined number of inter-frame point clouds (e.g., 10 frames) allows the relative poses of these inter-frame point clouds to be used to adjust their positional relationships and stitch them together to generate a submap point cloud. Since the point clouds within the submap are recursively matched using a feature-based matching algorithm, the accumulated number of frames is relatively small. Therefore, the cumulative error of the submap point cloud generated by stitching together these 10 frames (approximately 1 second of laser frame data) is considered negligible, and thus, the submap point cloud can be regarded as a rigid body. Furthermore, the submap point cloud obtained by merging 10 frames is denser, which is more conducive to feature extraction and matching, and therefore can serve as the basic unit for subsequent pose optimization.

[0108] After obtaining the submap point cloud Figure 2 The method used in the illustrated embodiment further includes:

[0109] S113: Based on the relative pose of the submap point cloud, use the loop closure detection algorithm to calculate the loop closure detection error of the submap point cloud and obtain the single-project loop closure detection constraint.

[0110] To reduce the cumulative error of point cloud stitching in a single project, this embodiment uses a loop closure detection algorithm to eliminate the cumulative error. The loop closure detection algorithm stitches together the point clouds of revisited regions within a single project, combining the layered point clouds of the revisited regions to further correct the overall pose of the submap point cloud, thereby reducing the cumulative error. The single-project loop closure detection algorithm is based on the submap point cloud and also uses a feature-based matching algorithm for stitching.

[0111] Specifically, as a preferred embodiment, such as Figure 4 As shown, in the above multi-project laser SLAM point cloud stitching method, step S113: calculating the loop closure detection error of the submap point cloud using a loop closure detection algorithm based on the relative pose of the submap point cloud, and obtaining the single-project loop closure detection constraint includes the following steps:

[0112] S1131: According to the loop closure detection error formula:

[0113]

[0114] The loop closure detection error is calculated, where T i T j To determine the relative pose of two submap point clouds associated with loop closure detection. This is the error in loop closure detection;

[0115] S1132: Using loop closure detection error, optimize the equation based on loop closure detection constraints:

[0116]

[0117] The single-project closure detection constraints are calculated, where, Let F be the covariance matrix of the laparoscing constraint. l This is a constraint for single-project loopback detection.

[0118] Using the above method, a submap point cloud can be obtained by stitching together. This submap point cloud can be regarded as a rigid body. In this way, a single-project SLAM solution can be performed using the submap point cloud as the basic unit to obtain the single-project loop closure detection constraint. Then, the single-project loop closure detection constraint is used to constrain the single-project point cloud to reduce the cumulative error and obtain the pose of the single-project point cloud.

[0119] After obtaining the single-project closure detection constraints Figure 1 The multi-engineering laser SLAM point cloud stitching method provided in the illustrated embodiment also includes:

[0120] S120: Determine whether there is an overlapping point cloud between any two related groups of projects in the multi-project point cloud. If there is an overlapping point cloud between the two groups of projects, then perform relative stitching using rigid bodies such as the submap point cloud as units to obtain the local pose correction amount.

[0121] S130: If it is determined that there is an overlapping point cloud between any two related project groups, then the submap point cloud at the overlapping point cloud is stitched together according to the generalized iterative nearest point algorithm (GICP) to obtain the multi-project stitching constraint. By extracting the submap point cloud at the overlapping point cloud, the above submap point cloud is relatively stitched together. The stitching algorithm selected is the generalized iterative nearest point algorithm (GICP) to obtain the multi-project stitching constraint.

[0122] Specifically, as a preferred embodiment, such as Figure 5 As shown, the steps described above for obtaining multi-project stitching constraints by stitching together the submap point cloud at the overlapping point cloud using the generalized iterative nearest point algorithm include:

[0123] S131: According to the multi-project splicing error formula:

[0124]

[0125] The multi-project splicing error was calculated, where T i and T j The poses of the two submap point clouds associated with the target project and the project to be stitched, respectively. To account for splicing errors in multiple projects;

[0126] S132: Optimize the equations based on multi-project splicing errors and multi-project constraints.

[0127]

[0128] The multi-project splicing constraints are calculated, among which, For the multi-engineering constraint covariance matrix, F m Constraints for splicing multiple projects.

[0129] The technical solution provided in this application embodiment matches overlapping point clouds of multiple projects. If there are layers in the overlapping areas of multiple projects, the processing can stitch the layered point clouds of multiple single projects to the middle position of the layers to obtain the local pose correction amount, that is, to ensure that the layered point clouds are stitched together.

[0130] After obtaining multi-project splicing constraints during splicing. Figure 1 The embodiment shown provides a multi-engineering laser SLAM point cloud stitching method, which also includes:

[0131] S140: Determine whether there are GPS control points in the submap point cloud; if there are GPS control points, the error between the submap point cloud and GPS can be calculated, and the submap point cloud can be made closer to the absolute coordinates by reducing the error.

[0132] S150: If it is determined that there are GPS control points in the submap point cloud, the GPS control point constraints are calculated based on the GPS control points associated with the submap point cloud.

[0133] Among them, GPS control point constraints can pull the trajectory in the local coordinate system obtained by SLAM to the position of GPS control points, so that the SLAM-solved point cloud has the global pose in the GPS coordinate system.

[0134] Specifically, as a preferred embodiment, such as Figure 6 As shown, the steps for calculating GPS control point constraints based on GPS control points associated with the submap point cloud include:

[0135] S151: According to the control point error calculation formula:

[0136]

[0137] The GPS control point error is calculated, where P c and P i These represent the GPS control point location and the associated submap point cloud observation point location, respectively. This refers to GPS control point errors;

[0138] S152: Optimization equation based on GPS control point constraints:

[0139]

[0140] The GPS control point constraints are calculated, where, Let F be the covariance matrix of GPS control points. c For GPS control point constraints.

[0141] The technical solution provided in this application embodiment obtains GPS control point constraints according to the control point error calculation formula, and then uses the GPS control point constraints to pull the point cloud trajectory in the local coordinate system obtained by SLAM to the GPS control point position, so that the point cloud obtained by SLAM has a global pose in the GPS coordinate system.

[0142] After calculating the GPS control point constraints Figure 1 The multi-engineering laser SLAM point cloud stitching method provided in the illustrated embodiment also includes:

[0143] S160: Establish a graph optimization model for the submap point cloud using single-project loop closure detection constraints, multi-project splicing constraints, and GPS control point constraints.

[0144] For graph optimization solutions, in this embodiment, single-project loop closure constraints, multi-project stitching constraints, and GPS control point constraints are added to the graph optimization model to obtain the final optimization equation form. Specifically, in this multi-project laser SLAM point cloud stitching method, the steps of establishing a graph optimization model of the submap point cloud using single-project loop closure constraints, multi-project stitching constraints, and GPS control point constraints include:

[0145] Optimize equations using graphs:

[0146] F = F l +F m +F c

[0147] Establish a graph optimization model for the submap point cloud; where F l For single-project closure detection constraints, F m For multi-project splicing constraints, F c Given GPS control point constraints, F is the graph optimization model of the submap point cloud.

[0148] Since the basic unit associated with all the optimization equations is the pose of a submap point cloud, the above optimization problem becomes a classic graph optimization problem. The graph optimization model combines the three constraints mentioned earlier—single-project closure constraint, multi-project stitching constraint, and GPS control point constraint—and solves them using nonlinear least squares. Here, the Ceres nonlinear optimization library is used to solve the graph optimization equations; the specific calculation process will not be detailed here, yielding the pose correction of the submap point cloud. Therefore… Figure 1 The multi-engineering laser SLAM point cloud stitching method provided in the illustrated embodiment also includes:

[0149] S170: Use the Ceres nonlinear optimization library to solve the graph optimization model, obtain the pose correction amount of the submap point cloud, and use the pose correction amount to correct the local pose of each single project of the multi-project point cloud.

[0150] Since the basic unit associated with all optimization equations is the pose of a submap point cloud, the above optimization problem is transformed into a classic graph optimization problem. The graph optimization model is solved using the Ceres nonlinear optimization library. Since Ceres nonlinear optimization is a commonly used algorithm, it will not be elaborated further. Finally, the pose correction amount with submap as the unit can be obtained, which corrects the local pose of each single project, ensuring the consistency of the point cloud of a single project and the consistency of the point cloud of multiple projects, while achieving the best fit with the GPS control point position.

[0151] In summary, the graph-optimized multi-project laser SLAM point cloud stitching method provided in the above embodiments of the present invention, because the laser SLAM point cloud of a single project is not a rigid point cloud, obtains a submap point cloud by stitching together multiple frames of laser point clouds. This submap point cloud can be regarded as a rigid body, thus enabling single-project SLAM solution to be performed on the submap point cloud as a unit, obtaining single-project loop closure detection constraints. For overlapping point clouds between any two related projects in a multi-project context, the submap point cloud at the overlapping point cloud is stitched together according to the generalized iterative nearest point algorithm to obtain multi-project stitching constraints, achieving optimal matching of multiple non-square point clouds. If GPS control points exist, the GPS control point constraints are calculated. The single-project loop closure detection constraints, multi-project stitching constraints, and GPS control point constraints are then incorporated into the graph optimization to establish a graph optimization model for the submap point cloud. The pose correction amount of the submap point cloud can then be obtained by solving the Ceres nonlinear optimization library. This pose correction amount is then used to correct the local pose of each single project, ensuring the consistency of the single-project point cloud while achieving the best match with the multi-project point cloud, and simultaneously achieving the best fit with the GPS control point position.

[0152] Based on the same concept of the above method embodiments, the present invention also provides a graph-optimized multi-engineering laser SLAM point cloud stitching system to implement the above method of the present invention. Since the principle and method of solving the problem in this system embodiment are similar, it has at least all the beneficial effects brought about by the technical solutions of the above embodiments, and will not be described in detail here.

[0153] See also: Figure 7 , Figure 7 This is a schematic diagram of a graph-optimized multi-engineering laser SLAM point cloud stitching system provided in an embodiment of the present invention. Figure 7 As shown, the graph-optimized multi-engineering laser SLAM point cloud stitching system includes:

[0154] The point cloud solving module 110 is used to stitch together multiple frames of laser point cloud to obtain a submap point cloud, and use the submap point cloud as a key frame to perform single-engine laser SLAM loop closure detection to obtain single-engine loop closure detection constraints.

[0155] The point cloud judgment module 120 is used to determine whether there are overlapping point clouds between any two related groups of projects in a multi-project point cloud;

[0156] The point cloud stitching module 130 is used to stitch the submap point cloud at the overlapping point cloud according to the generalized iterative nearest point algorithm if the point cloud judgment module determines that there is an overlapping point cloud between any two related projects, so as to obtain the multi-project stitching constraint.

[0157] The control point determination module 140 is used to determine whether GPS control points exist in the submap point cloud.

[0158] The constraint calculation module 150 is used to calculate the GPS control point constraint based on the GPS control point associated with the submap point cloud if the control point judgment module determines that there are GPS control points in the submap point cloud.

[0159] The model building module 160 is used to build a graph optimization model of the submap point cloud using single-project loop closure detection constraints, multi-project splicing constraints, and GPS control point constraints.

[0160] The pose correction calculation module 170 is used to solve the graph optimization model using the Ceres nonlinear optimization library to obtain the pose correction of the submap point cloud, and to correct the local pose of each single project of the multi-project point cloud using the pose correction.

[0161] Among them, as a preferred embodiment, such as... Figure 8 As shown, in the above-mentioned multi-project laser SLAM point cloud stitching system, the point cloud solving module 110 specifically includes:

[0162] The point cloud matching submodule 111 is used to match the inter-frame point clouds in a single-project point cloud using a feature matching algorithm to obtain the relative pose of the inter-frame point clouds.

[0163] The point cloud stitching submodule 112 is used to stitch together a predetermined number of inter-frame point clouds according to the relative pose of the inter-frame point clouds to obtain the submap point cloud.

[0164] The error detection submodule 113 is used to calculate the loop closure detection error of the submap point cloud based on the relative pose of the submap point cloud using a loop closure detection algorithm, and obtain the single-project loop closure detection constraint.

[0165] In summary, the graph-optimized multi-engineering laser SLAM point cloud stitching system provided in this embodiment of the invention is mainly composed of the following four parts:

[0166] Single-project SLAM solution, multi-project point cloud stitching, GPS control point constraints and graph optimization overall adjustment.

[0167] First, single-project SLAM is performed using submap point clouds as basic units to obtain the global trajectory pose and submap point clouds. Loop closure detection constraints are added to reduce accumulated errors. A submap point cloud is typically formed by merging 1 second of laser frame data. Internally, the submap point cloud is recursively matched using a feature-based matching algorithm. The merged submap is considered a rigid body, serving as the basic unit for subsequent pose adjustment optimization. Then, for overlapping areas between multiple projects, relative stitching is performed using submaps as units to obtain local pose corrections. If GPS control points are provided, the multi-project relative stitching constraints and GPS control point constraints are added to the graph optimization. Finally, a large-scale graph optimization model is established by combining the loop closure detection constraints from single-project mapping, the multi-project relative stitching constraints, and the GPS control point constraints, and solved using the Ceres nonlinear optimization library.

[0168] In addition, such as Figure 9 As shown, the present invention also provides a graph-optimized multi-engineering laser SLAM point cloud stitching system, comprising:

[0169] The processor 1001, communication bus 1002, communication module 1003, memory 1004, and a graph-optimized multi-engineering laser SLAM point cloud stitching program stored in memory 1004 and running on processor 1001, wherein when the multi-engineering laser SLAM point cloud stitching program is executed by processor 1001, it implements the steps of the multi-engineering laser SLAM point cloud stitching method described in any of the above technical solutions.

[0170] In summary, compared with the prior art, this patent has at least two advantages:

[0171] 1. The graph-optimized multi-project stitching algorithm performs local pose correction based on the non-rigid assumption, which not only ensures the relative stitching consistency of multiple sets of point clouds, but also ensures the best fit between the point cloud and the control point position. Compared with the traditional rigid matching algorithm, the point cloud stitching effect of this patented algorithm is better.

[0172] 2. The graph-optimized multi-project stitching algorithm has stronger scalability and can easily incorporate other sensor data for auxiliary matching. For example, adding GPS control point data can bring the point cloud closer to its absolute coordinates. If IMU data is available, IMU pre-integration constraints can be added during the matching stage to assist inter-frame matching, making the recursive pose more accurate.

[0173] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0174] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0175] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0176] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0177] It should be noted that any reference signs placed between parentheses in the claims should not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In a unit claim enumerating several means, several of these means may be embodied by the same item of hardware. The use of the words first, second, and third, etc., does not indicate any order. These words can be interpreted as names.

[0178] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0179] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A multi-engineering laser SLAM point cloud stitching method based on graph optimization, characterized in that, include: Multi-frame laser point cloud is stitched together to obtain submap point cloud. Submap point cloud is used as keyframe for single-engineer laser SLAM loop closure detection to obtain single-engineer loop closure detection constraints. Determine whether there is an overlap between any two related sets of projects in a multi-project point cloud; If it is determined that there is an overlapping point cloud between any two related projects, then the submap point cloud at the overlapping point cloud is stitched together according to the generalized iterative nearest point algorithm to obtain the multi-project stitching constraint. Determine whether the submap point cloud contains GPS control points; If it is determined that there are GPS control points in the submap point cloud, then the GPS control point constraints are calculated based on the GPS control points associated with the submap point cloud. Using the single-project loop closure detection constraint, the multi-project splicing constraint, and the GPS control point constraint, a graph optimization model for the submap point cloud is established. The graph optimization model is solved using the Ceres nonlinear optimization library to obtain the pose correction amount of the submap point cloud. The pose correction amount is then used to correct the local pose of each single project of the multi-project point cloud.

2. The multi-engineering laser SLAM point cloud stitching method according to claim 1, characterized in that, The steps for single-engineering laser SLAM loop closure detection using submap point clouds as keyframes to obtain single-engineering loop closure detection constraints include: The relative pose of the inter-frame point clouds is obtained by matching the inter-frame point clouds in a single-project point cloud using a feature matching algorithm. The submap point cloud is obtained by stitching together a predetermined number of inter-frame point clouds according to their relative poses. Based on the relative pose of the submap point cloud, the loop closure detection error of the submap point cloud is calculated using a loop closure detection algorithm, and the single-project loop closure detection constraint is obtained.

3. The multi-engineering laser SLAM point cloud stitching method according to claim 2, characterized in that, The step of using a feature matching algorithm to match inter-frame point clouds in a single-project point cloud to obtain the relative pose of the inter-frame point clouds includes: According to the point cloud computing formula: P i =R·P i+1 +P The target point cloud frame is calculated, where P i+1 For the point cloud frame to be matched, P i Let P be the target point cloud frame, and R be the translation parameter and R be the rotation parameter. Using the point cloud frame to be matched and the target point cloud frame, according to the point-to-plane distance formula: The formula for the distance from a point to a line: The distance from the point to the plane and the distance from the point to the line are calculated separately, where d p and d e These represent the distance from a point to a plane and the distance from a point to a line, respectively. For the feature points in the point cloud frame to be matched, and For feature points in the target point cloud frame; Using the point-to-plane distance and the point-to-line distance, the distance residual function is obtained; The distance residual function is solved using the least squares method to obtain the relative pose of the inter-frame point cloud.

4. The multi-engineering laser SLAM point cloud stitching method according to claim 2, characterized in that, The step of calculating the loop closure detection error of the submap point cloud based on its relative pose using a loop closure detection algorithm, and obtaining the single-project loop closure detection constraint, includes: According to the loop closure detection error formula: The loop closure detection error is calculated, where T i and T j To determine the relative pose of two submap point clouds associated with loop closure detection. This is the error in loop closure detection; Using the loop closure detection error, optimize the loop closure detection constraint equation as follows: The single-project loop closure detection constraint is calculated, wherein, Let F be the covariance matrix of the laparoscing constraint. l This is a constraint for single-project loopback detection.

5. The multi-engineering laser SLAM point cloud stitching method according to claim 1, characterized in that, The step of stitching the submap point cloud at the overlapping point cloud according to the generalized iterative nearest point algorithm to obtain multi-project stitching constraints includes: According to the multi-project splicing error formula: The multi-project splicing error was calculated, where T i and T j The poses of the two submap point clouds associated with the target project and the project to be stitched, respectively. This refers to the splicing error of the multiple projects; Using the aforementioned multi-project splicing error, optimize according to the multi-project constraint equation: The multi-project splicing constraints are calculated, among which, For the multi-engineering constraint covariance matrix, F m Constraints for splicing multiple projects.

6. The multi-engineering laser SLAM point cloud stitching method according to claim 1, characterized in that, The step of calculating GPS control point constraints based on GPS control points associated with the submap point cloud includes: According to the formula for calculating control point error: The error of the GPS control point is calculated, where P c and P i These represent the GPS control point location and the associated submap point cloud observation point location, respectively. This refers to GPS control point errors; Based on the GPS control point constraint optimization equation: The GPS control point constraints are calculated, where, Let F be the covariance matrix of GPS control points. c The GPS control point is constrained.

7. The multi-engineering laser SLAM point cloud stitching method according to claim 1, characterized in that, The steps for establishing a graph optimization model of the submap point cloud using the single-project loop closure detection constraint, the multi-project stitching constraint, and the GPS control point constraint include: Optimize equations using graphs: F=F l +F m +F c Establish a graph optimization model for the submap point cloud; where F l For single-project closure detection constraints, F m For multi-project splicing constraints, F c Given GPS control point constraints, F is the graph optimization model of the submap point cloud.

8. A multi-engineering laser SLAM point cloud stitching system based on graph optimization, characterized in that, include: The point cloud processing module is used to stitch together multiple frames of laser point cloud to obtain a submap point cloud, and use the submap point cloud as a key frame to perform single-engine laser SLAM loop closure detection to obtain single-engine loop closure detection constraints. The point cloud judgment module is used to determine whether there are overlapping point clouds between any two related groups of projects in a multi-project point cloud. The point cloud stitching module is used to stitch the submap point cloud at the overlapping point cloud according to the generalized iterative nearest point algorithm if the point cloud judgment module determines that there is an overlapping point cloud between any two related projects, so as to obtain the multi-project stitching constraint. The control point determination module is used to determine whether there are GPS control points in the submap point cloud; The constraint calculation module is used to calculate GPS control point constraints based on the GPS control points associated with the submap point cloud if the control point judgment module determines that there are GPS control points in the submap point cloud. The model building module is used to build a graph optimization model of the submap point cloud using the single-project loop closure detection constraint, the multi-project splicing constraint, and the GPS control point constraint. The pose correction calculation module is used to solve the graph optimization model using the Ceres nonlinear optimization library to obtain the pose correction of the submap point cloud, and to use the pose correction to correct the local pose of each single project of the multi-project point cloud.

9. The multi-project laser SLAM point cloud stitching system according to claim 8, characterized in that, The point cloud solving module includes: The point cloud matching submodule is used to match the inter-frame point clouds in a single-project point cloud using a feature matching algorithm to obtain the relative pose of the inter-frame point clouds. The point cloud stitching submodule is used to stitch together a predetermined number of inter-frame point clouds according to the relative pose of the inter-frame point clouds to obtain the submap point cloud. The error detection submodule is used to calculate the loop closure detection error of the submap point cloud based on the relative pose of the submap point cloud using a loop closure detection algorithm, and obtain the single-project loop closure detection constraint.

10. A multi-engineering laser SLAM point cloud stitching system based on graph optimization, characterized in that, include: The system includes a memory, a processor, and a graph-optimized multi-engineering laser SLAM point cloud stitching program stored in the memory and running on the processor, wherein the multi-engineering laser SLAM point cloud stitching program, when executed by the processor, implements the steps of the multi-engineering laser SLAM point cloud stitching method as described in any one of claims 1 to 7.

Images