A method and system for optimizing vehicle-mounted laser point cloud pose graph combined with multi-class constraints

Abstract

The application discloses a kind of combined multi-class constraint's vehicle-mounted laser point cloud pose graph optimization method and system, by the trajectory data of mobile platform statistical analysis, point cloud is segmented using adaptive segmentation mode, and is abstracted as the node in pose graph;Using the intersection ratio of point cloud segmentation bounding box obtains rough segmented revisit relationship, adopts the same name matching relationship between the key point detection based on curvature and local point cloud feature descriptor in the inter-revisit segment;Sample virtual observation point between adjacent segments, construct smooth constraint edge, combine matching homonyme point initialization revisit corresponding matching edge, construct multi-type constraint's pose graph model, finally realize the global adjustment of vehicle-mounted laser point cloud data.The application can automatically find revisit area from vehicle-mounted laser point cloud data, and construct multi-type constraint's pose graph optimization model, realize the global optimization adjustment of vehicle-mounted laser point cloud, provide high-precision map data support for city road analysis and other applications.

Description

Technical Field

[0001] This invention belongs to the interdisciplinary field of machine vision, robot mapping, and vehicle-mounted laser point cloud data processing, and specifically relates to a method and system for optimizing vehicle-mounted laser point cloud pose graphs by combining multiple types of constraints. Background Technology

[0002] Laser point cloud data holds immense potential in applications such as urban infrastructure construction, road analysis, traffic analysis, and urban green space assessment. Vehicle-mounted laser scanning systems, with their high efficiency, flexibility, and high precision, have become one of the most widely used systems for acquiring geometric information of large-scale scenes. These tasks all presuppose that the laser point cloud data has a consistent spatial location.

[0003] The vehicle-mounted laser scanning system consists of hardware such as a Global Navigation Satellite System (GNSS), an inertial navigation system, and a laser scanning system, as well as software such as a time synchronization system, all integrated on a mobile platform. The laser scanner mounted on the vehicle-mounted laser point cloud is typically a high-precision 2D laser scanner that scans the scene through high-frequency rotation and acquires the scene's three-dimensional geometric information while the mobile platform is moving. During data acquisition, a reference GNSS receiver first stably and continuously receives GNSS satellite positioning data. Then, the mobile platform corrects its GNSS positioning value by receiving real-time differential values ​​from the base station, thus achieving real-time and accurate acquisition of the geographical location during the data acquisition process. Therefore, the positioning accuracy directly determines the positional accuracy of the acquired point cloud.

[0004] However, in urban "canyons," towering buildings obscure the satellite constellation distribution observed by vehicle-mounted laser scanning systems. Glass curtain walls and dense vegetation can cause GNSS signal obstruction and severe multipath effects, significantly reducing GNSS positioning accuracy in urban scenarios. Furthermore, existing urban roads are typically designed with two-way, multi-channel configurations, requiring multiple data collection trips across different lanes of the same road. This decrease in GNSS positioning accuracy leads to inconsistencies in the location of laser point cloud data obtained by the mobile measurement system through direct geoorientation when encountering data loops. This manifests as ghosting, tortuosity, or even distortion in the data collected from the same area, severely impacting subsequent data processing and practical application analysis. Summary of the Invention

[0005] To address the positional deviation problem in vehicle-mounted laser point cloud data, this invention provides a method and system for optimizing the pose graph of vehicle-mounted laser point cloud data by combining multiple types of constraints. Using segmented vehicle-mounted laser point cloud data as the basic processing node unit, a point cloud matching algorithm based on feature points is used to obtain corresponding points of the same name between revisited segments. Virtual sampling points are set between adjacent segments. A pose graph model with multiple types of constraint edges is constructed based on the matching points and virtual points. Through global optimization adjustment, a point cloud map of a large urban scene is constructed.

[0006] To achieve the above objectives, the present invention provides a method for optimizing the pose graph of a vehicle-mounted laser point cloud by combining multiple constraints, comprising the following steps:

[0007] Step 1: The original laser point cloud data is divided into non-overlapping point cloud segments using an adaptive segmentation method, and the point cloud segments are abstracted as nodes in the pose graph model.

[0008] Step 2: Obtain the 2D bounding box of the point cloud segment based on the coordinates of the point cloud within each segment. By calculating the intersection-union ratio between the bounding boxes of two point cloud segments, obtain a rough segment revisit relationship.

[0009] Step 3: Use a curvature-based key point detection method to obtain key points in the point cloud segmentation;

[0010] Step 4: Use a point cloud feature descriptor matching method based on local geometric features to obtain the same-name matching relationship between revisited segments;

[0011] Step 5: Construct lap-loop constraint edges based on the same-name matching relationships between revisited segments;

[0012] Step 6: Set virtual sampling points between adjacent segments and construct smooth constraint edges;

[0013] Step 7: Optimize the pose graph to achieve global optimization adjustment of the vehicle-mounted laser point cloud.

[0014] Furthermore, in step 1, the velocity and acceleration status information of the mobile platform is first statistically analyzed to identify locations with drastic changes in velocity and acceleration as candidate points for point cloud segmentation. Then, trajectory points with drastic instantaneous direction changes in the trajectory data are also included as candidate points for point cloud segmentation. The two sets of candidate segmentation points are then non-uniformly sampled using filtering to obtain the final segmentation points. Based on the final segmentation points, the original vehicle-mounted laser point cloud data is divided into point cloud segments of unequal length, and each point cloud segment is abstracted as a node in a pose graph model. The definition of a node is:

[0015] S:={S1,...,S i ,...,S n ,n=|S} (1)

[0016] In the formula, S represents the set of point cloud segments, S i Let |S| represent the i-th point cloud segment, which contains the pose T corresponding to this point cloud segment and the point cloud data it contains. n and |S| represent the total number of point cloud segments.

[0017] The pose T corresponding to a point cloud segment is defined as follows:

[0018]

[0019] In the formula, R∈SO(3) is a rotation matrix, which belongs to the three-dimensional special orthogonal group space; is a translation vector, belonging to three-dimensional space; pose T∈SE(3) represents the transformation between the current point cloud segment pose state and the correct point cloud segment pose state, belonging to three-dimensional special Euclidean group space.

[0020] Furthermore, in step 2, the intersection-union ratio between the two point cloud segment bounding boxes is calculated to obtain a rough segment revisit relationship, thus obtaining the initial candidate revisit segments. The judgment method is as follows:

[0021]

[0022] In the formula, Represents the segmentation S of the point cloud i Let S represent the set of point cloud segments with revisit relationships, and n be the total number of segments. This indicates that the point cloud is divided into two segments S. i S j The cross-union ratio (CUR) is calculated from the bounding boxes, where δ1 is the CUR threshold between two point cloud segment bounding boxes, and τ(S) is the cross-union ratio. i ,S j ) is an indicator function, if the point cloud is segmented into S i S j If the intersection-union ratio is greater than the threshold δ1, then a revisited relationship is considered to exist, τ(S i ,S j ) = 1, otherwise it does not constitute a revisit, τ(S) i ,S j ) = 0.

[0023] Furthermore, in step 3, each point in the point cloud segment is searched. Points within the surrounding neighborhood form a neighborhood point set. The covariance matrix M of a point is calculated using points in the neighborhood point set. i Principal component analysis (PCA) is used to obtain the eigenvalues ​​and eigenvectors of the neighborhood point set, and then the approximate curvature corresponding to that point is calculated. The formula for calculating the approximate curvature is as follows:

[0024]

[0025] In the formula, C represents the curvature of the point, and λ1>λ2>λ3 are the eigenvalues ​​arranged in descending order.

[0026] The point with the maximum local curvature is obtained by suppressing local maxima and is used as the key point. The key point is defined as follows:

[0027]

[0028] In the formula, A set of key points Let |P| represent the t-th key point, and |P| represent the number of key points.

[0029] Furthermore, step 4 employs a method based on local triorthogonal plane projection to encode local geometric spatial features as feature descriptors for key points. The specific calculation method is as follows: Local neighborhood points of the current key point are searched; principal component analysis is used to calculate the principal direction; a local independent coordinate system is constructed with the current key point as the origin, the principal direction as the X-axis, the direction perpendicular to the ground upwards as the Y-axis, and the orthogonal direction of the X-axis and Y-axis as the Z-axis. The local neighborhood points are then transformed into this local independent coordinate system, and projected onto three orthogonal planes XOY, XOZ, and YOZ respectively. The projected points on each plane are then rasterized and statistically analyzed. The density and projection distance of points within each raster are normalized to values ​​within the range of 0-255. These values ​​are further converted into binary values ​​within the range of 0-1 using a random feature difference test. The binary values ​​from the three projection planes are concatenated to obtain the binary point cloud feature descriptor for the current key point. The feature descriptors of key points within each point cloud segment are aggregated to obtain the feature descriptor for that point cloud segment. The aggregation method is as follows:

[0030]

[0031] In the formula, F(S) k ) represents a point cloud segment S k The feature descriptor subset, ∪ is the union operation, and k′ is the point cloud segmentation S. k The k′-th key point in F k′ For the binary feature descriptor of the k′-th key point, Segmenting the point cloud k The number of key points contained therein.

[0032] By calculating the Hamming distance between two binary feature descriptors and using bidirectional feature selection, the corresponding relationships Ω between revisited segments can be obtained. p ,Right now:

[0033]

[0034] In the formula, Segmenting the point cloud i S j The set of key points, p t q t′ They respectively represent belonging to the set of key points The key point, F t F t′ Indicates key point p t q t′ The binary feature descriptor, F(S) i ), F(S j ) represents a point cloud segment S i S j Feature descriptor subset, Min hm (A,B) represents the key point selected from the feature descriptor subset B that has the smallest Hamming distance to the binary feature descriptor A.

[0035] Using the SVD decomposition of the bidirectional feature selection to obtain the same-name matching relation matrix, the relative pose transformation between two revisited point cloud segments can be obtained.

[0036] Furthermore, the formula for calculating the cost function of the loop constraint edge in step 5 is as follows:

[0037]

[0038] ω p =-log(d hm / D f (9)

[0039] In the formula, E r Let T be the cost function for the loop constraint edge; n is the total number of segments; τ is an indicator function, which is 1 if there is a revisit relationship between two segments, and 0 otherwise; i ,T j ,p k ,q k Segmentation Pose and matching feature homonyms; |Ω p | represents the number of matching points with the same name contained in the current revisited segment; the (·) operator indicates that the current point p will be revisited. k According to posture T i ,T j The process of transforming from the source coordinate system to the target coordinate system; Represented as segment S i The total number of revisited segments; ||||2 is the Euclidean distance between the two points; ω p D represents the weight of a pair of matching points with the same name. f d represents the dimension of the feature descriptor. hmThe Hamming distance is between two binary feature descriptors; log() represents the logarithmic function.

[0040] Furthermore, in step 6, rays are emitted in eight directions—upper left, lower left, upper right, lower right, upper left, lower left, lower right, upper left, lower left, upper right, and lower right—centered on the segment point and using the direction of the trajectory line as the starting direction. Multiple samples are taken along these eight rays, each using a fixed distance, which varies between batches. Residual connection vectors are constructed between sampling points belonging to different segments. Each set of eight sampled points forms a cube, and the corresponding true vector is obtained based on the cube's side length. This allows for the construction of a smooth constraint edge cost function for the virtual sampling points, as follows:

[0041]

[0042] In the formula, E s The cost function for smoothing constraint edges; n is the total number of segments; N sp This represents the number of virtual sampling points near the segmentation point; Each represents a segment S i-1 and S i Virtual sampling points; for A predefined distance vector between them; T i ,T i-1 These are two segments S i and S i-1 The corresponding pose; the (·) operator indicates that the current point According to posture T i ,T i-1 Perform the transformation; ||||2 is the Euclidean distance between the two points.

[0043] Moreover, the pose graph in step 7 The optimal parameter solution is expressed in the following form:

[0044] T * =argmaxp(Z|T) (11)

[0045] In the formula, T * Let represent the optimal pose parameter solution, and argmaxp(Z|T) represent the maximum conditional probability of observing Z under the condition that T is satisfied. This represents the set of segmented poses of a point cloud. Z represents a node in the pose graph, Z:={Z ij ∈SE(3)|(i,j)∈ε} is the set of observations between point cloud segments, and ε represents the edge in the pose graph.

[0046] The cost function for constructing the entire pose graph model is obtained by globally adjusting the vehicle-mounted laser point cloud data. The optimal pose parameter solution is then obtained. The cost function E for the entire pose graph model is calculated as follows:

[0047] E=ω r E r +ω s E s (12)

[0048] In the formula, E r E s These are the closure constraint edges formed by matching corresponding points between revisited segments and the smooth constraint edges formed by virtual sampling points between adjacent segments, respectively. ω r ω s These are the weighting factors for the revisit cost edge and the smoothing cost edge, respectively.

[0049] The present invention also provides a vehicle-mounted laser point cloud pose graph optimization system with multiple constraints, which is used to implement the vehicle-mounted laser point cloud pose graph optimization method with multiple constraints as described above.

[0050] Furthermore, it includes a processor and a memory, the memory being used to store program instructions, and the processor being used to call the stored instructions in the memory to execute the vehicle-mounted laser point cloud pose graph optimization method with joint multi-class constraints as described above.

[0051] Alternatively, it may include a readable storage medium storing a computer program that, when executed, implements the vehicle-mounted laser point cloud pose graph optimization method with multiple constraints as described above.

[0052] Compared with the prior art, the present invention has the following advantages:

[0053] 1) This invention utilizes the same-name matching edges between revisited segments and the virtual same-name point constraint edges between adjacent segments to construct a pose graph model with multiple types of constraints, thereby realizing the construction of a vehicle-mounted laser point cloud data map with consistent spatial geometry in a large urban scene.

[0054] 2) This invention combines matching points of the same name between segments and virtual sampling points between adjacent segments. By setting different weights for matching points of the same name and virtual sampling points, a robust global pose graph model can be solved, and the results can be displayed from both global and local perspectives.

[0055] 3) This invention is highly robust to urban road target scenarios with repetitive, symmetrical, and simple structures. It can achieve excellent results in improving the geometric position accuracy of vehicle-mounted laser point cloud data collected in various urban road scenarios. It can be applied to major tasks such as urban high-precision point cloud map construction, urban forest green volume estimation, and urban road analysis. Attached Figure Description

[0056] Figure 1 This is an overall technical flowchart of an embodiment of the present invention.

[0057] Figure 2 This is a schematic diagram of the matching corresponding points extracted by revisiting segments in an embodiment of the present invention.

[0058] Figure 3 This is a schematic diagram of the virtual sampling point generation process according to an embodiment of the present invention, wherein... Figure 3 (a) represents the sampling points and trajectory points. Figure 3 (b) represents the set of sampling points for a single segment point. Figure 3 (c) indicates that the residual vectors of sampling points belonging to different segments are connected.

[0059] Figure 4 This is a schematic diagram of the pose graph model constructed according to an embodiment of the present invention.

[0060] Figure 5 This is a comparison chart of the results of an embodiment of the present invention, wherein... Figure 5 (a-1), 5(b-1), 5(c-1), 5(d-1), and 5(e-1) are all positional relationships with the same name in the original vehicle-mounted laser point cloud. Figure 5 (a-2), 5(b-2), 5(c-2), 5(d-2), and 5(e-2) are all positional relationships of the same name in the point cloud after optimization using the method proposed in this invention. Detailed Implementation

[0061] This invention provides a method and system for optimizing the pose graph of a vehicle-mounted laser point cloud using multiple constraints. For the task of optimizing the global pose graph of a vehicle-mounted laser point cloud, the original laser point cloud data is segmented into non-overlapping point cloud segments using an adaptive segmentation approach. These segments serve as nodes, and corresponding matching points are obtained through feature point matching to achieve initial transformation estimation. Revisit constraint edges based on matching corresponding points are constructed, and smooth constraint edges between adjacent segments are built using virtual sampling points. Global adjustment of the laser point cloud is achieved through pose graph solving. The technical solution of this invention will be further described below with reference to the accompanying drawings and embodiments.

[0062] Example 1

[0063] like Figure 1 As shown, this invention provides a method for optimizing the pose of a vehicle-mounted laser point cloud based on multiple constraints, comprising the following steps:

[0064] Step 1: The original laser point cloud data is divided into non-overlapping point cloud segments using an adaptive segmentation method, and the point cloud segments are abstracted as nodes in the pose graph model.

[0065] An adaptive segmentation method is adopted to adaptively segment the point cloud into point cloud segments that satisfy internal positional consistency. There are no overlapping areas between segments, and the segments have varying lengths. These segments are abstracted as nodes in the pose graph model. First, statistical analysis is performed on the velocity and acceleration state information of the mobile platform to identify locations with drastic changes in velocity and acceleration as candidate points for point cloud segmentation. Then, trajectory points with drastic instantaneous direction changes in the trajectory data are also included as candidate points. Non-uniform sampling is performed on both candidate segments using filtering to obtain the final segment points. Based on the final segment points, the original vehicle-mounted laser point cloud data is segmented into point cloud segments of unequal length, and each point cloud segment is abstracted as a node in the pose graph model. The node is defined as follows:

[0066] S:={S1,...,S i ,...,S n ,n=|S} (1)

[0067] In the formula, S represents the set of point cloud segments, S i Let represent the i-th point cloud segment (node), which contains the pose T corresponding to this point cloud segment (node) and the point cloud data it contains. n and |S| represent the total number of point cloud segments.

[0068] The pose T corresponding to a point cloud segment (node) is defined as follows:

[0069]

[0070] In the formula, R∈SO(3) is a rotation matrix, which belongs to the three-dimensional special orthogonal group space; It is a translation vector and belongs to three-dimensional space; pose T∈SE(3) represents the transformation between the current point cloud segment (node) pose state and the correct point cloud segment (node) pose state, and belongs to three-dimensional special Euclidean group space.

[0071] Step 2: Obtain the 2D bounding box of the point cloud segment based on the coordinates of the point cloud within each segment. By calculating the intersection-union ratio between the bounding boxes of two point cloud segments, obtain a rough segment revisit relationship.

[0072] Calculate the intersection-union ratio (IoU) between the bounding boxes of two point cloud segments to obtain a rough segment revisit relationship and obtain the initial candidate segments for revisiting. The judgment method is as follows:

[0073]

[0074] In the formula, Represents the segmentation S of the point cloud i Let S represent the set of point cloud segments with revisit relationships, and n be the total number of segments. This indicates that the point cloud is divided into two segments S.i S j The cross-union ratio (CUR) is calculated from the bounding boxes, where δ1 is the CUR threshold between two point cloud segment bounding boxes, and τ(S) is the cross-union ratio. i ,S j ) is an indicator function, if the point cloud is segmented into S i S j If the intersection-union ratio is greater than the threshold δ1, then a revisited relationship is considered to exist, τ(S i ,S j ) = 1, otherwise it does not constitute a revisit, τ(S) i ,S j ) = 0.

[0075] Step 3: Use a curvature-based key point detection method to obtain key points in the point cloud segment.

[0076] Find each point in a point cloud segment Points within the surrounding neighborhood form a neighborhood point set. The covariance matrix M of a point is calculated using points in the neighborhood point set. i Principal component analysis (PCA) is used to obtain the eigenvalues ​​and eigenvectors of the neighborhood point set, and then the approximate curvature corresponding to that point is calculated. The formula for calculating the approximate curvature is as follows:

[0077]

[0078] In the formula, C represents the curvature of the point, and λ1>λ2>λ3 are the eigenvalues ​​arranged in descending order.

[0079] The point with the maximum local curvature is obtained by suppressing local maxima and is used as the key point. The key point is defined as follows:

[0080]

[0081] In the formula, A set of key points Let |P| represent the t-th key point, and |P| represent the number of key points.

[0082] Step 4: Use point cloud feature descriptors based on local geometric features to obtain the same-name matching relationship between revisited segments.

[0083] Local geometric spatial features are encoded using a local triorthogonal plane projection method as the feature descriptor for keypoints. The specific calculation method is as follows: Local neighborhood points of the current keypoint are searched, and principal component analysis is used to calculate the principal direction. A local independent coordinate system is constructed with the current keypoint as the origin, the principal direction as the X-axis, the direction perpendicular to the ground upwards as the Y-axis, and the orthogonal direction of the X-axis and Y-axis as the Z-axis. The local neighborhood points are transformed into this local independent coordinate system and projected onto three orthogonal planes XOY, XOZ, and YOZ respectively. The projected points on each plane are then rasterized and statistically analyzed. The density and projection distance of points within each grid are normalized to values ​​within the range of 0-255. These values ​​are further converted into binary values ​​within the range of 0-1 using a random feature difference test. The binary values ​​from the three projection planes are concatenated to obtain the binary point cloud feature descriptor for the current keypoint. The feature descriptors of keypoints within each point cloud segment are aggregated to obtain the feature descriptor for that point cloud segment. The aggregation method is as follows:

[0084]

[0085] In the formula, F(S) k ) represents a point cloud segment S k The feature descriptor subset, ∪ is the union operation, and k′ is the point cloud segmentation S. k The k′-th key point in F k′ For the binary feature descriptor of the k′-th key point, Segmenting the point cloud k The number of key points contained therein.

[0086] By calculating the Hamming distance between two binary feature descriptors and using bidirectional feature selection, the corresponding relationships Ω between revisited segments can be obtained. p ,Right now:

[0087]

[0088] In the formula, Segmenting the point cloud i S j The set of key points, p t q t′ They respectively represent belonging to the set of key points The key point, F t F t′ Indicates key point p t q t′ The binary feature descriptor, F(S) i ), F(S j ) represents a point cloud segment S i S j Feature descriptor subset, Minhm (A,B) represents the key point selected from the feature descriptor subset B that has the smallest Hamming distance to the binary feature descriptor A.

[0089] The final matching result is determined by Figure 2 As shown, the relative pose transformation between two revisited point cloud segments can be obtained by using the SVD decomposition of the corresponding matching matrix obtained from bidirectional feature selection.

[0090] Step 5: Based on the same-name matching relationship between revisited segments, construct the lap-loop constraint edge cost function for the revisited segments.

[0091] The formula for calculating the cost function of the loop constraint edge is as follows:

[0092]

[0093] ω p =-log(d hm / D f (9)

[0094] In the formula, E r Let T be the cost function for the loop constraint edge; n is the total number of segments; τ is an indicator function, which is 1 if there is a revisit relationship between two segments, and 0 otherwise; i ,T j ,p k ,q k Segmentation Pose and matching feature homonyms; |Ω p | represents the number of matching points with the same name contained in the current revisited segment; the (·) operator adds the current point p to the list of matching points with the same name. k According to posture T i ,T j The process of transforming from the source coordinate system to the target coordinate system; Represented as segment S i The total number of revisited segments; ||||2 is the Euclidean distance between the two points; ω p D represents the weight of a pair of matching points with the same name. f d represents the dimension of the feature descriptor. hm The Hamming distance is between two binary feature descriptors; log() represents the logarithmic function.

[0095] Step 6: Set virtual sampling points between adjacent segments and construct a smooth edge cost function.

[0096] like Figure 3As shown, with the segment point as the center and the direction of the trajectory line as the starting direction, rays are emitted in eight directions: left front-up, left front-down, right front-up, right front-down, left rear-up, left rear-down, right rear-up, and right rear-down. Multiple samples are taken along these eight rays, each using a fixed distance, which varies between batches. Residual connection vectors are constructed between sampling points belonging to different segments. Each set of eight sampled points forms a cube, and the corresponding true vector is obtained based on the cube's side length. This allows the construction of a smooth constraint edge cost function for the virtual sampling points, as follows:

[0097]

[0098] In the formula, E s The cost function for smoothing constraint edges; n is the total number of segments; N sp This represents the number of virtual sampling points near the segmentation point; Each represents a segment S i-1 and S i Virtual sampling points; for A predefined distance vector between them; T i ,T i-1 These are two segments S i and S i-1 The corresponding pose; the (·) operator indicates that the current point According to posture T i ,T i-1 Perform the transformation; ||||2 is the Euclidean distance between the two points.

[0099] Step 7: Optimize the pose graph using the cost function of matching corresponding points with loop constraints and the cost function of virtual sampling points with smooth constraints, thereby achieving global optimization adjustment of the vehicle-mounted laser point cloud.

[0100] The constructed pose graph model is as follows Figure 4 As shown, S i This represents the i-th point cloud segment (node). S represents i The revisited point cloud segments (nodes), S represents i The adjacent point cloud segments (nodes) are connected by arrows. The arrowed line represents the smooth constraint edge formed by the virtual sampling points between adjacent segments, while the line without arrows represents the loop constraint edge formed by the matching corresponding points between revisited segments.

[0101] pose diagram The optimal parameter solution can be expressed as follows:

[0102] T *=argmaxp(Z|T) (11)

[0103] In the formula, T * Let represent the optimal pose parameter solution, and argmaxp(Z|T) represent the maximum conditional probability of observing Z under the condition that T is satisfied. This represents the set of segmented poses of a point cloud. Z represents a node in the pose graph, Z:={Z ij ∈SE(3)|(i,j)∈ε} is the set of observations between point cloud segments, and ε represents the edge in the pose graph.

[0104] The cost function for constructing the entire pose graph model is obtained by globally adjusting the vehicle-mounted laser point cloud data. The optimal pose parameter solution is then obtained. The cost function E for the entire pose graph model is calculated as follows:

[0105] E=ω r E r +ω s E s (12)

[0106] In the formula, E r E s These are the closure constraint edges formed by matching corresponding points between revisited segments and the smooth constraint edges formed by virtual sampling points between adjacent segments, respectively. ω r ω s These are the weighting factors for the revisit cost edge and the smoothing cost edge, respectively. In this embodiment, ω is taken as... r =0.16, ω s =0.84.

[0107] Figure 5 (a-1), 5(b-1), 5(c-1), 5(d-1), and 5(e-1) represent the corresponding positional relationships in the original vehicle-mounted laser point cloud. Figure 5 (a-2), 5(b-2), 5(c-2), 5(d-2), and 5(e-2) represent the corresponding positional relationships in the point cloud after optimization using the method proposed in this invention. The above comparison results demonstrate that the method proposed in this invention can effectively improve the matching accuracy of corresponding points.

[0108] Example 2

[0109] Based on a unified inventive concept, this invention also provides a vehicle-mounted laser point cloud pose optimization system with multiple constraints, including a processor and a memory. The memory is used to store program instructions, and the processor is used to call the stored instructions in the memory to execute the vehicle-mounted laser point cloud pose optimization method with multiple constraints as described above.

[0110] Example 3

[0111] Based on a unified inventive concept, the present invention also provides a vehicle-mounted laser point cloud pose graph optimization system with multiple constraints, including a readable storage medium on which a computer program is stored. When the computer program is executed, it implements the vehicle-mounted laser point cloud pose graph optimization method with multiple constraints as described above.

[0112] In specific implementation, the method proposed in the technical solution of this invention can be automatically executed by those skilled in the art using computer software technology. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of this invention and computer equipment including the computer program running the corresponding computer program, should also be within the protection scope of this invention.

[0113] The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which this invention pertains may make various modifications or additions to the described specific embodiments or use similar methods to replace them, without departing from the spirit of the invention or exceeding the scope defined by the appended claims.

Claims

1. A method for vehicle laser point cloud pose graph optimization combined with multi-class constraints, characterized in that, Comprise the following steps: Step 1, the original laser point cloud data is segmented into non-overlapping point cloud segments in an adaptive segmentation manner, and the point cloud segments are abstracted into nodes in the pose graph model; Step 2, the 2D bounding box of the point cloud segment is obtained according to the coordinates of the point cloud in each segment, and the intersection-over-union between the bounding boxes of two point cloud segments is calculated to obtain the rough revisit relationship of the segments; Step 3, the key points in the point cloud segment are obtained by using a curvature-based key point detection method; Step 4, a local geometric feature-based point cloud feature descriptor matching method is used to obtain the homonymic matching relationship between the revisit segments; The local geometric space features are encoded based on local three orthogonal plane projection, which are used as the feature descriptors of the key points, and the specific calculation method is as follows: search the local neighborhood points of the current key point, calculate the principal direction using principal component analysis, construct a local independent coordinate system with the current key point as the origin, the principal direction as the X-axis, and the direction perpendicular to the ground upward as the Y-axis, and the orthogonal direction of the X-axis and the Y-axis as the Z-axis, and transform the local neighborhood points to the local independent coordinate system, then project the neighborhood points onto three orthogonal planes XOY, XOZ and YOZ, and then grid the projected points on each plane to obtain the density and projection distance of the points in each grid, and normalize them to the range of 0-255, and further convert them to binary values in the range of 0-1 through random feature difference test, and concatenate the binary values in the three projection planes to obtain the binary point cloud feature descriptor of the current key point; the feature descriptors of the key points in each point cloud segment are aggregated to obtain the feature descriptor of the point cloud segment, and the aggregation method is as follows: （6） wherein, representing a set of feature descriptors of a point cloud segment, is a union operation, representing a point cloud segment, is the i-th key point in the point cloud segment, is the i-th key point in the point cloud segment, is the i-th key point in the point cloud segment, is the i-th key point in the point cloud segment, is the i-th key point in the point cloud segment, is the number of key points contained in the point cloud segment.​ The same-name corresponding relationship between revisit segments can be obtained by calculating the Hamming distance between two binary feature descriptors and using a bidirectional feature selection manner That is: （7） wherein, , is a keypoint set of the point cloud segment , respectively represents a keypoint belonging to the keypoint set , , , , is a binary feature descriptor of the keypoint , , is a feature descriptor set of the point cloud segment B A corresponding to the keypoint with the smallest Hamming distance to the binary feature descriptor​​​​​​ The relative pose transformation between two revisit point cloud segments can be obtained by using SVD decomposition on the homonymic matching relationship matrix obtained by bidirectional feature selection; Step 5, construct loop closure constraint edges according to the homonymic matching relationship between the revisit segments; The loop closure constraint edge cost function is calculated as follows: （8） （9） wherein, is the loop constraint edge cost function; n is the total number of segments; is an indicator function, if there is a revisit relationship between two segments, is 1, otherwise is 0; are the poses and matching feature correspondences of segment respectively; is the number of matching correspondences contained in the current revisit segment; operator denotes the transformation of the current point according to the pose the process of transforming from the source coordinate system to the target coordinate system; denotes the total number of revisit segments for segment ; is the Euclidean distance between two points; is the weight of a pair of matching correspondences; is the dimension of the feature descriptor; is the Hamming distance between two binary feature descriptors; log( ) denotes the logarithm function; Step 6, set virtual sampling points between adjacent segments and construct smooth constraint edges; Step 7, optimize the pose graph to achieve global optimization adjustment of the vehicle-mounted laser point cloud. 2.The method of claim 1, wherein: In step 1, the velocity and acceleration state information of the mobile platform is statistically analyzed, and the positions with sharp changes in velocity and acceleration are found as point cloud segment candidate points, then the trajectory points with sharp changes in instantaneous direction in the trajectory data are also taken as point cloud segment candidate points, and the two parts of candidate segment points are non-uniformly sampled through filtering to obtain the final segment points; according to the final segment points, the original vehicle-mounted laser point cloud data is segmented into point cloud segments of different lengths, and each point cloud segment is abstracted into a node in the pose graph model; the definition of the node is as follows: （1） wherein, denotes a set of point cloud segments, denotes a first i point cloud segment comprising a pose corresponding to the present point cloud segment and comprising point cloud data, n and denotes the number of all point cloud segments; Pose corresponding to point cloud segmentation is defined as: （2） wherein is a rotation matrix belonging to the three-dimensional special orthogonal group space; is a translation vector belonging to the three-dimensional space; pose denotes a transformation from the current point cloud segment pose state to the correct point cloud segment pose state, belonging to the three-dimensional special Euclidean group space. 3.The method of claim 1, wherein: The intersection over union between two point cloud segment bounding boxes is calculated in step 2 to obtain a rough segment revisit relationship, and an initial revisit candidate segment is obtained The judgment mode is: （3） wherein, denotes point cloud segments a set of point cloud segments with revisit relationship, denotes a set of point cloud segments, n is the total number of segments, denotes two point cloud segments , intersection over union calculated from the bounding boxes of the two point cloud segments, is the intersection over union threshold between the bounding boxes of two point cloud segments, is an indicator function, if the intersection over union value of the two point cloud segments , is greater than the threshold , it is considered to have a revisit relationship, , otherwise it does not constitute a revisit, . 4.The method of claim 1, wherein: Finding the point cloud segment of each point in step 3 Points in the surrounding neighborhood form a neighborhood point set, and the covariance matrix of the point is calculated using the points in the neighborhood point set Eigenvalues and eigenvectors of the neighborhood point set are obtained by principal component analysis, and the approximate curvature corresponding to the point is calculated, and the approximate curvature calculation formula is as follows: （4） wherein C the curvature of the point of representation, are the eigenvalues in descending order; The point with the maximum local curvature is obtained by local maximum suppression operation as the key point, and the definition of the key point is as follows: （5） In the formula, is a set of key points, represents the i-th key point, t represents the number of key points.​ 5. The method of claim 1, wherein: In step 6, taking the segment point as the center, the direction of the trajectory line as the initial direction, the rays are emitted in 8 directions of left front up, left front down, right front up, right front down, left rear up, left rear down, right rear up and right rear down, and multiple samplings are performed on the 8 direction rays, each sampling using a fixed distance, and the fixed distances used in different batches of sampling being different; the residual connection vectors are constructed between the sampling points belonging to different segments, 8 points of each sampling forming a cube, and the corresponding real vector being obtained according to the side length of the cube, so as to construct the smooth constraint edge cost function of the virtual sampling point, and the following is realized: （10） where, is the smoothness constraint cost function; n is the total number of segments; is the number of virtual sampling points around a segment point; are the virtual sampling points of segment and respectively; is the predefined distance vector between ; are the poses of segment and respectively; is the operator that transforms the current point according to the pose ; is the Euclidean distance between two points.

6. The method of claim 1, wherein: Pose graph in step 7 The optimal parameter solution of the form of the pose graph is represented as follows: （11） wherein, denotes the optimal pose parameter solution, denotes the maximum value of the conditional probability of observing Z given T, denotes the set of point cloud segment poses, denotes a node in the pose graph, is a set of observations between point cloud segments, denotes an edge in the pose graph; The cost function of the whole pose graph model is used for global adjustment of the vehicle-mounted laser point cloud data to obtain an optimal pose parameter solution; the cost function of the whole pose graph model E The calculation is as follows: （12） wherein , are loop closure constraint edges between matched common points between revisit segments and smooth constraint edges between adjacent segments, respectively, and , are weight factors for the loop closure cost edges and the smooth cost edges, respectively. 7.A system for joint multi-class constraint vehicle-mounted laser point cloud pose graph optimization, characterized in that, The processor and the memory are included, the memory is used for storing program instructions, and the processor is used for calling the storage instructions in the memory to execute the joint multi-class constraint vehicle-mounted laser point cloud pose graph optimization method according to any one of claims 1-6. 8.A system for joint multi-class constraint vehicle-mounted laser point cloud pose graph optimization, characterized in that, The readable storage medium is included, and the computer program is stored on the readable storage medium, and the computer program is executed to realize the joint multi-class constraint vehicle-mounted laser point cloud pose graph optimization method according to any one of claims 1-6.

Images