A posture optimization method and device

Abstract

This specification discloses a posture optimization method and device. According to the acquisition order, for each frame point cloud that has been collected, feature extraction is performed on the frame point cloud, and each static target and its type in the frame point cloud is determined. And from the several frame point clouds optimized before the frame point cloud, at least one frame point cloud and the frame point cloud are determined as matching point clouds. Based on at least the distance between static targets belonging to the same type and belonging to different frame matching point clouds, the matching static target pairs are determined, and based on the distance and angle between each static target pair, an optimization objective function is constructed. With the minimum of the optimization objective function as the goal, the posture of at least part of the matching point cloud is adjusted to obtain the optimized posture of at least part of the matching point cloud. It does not rely on global positioning system data, and because all points are not matched in units of points, the amount of calculation is small and the efficiency is high.

Description

Technical Field

[0001] This specification relates to the field of unmanned driving technology, and in particular to a posture optimization method and device. Background Art

[0002] In the field of unmanned driving, map construction depends on the relative poses between the collected point clouds of each frame. The accuracy of the relative poses affects the accuracy of the constructed map, which makes pose optimization an important part of the map construction process.

[0003] Current pose optimization methods typically use an iterative closest point (ICP) algorithm to match point clouds across frames. This method relies on Global Positioning System (GPS) data to determine the initial relative pose between point clouds (i.e., the relative pose between point clouds at the time of initial optimization). Based on this initial value, optimization is performed, iteratively obtaining the optimized relative pose between point clouds.

[0004] However, the optimization results of this method are greatly affected by GPS. In scenarios with no GPS signal or weak GPS signal, such as missing satellite signals or signal interference, the initial value error determined based on GPS data is large, which will have a negative impact on the optimization process and results. In addition, this method has a large amount of calculation and low optimization efficiency. Summary of the Invention

[0005] This specification provides a posture optimization method and device to partially solve the above-mentioned problems existing in the prior art.

[0006] This manual adopts the following technical solutions:

[0007] This specification provides a posture optimization method, including:

[0008] According to the acquisition order of each frame of point cloud, feature extraction is performed on each frame of point cloud in turn to determine each static target and the type of each static target in the frame of point cloud;

[0009] Determine at least one frame of point cloud and the frame of point cloud from among a plurality of frame point clouds optimized before the frame point cloud as a matching point cloud;

[0010] Determine the matched static object pairs based on at least the distances between static objects of the same type and belonging to different frame matching point clouds, and construct an optimization objective function based on the distances and angles between the static object pairs;

[0011] With the goal of minimizing the optimization objective function, the pose of at least a portion of the matching point cloud is adjusted to obtain an optimized pose of the at least portion of the matching point cloud.

[0012] Optionally, determining at least one frame of point cloud and the frame of point cloud as a matching point cloud from among several frame of point clouds optimized before the frame of point cloud specifically includes:

[0013] Determine the previous frame point cloud of the frame point cloud from several frames of point cloud before the frame point cloud;

[0014] The previous frame point cloud and the current frame point cloud are used as matching point clouds.

[0015] Optionally, determining the matched pairs of static objects based at least on distances between static objects of the same type and belonging to matching point clouds of different frames includes:

[0016] Determine the geometric model of each static target in each matching point cloud;

[0017] For each geometric model in each matching point cloud, determine the mass point of the geometric model and the vector that uniquely identifies the direction of the geometric model;

[0018] For each geometric model in the frame point cloud, determine other geometric models of the same type as the geometric model from other geometric models in the previous frame point cloud as similar models of the geometric model;

[0019] Determine the distance and angle between each geometric model and its own similar model based on the initial value of the relative pose between the frame point cloud and the previous frame point cloud, and the mass points and vectors of each geometric model;

[0020] According to the determined distances and angles, matching pairs of static targets are determined.

[0021] Optionally, determining at least one frame of point cloud and the frame of point cloud as a matching point cloud from among several frame of point clouds optimized before the frame of point cloud specifically includes:

[0022] According to a preset interval, several frame point clouds and the frame point cloud are determined from several frame point clouds optimized before the frame point cloud as matching point clouds.

[0023] Optionally, determining the matched pairs of static objects based at least on distances between static objects of the same type and belonging to matching point clouds of different frames includes:

[0024] Determine the geometric model of each static target in each matching point cloud;

[0025] For each geometric model in each matching point cloud, determine the mass point of the geometric model and the vector that uniquely identifies the direction of the geometric model;

[0026] When the frame point cloud is determined to be a key frame based on at least one of the distance and the heading angle difference between the frame point cloud and the previous key frame, for each geometric model in the matching point cloud of each frame, other geometric models of the same type as the geometric model are determined from other geometric models in the matching point clouds of other frames as similar models of the geometric model;

[0027] According to the initial value of the relative pose between the two matching point clouds, the mass points and vectors of the geometric models in each matching point cloud, the distance and angle between each geometric model of each matching point cloud and its own similar model are determined;

[0028] According to the distances and angles between the geometric models of each matching point cloud and their respective similar models, the static target pairs matched between the matching point clouds are determined.

[0029] Optionally, the method further includes:

[0030] When the frame point cloud is determined to be a non-key frame based on at least one of the distance and the heading angle difference between the frame point cloud and the previous key frame, for each geometric model in the frame point cloud, other geometric models of the same type as the geometric model are determined from other geometric models in the matching point clouds of other frames as the same type of models of the geometric model;

[0031] According to the initial values ​​of the relative poses between the frame point cloud and other matching point clouds, and the mass points and vectors of the geometric models in each matching point cloud, the distances and angles between each geometric model of the frame point cloud and its own similar models are determined;

[0032] According to the distance and angle between each geometric model in the frame point cloud and its respective similar models, the static target pairs matched between the frame point cloud and other matching point clouds are determined.

[0033] Optionally, an optimization objective function is constructed based on the distance and angle between each pair of static targets, specifically including:

[0034] An optimization objective function is constructed according to the distance between the mass point of each geometric model in each static target pair and the matching other geometric model, and the angle between the vectors of each geometric model in each static target pair.

[0035] Optionally, before constructing the optimization objective function based on the distances and angles between each pair of static targets, the method further includes:

[0036] The initial value is linearly optimized by a random sampling consensus algorithm to re-determine the initial value of the relative pose between the point cloud of the frame and the point cloud of the previous frame.

[0037] Optionally, adjusting the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain the optimized pose of the at least portion of the matching point cloud specifically includes:

[0038] With the goal of minimizing the optimization objective function, the pose of the frame point cloud is adjusted based on the initial value of the relative pose between the frame point cloud and the previous frame point cloud to obtain the optimized pose of the frame point cloud.

[0039] Optionally, adjusting the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain the optimized pose of the at least portion of the matching point cloud specifically includes:

[0040] When the point cloud of the frame is a key frame, the optimization objective function is minimized, and based on the initial value of the relative pose between the matching point clouds of each frame, the pose of each matching point cloud is adjusted to obtain the optimized pose of each matching point cloud;

[0041] When the frame point cloud is a non-key frame, the optimization objective function is minimized. Based on the initial values ​​of the relative poses between the frame point cloud and other matching point clouds, the pose of the frame point cloud is adjusted to obtain the optimized pose of the frame point cloud.

[0042] This specification provides a posture optimization device, including:

[0043] A feature extraction module is used to extract features from each frame of point cloud according to the order in which each frame of point cloud has been collected, and to determine each static target and its type in the frame of point cloud;

[0044] a determination module, configured to determine at least one frame of point cloud and the frame of point cloud from among a plurality of frame point clouds optimized before the frame of point cloud, as a matching point cloud;

[0045] a matching module for determining matched pairs of static objects based on at least the distances between static objects of the same type and belonging to matching point clouds of different frames, and constructing an optimization objective function based on the distances and angles between the pairs of static objects;

[0046] The optimization module is used to adjust the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain the optimized pose of the at least portion of the matching point cloud.

[0047] This specification provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the above-mentioned posture optimization method is implemented.

[0048] This specification provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned posture optimization method when executing the program.

[0049] At least one of the above technical solutions adopted in this specification can achieve the following beneficial effects:

[0050] In the pose optimization method provided in this specification, feature extraction is performed on each frame of point cloud that has been collected, according to the acquisition order, to determine the static targets and the types of the static targets in the frame of point cloud. At least one frame of point cloud and the frame of point cloud are determined from the multiple frame point clouds optimized before the frame of point cloud as the matching point cloud. Afterwards, based on at least the distance between static targets of the same type and belonging to different frame matching point clouds, the matching pairs of static targets are determined, and an optimization objective function is constructed based on the distance and angle between the pairs of static targets. With the goal of minimizing the optimization objective function, the pose of at least part of the matching point cloud is adjusted to obtain the optimized pose of at least part of the matching point cloud.

[0051] As can be seen from the above method, this method does not rely on GPS data. Even in the case of weak or no GPS signal, it can still match static targets in the point cloud to obtain static target pairs. The objective function is constructed based on the distance and angle between the static target pairs to adjust the pose of the at least partially matched point clouds corresponding to each static target pair, thereby obtaining the optimized pose of the at least partially matched point cloud when the objective function is minimized. This method matches static targets to construct the objective function, rather than matching all points on a point-by-point basis, resulting in low computational complexity and high computational efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The drawings described herein are used to provide a further understanding of this specification and constitute a part of this specification. The exemplary embodiments and descriptions of this specification are used to explain this specification and do not constitute an improper limitation of this specification. In the drawings:

[0053] Figure 1 This is a flow chart of a posture optimization method in this specification;

[0054] Figure 2 A schematic diagram of a posture optimization process provided in this specification;

[0055] Figure 3 A schematic diagram of a posture optimization device provided in this specification;

[0056] Figure 4 This is a structural diagram of an electronic device provided in this manual. DETAILED DESCRIPTION

[0057] At present, when building a map, the relative pose of two frames of point clouds is usually determined frame by frame based on the Global Positioning System (GPS) and the Inertial Measurement Unit (IMU) to obtain the pose of the point cloud. Due to the cumulative error of the IMU, the pose of the determined point cloud also has a cumulative error. In addition, in scenes such as underground, high-rise buildings, and tree-lined roads, there may be missing satellite signals or interference with the signals, resulting in missing or inaccurate GPS data. As a result, the map constructed based on the pose of the point cloud determined by GPS data and IMU data will have ghosting and large errors. Therefore, it is necessary to optimize the pose of each frame of the point cloud.

[0058] In the method of optimizing the point cloud pose based on the Iterative Closest Point (ICP) algorithm, GPS data is still relied on to determine the initial value of the relative pose between each frame of the point cloud, and the optimization result is still affected by GPS. In scenarios with no GPS signal or weak GPS signal, such as missing satellite signals or signal interference, the initial value error determined based on GPS data is large, which will have a negative impact on the optimization process and optimization results, resulting in inaccurate optimization results. In addition, this method requires matching each point in the point cloud with the nearest point in the point cloud of other frames, which requires a large amount of calculation and low optimization efficiency. In addition, this method is easily interfered with by the partial point cloud corresponding to the dynamic target in each frame of the collected point cloud. The position of the moving dynamic target is different in the point cloud of different frames, which will cause matching errors or failures during point cloud matching.

[0059] In this specification, the pose optimization method provided herein is independent of GPS and IMU, and can still optimize and obtain accurate point cloud poses in scenarios where there is no GPS signal or the GPS signal is weak. It can identify static targets (such as tree trunks, pillars, planes, etc.) in each frame of the point cloud, and based on the portion of the point cloud corresponding to the static target in each frame of the point cloud, fit the geometric model of the static target in each frame of the point cloud, and determine matching static target pairs (also obtaining matching geometric model pairs). Based on the matching geometric model pairs, the distance and angle between the geometric models of each static target pair are calculated to construct an optimization objective function. The pose of each frame of the point cloud that minimizes the optimization objective function is obtained, i.e., the optimized pose. A matching static target pair is a pair of static targets in different frame point clouds that correspond to the same target in the real environment, and a geometric model pair is a pair of geometric models in different frame point clouds that correspond to the same target in the real environment. The geometric model is stable. Based on a stable geometric model, the matching between different frame point clouds can be unaffected by noise, resulting in more accurate matching results.

[0060] Furthermore, the pose optimization method provided in this specification avoids errors introduced by GPS and IMU, optimizing the pose of each point cloud frame corresponding to the map to obtain a more accurate map, thereby resolving the problem of map ghosting. Furthermore, rather than calculating the distance between points, this specification uses static objects as units, matching by calculating the distance between their geometric models. This results in matched pairs of static objects, which are then used to construct the optimization objective function. This reduces computational complexity and improves efficiency.

[0061] It should be noted that the pose of the point cloud in this specification refers to the pose of the radar that collects the frame of point cloud when collecting the frame of point cloud.

[0062] To make the objectives, technical solutions, and advantages of this specification more clear, the following will clearly and completely describe the technical solutions of this specification in conjunction with the specific embodiments of this specification and the corresponding drawings. Obviously, the embodiments described are only part of the embodiments of this specification, not all of the embodiments. Based on the embodiments in this specification, all other embodiments obtained by ordinary technicians in this field without making any creative efforts are within the scope of protection of this specification.

[0063] The technical solutions provided by the embodiments of this specification are described in detail below with reference to the accompanying drawings.

[0064] Figure 1 This is a flow chart of a posture optimization method in this specification, which specifically includes the following steps:

[0065] S100: performing feature extraction on each frame of point cloud according to the order of acquisition of each frame of point cloud, and determining each static target and the type of each static target in the frame of point cloud.

[0066] In this specification, the posture optimization method can be executed by a server or by an unmanned driving device. The following description will take the execution of this method by a server as an example.

[0067] In one or more embodiments of this specification, the posture optimization method is used in the back-end optimization process. For several frames of point clouds collected in advance for building a map, the posture optimization method can be used to perform posture optimization frame by frame.

[0068] As mentioned above, this specification performs point cloud matching based on static objects within the point cloud, rather than matching individual points. Furthermore, since dynamic objects can vary in position across point clouds from one frame to the next, this can interfere with matching. Therefore, this specification only identifies static objects within the point cloud to determine which static objects are matched across point clouds from one frame to another. While static objects in point clouds from one frame to another may vary in type, matched static objects should be of the same type.

[0069] Therefore, in one or more embodiments of the present specification, the server may first determine each frame of point cloud that has been collected, and then, based on the order in which the frame of point cloud was collected, perform feature extraction on each frame of point cloud to determine each static object and its type in the frame of point cloud. This facilitates matching the static objects based on their types in subsequent steps to obtain static object pairs.

[0070] In one or more embodiments of this specification, the method for feature extraction is not limited. For example, feature extraction can be performed based on point curvature or through semantic segmentation to determine each static object and its type. Of course, other methods can also be used and this specification does not limit them.

[0071] S102: Determine at least one frame of point cloud and the frame of point cloud from among several frame point clouds optimized before the frame of point cloud as a matching point cloud.

[0072] In one or more embodiments of this specification, since the pose optimization process is frame-by-frame, all point clouds prior to the current point cloud are optimized and have relatively accurate poses. Therefore, the pose of the current point cloud can be optimized by matching the current point cloud with at least a portion of the point clouds optimized prior to the current point cloud.

[0073] Therefore, in one or more embodiments of this specification, the server may determine at least one frame of point cloud from among several frame point clouds optimized before the frame point cloud, and use it as a matching point cloud with the frame point cloud. Matching point clouds are point clouds that are subsequently used to match static objects contained therein and optimize the point cloud pose based on the matching results. However, not all matching point clouds will be used as optimized point clouds.

[0074] It should be noted that since step S102 and step S100 do not interfere with each other, the order of step S100 and step S102 is not limited. Of course, for other steps in this specification, the order of their execution is not limited if they do not interfere with each other.

[0075] S104: determining matching pairs of static objects based at least on the distances between static objects of the same type and belonging to matching point clouds of different frames, and constructing an optimization objective function based on the distances and angles between the pairs of static objects.

[0076] In one or more embodiments of this specification, the position of a point cloud can be represented by a six-degree-of-freedom pose, including position represented by x, y, and z coordinates, and pose represented by heading, roll, and pitch angles. The relative pose between point clouds can be represented by a transformation matrix (e.g., a rotation matrix and a translation matrix) between the point clouds.

[0077] When the relative pose between two frames of point clouds is determined more accurately, the distance between the matched static targets and the angle between them will be smaller after the point clouds are transformed such as rotated and translated according to the relative pose.

[0078] Therefore, in one or more embodiments of this specification, the server may determine matching pairs of static objects based on at least the distances between static objects of the same type and belonging to matching point clouds in different frames, and construct an optimization objective function based on the distances and angles between each pair of static objects. The specific method for determining static object pairs will be described in detail later and is not discussed here.

[0079] In one or more embodiments of the present disclosure, for each static object pair, a residual of the static object pair may be constructed based on the distance and angle between the static objects in the static object pair. Subsequently, the server may construct an optimization objective function by summing the residuals of the static object pairs.

[0080] S106: With the optimization objective function minimized, the pose of at least a portion of the matching point cloud is adjusted to obtain an optimized pose of the at least portion of the matching point cloud.

[0081] In one or more embodiments of the present specification, after determining the optimization objective function, the server may adjust the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain the optimized pose of the at least portion of the matching point cloud.

[0082] based on Figure 1 The pose optimization method shown here performs feature extraction on each frame of point cloud acquired, sequentially according to the acquisition order, to determine the static objects and their types within that frame. At least one frame of point cloud optimized before the frame is then identified as a matching point cloud with the frame. Matching pairs of static objects are then determined based on at least the distance between static objects of the same type but belonging to different matching point clouds. An optimization objective function is then constructed based on the distances and angles between these pairs of static objects. With the goal of minimizing this optimization objective function, the pose of at least a portion of the matching point cloud is adjusted to obtain an optimized pose for the at least portion of the matching point cloud.

[0083] As can be seen from the above method, this method does not rely on GPS data. Even in the case of weak or no GPS signal, it can still match static targets in the point cloud to obtain static target pairs. The objective function is constructed based on the distance and angle between the static target pairs to adjust the pose of the at least partially matched point clouds corresponding to each static target pair, thereby obtaining the optimized pose of the at least partially matched point cloud when the objective function is minimized. This method matches static targets to construct the objective function, rather than matching all points on a point-by-point basis, resulting in low computational complexity and high computational efficiency.

[0084] In addition, in one or more embodiments provided in this specification, when matching between static targets, frame-by-frame matching can be performed, that is, each pair of matched static targets is determined based only on the two frame point clouds, namely, the frame point cloud and the previous frame point cloud. When performing pose optimization based on the matching results, only the pose of the latter frame point cloud of the two frame point clouds can be optimized. Then, in step S102, when determining at least one frame point cloud and the frame point cloud from several frame point clouds optimized before the frame point cloud as matching point clouds, specifically, the server can determine the previous frame point cloud of the frame point cloud from several frame point clouds before the frame point cloud, and use the previous frame point cloud and the frame point cloud as matching point clouds. That is, the so-called frame-by-frame matching is to match the static target in the frame point cloud with the static target in the previous frame point cloud.

[0085] In one or more embodiments of this specification, the server may determine the geometric model of each static target in each matching point cloud. Furthermore, for each geometric model in each matching point cloud, the server may determine the geometric model's mass points and a vector uniquely identifying the geometric model's orientation. For each geometric model, the geometric model may be represented by the mass points corresponding to the geometric model and the vector uniquely identifying the geometric model's orientation. Thus, each static target also corresponds to a mass point and a vector. The determination of mass points and vectors facilitates the subsequent determination of distances and angles between geometric models.

[0086] In one or more embodiments of the present specification, a geometric model of a static object may be determined based on a partial point cloud corresponding to the static object. A mass point of the geometric model may be a center point.

[0087] In one or more embodiments of the present specification, when determining the matched static object pairs in step S104, the server may further determine, for each geometric model in the point cloud frame, other geometric models of the same type as the geometric model from the other geometric models in the point cloud frame before the point cloud frame, as similar models of the geometric model. The server may then determine the distance between each geometric model and its corresponding similar model based on the initial relative pose between the point cloud frame before the point cloud frame, and the mass points and vectors of each geometric model. Finally, based on the determined distances, the matched static object pairs are determined.

[0088] Specifically, the server may determine, for each geometric model in the frame point cloud, the distances between the geometric model and its own similar models. After obtaining the distances to each similar model, the server determines, for each similar model, whether the distance between the geometric model and the similar model is less than a preset distance threshold. If so, the similar model and the geometric model are determined to be a matching pair of geometric models, and the static targets corresponding to the similar model and the geometric model are a static target pair.

[0089] When determining the distance between each geometric model and its own similar model based on the initial value of the relative posture between the point cloud of this frame and the point cloud of the previous frame, the mass points and vectors of each geometric model, the point cloud of each frame is transformed according to the initial value of the relative posture between the point cloud of this frame and the point cloud of the previous frame, and the distance is calculated after the posture transformation.

[0090] The posture transformation includes at least one of translation and rotation.

[0091] In one or more embodiments of the present specification, when there are multiple similar models whose distance to the geometric model is less than the distance threshold, the similar model with the smallest distance can be used as the geometric model that matches the geometric model, and the static target corresponding to the similar model with the smallest distance and the static target corresponding to the geometric model are a static target pair.

[0092] In one or more embodiments of this specification, the static targets determined in step S100 may include pole-shaped targets, planar targets, and the like. For example, linear and columnar objects such as pillars in an underground garage and tree trunks above ground can all be considered pole-shaped targets. Static target types include at least pole-shaped and planar targets. The determined geometric models may include pole-shaped and surface-shaped geometric models. Distance calculation methods differ for different types of geometric models.

[0093] In one or more embodiments of the present specification, the distance between rod-shaped geometric models can be determined by calculating the distance from a point to a line (the distance between a mass point of a rod-shaped geometric model and another matching rod-shaped geometric model), and the distance between plane geometric models can be determined by calculating the distance from a point to a surface (the distance between a mass point of a plane geometric model and another matching plane geometric model).

[0094] In one or more embodiments of this specification, since a single frame of a point cloud typically captures only a portion of the surface point cloud of a rod-shaped object, rather than the entire object, for the same rod-shaped object, the corresponding portions of the point cloud in different frame point clouds may not be identical, or even completely different. To increase the success rate of matching between geometric models, for a rod-shaped static object, a geometric model corresponding to the central axis of the static object can be determined and used as the geometric model of the static object. The mass of this geometric model can then be the center point of the central axis. Regardless of whether the point clouds on the surface of the same rod-shaped static object captured in different frame point clouds are identical, the corresponding central axis is always the same, thereby increasing the success rate and accuracy of matching.

[0095] Of course, considering that the central axis may be determined inaccurately, the geometric model corresponding to the static target may also be determined based on the partial point cloud actually corresponding to the static target.

[0096] In one or more embodiments of the present specification, the formula for determining the distance between rod-shaped geometric models may be specifically as follows:

[0097]

[0098] Where D1 represents the distance between the rod-shaped geometric model in the i-th matching frame point cloud and the rod-shaped geometric model in the j-th matching frame point cloud, P i represents the coordinates of the mass point of the rod-shaped geometric model in the matching point cloud of the i-th frame, P j Represents the coordinates of the mass point of the rod-shaped geometric model in the matching point cloud of the jth frame. Represents the vector of the rod-shaped geometric model in the matching point cloud of the i-th frame, The vector representing the rod-shaped geometric model in the matching point cloud of the jth frame. It is the expression of the distance between the rod-shaped geometric model in the j-th frame point cloud and the rod-shaped geometric model in the i-th frame point cloud. is the distance between the rod-shaped geometric model in the i-th frame point cloud and the rod-shaped geometric model in the j-th frame point cloud. When performing frame-by-frame matching, the i-th frame matching point cloud and the j-th frame matching point cloud are two adjacent frame point clouds.

[0099] In one or more embodiments of this specification, the formula for determining the distance between planar geometric models may be specifically as follows:

[0100] D2=|a i x j +b i y j +c i z j +d i |+|a j x i +b j y i +c j z i +d j |

[0101] Where D2 represents the distance between the plane geometric model in the matching point cloud of the i-th frame and the plane geometric model in the matching point cloud of the j-th frame, and a i 、b i 、c i d i represents the plane parameters of the plane geometric model in the matching point cloud of the i-th frame, a j 、b j 、c j d j Represents the plane parameters of the plane geometric model in the matching point cloud of the jth frame. j 、y j 、z jRepresents the coordinates of the mass point of the plane geometric model of the matching point cloud in the jth frame, x i 、y i 、z i Indicates the coordinates of the particle in the plane geometric model of the i-th frame matching point cloud. When performing frame-by-frame matching, the i-th frame matching point cloud and the j-th frame matching point cloud are two adjacent frame point clouds.

[0102] Alternatively, in order to make the determined static target pairs more accurate, the matched static target pairs may be determined based on the angles between the geometric models of the static targets.

[0103] In one or more embodiments of the present specification, when determining matching pairs of static targets, the server may also filter, for each geometric model in the point cloud of a frame, the other geometric models in the point cloud of the previous frame, to identify other geometric models of the same type as the geometric model from the other geometric models in the point cloud of the previous frame as similar models of the geometric model. The server may then determine the distances and angles between each geometric model and its respective similar models based on the initial values ​​of the relative poses between the point cloud of the frame and the point cloud of the previous frame, and the mass points and vectors of each geometric model. Finally, matching pairs of static targets are determined based on the determined distances and angles.

[0104] In one or more embodiments of this specification, the distances and angles between geometric models in different point cloud frames are calculated by transforming the pose of one of the point cloud frames based on the initial relative poses between the point cloud frames. For example, during frame-to-frame matching, the point cloud frame can be rotated and / or translated based on the initial relative poses of the point cloud frame and the previous point cloud frame before calculating the distances and angles between the geometric models.

[0105] In this specification, the initial value, or initial estimate, of the relative poses between point clouds in different frames, is used during optimization based on the optimization objective function. The relative poses between point clouds are adjusted based on the initial value to ultimately achieve an optimized relative pose that minimizes the optimization objective function. When the optimized relative poses are accurate, the absolute poses of each point cloud determined based on the relative poses are also more accurate.

[0106] In one or more embodiments of the present specification, the angle between geometric models may be determined by the angle between vectors of the geometric models.

[0107] In one or more embodiments of the present specification, when determining matching pairs of static objects based on the distances and angles between each geometric model and its own similar models, the server may specifically determine, for each geometric model, an angle expression value based on the vectors of the geometric model and its own similar models. Similarly, models whose expression values ​​are less than a preset expression value threshold and whose distances from the geometric model are less than a preset distance threshold are considered geometric models that match the geometric model.

[0108] Of course, in one or more embodiments of the present specification, when there are multiple similar models that meet the condition that their expression value is less than a preset expression value threshold and their distance from the geometric model is less than a preset distance threshold, one of the similar models that meet the condition may be determined as the geometric model that matches the geometric model. For example, the similar model that meets the condition and has the smallest angle expression value and the smallest distance from the geometric model may be determined as the geometric model that matches the geometric model.

[0109] In one or more embodiments of this specification, the formula for determining the expression value of the angle may be specifically as follows:

[0110]

[0111] Among them, Ag represents the expression value, Represents the vector of the qth geometric model in the matching point cloud of the i-th frame, The vector of the sth geometric model in the jth frame matching point cloud. The qth geometric model in the ith frame matching point cloud and the sth geometric model in the jth frame matching point cloud form a geometric model pair. When performing frame matching, the ith frame matching point cloud and the jth frame matching point cloud are adjacent frame point clouds.

[0112] Of course, the above formula for calculating distance and angle can also be used when calculating the distance and angle between similar geometric models between two non-adjacent frames of matching point clouds.

[0113] In one or more embodiments of this specification, for a rod-shaped geometric model, the vector that uniquely identifies the orientation of the geometric model may be its principal direction vector, which can be determined using principal component analysis. For a planar geometric model, its normal vector may be determined to be the vector that uniquely identifies the orientation of the geometric model.

[0114] In one or more embodiments of the present specification, the initial values ​​of the relative poses between each frame of point cloud can be set as needed. For example, the relative pose of the frame of point cloud can be set to be the same as the relative pose from the previous frame of point cloud to the frame of point cloud before the previous frame of point cloud. Alternatively, the absolute pose of the frame of point cloud can be set to be the same as the previous frame of point cloud. Since the frequency of radar point cloud acquisition is high, the pose changes between two adjacent frames of point cloud are small. The error in determining the relative pose of the frame of point cloud relative to the previous frame of point cloud based on the previous frame of point cloud is smaller, making the initial values ​​of the relative poses of each frame of point cloud more reasonable. Compared with the method of relying on GPS for pose optimization, the initial values ​​determined based on GPS in scenarios such as weak GPS signals are more accurate.

[0115] The frame-by-frame optimization process based on frame matching can gradually optimize the relative pose between each frame point cloud and the previous frame point cloud based on the set initial value.

[0116] In one or more embodiments of the present specification, in step S102, when at least one frame point cloud and the frame point cloud are determined as matching point clouds from several frame point clouds optimized before the frame point cloud, the server may also determine multiple frames of matching point clouds so that the current frame can be subsequently matched with a local map composed of each matching point cloud, that is, frame-to-map matching is performed rather than frame-to-frame matching.

[0117] Specifically, the server may determine, according to a preset interval, from several frame point clouds optimized before the current frame point cloud, several frame point clouds and the current frame point cloud as matching point clouds.

[0118] The interval can be set as needed. For example, the interval can be set to 10, and the server can determine the 10 consecutive frames of point clouds before the frame point cloud from the several frames of point clouds optimized before the frame point cloud, and use them as the matching point cloud with the frame point cloud.

[0119] Then, further, in one or more embodiments of the present specification, in step S104, when determining each matched static target pair, the server may determine whether the current frame point cloud is a key frame based on at least one of the distance and heading angle difference between the frame point cloud and the previous key frame.

[0120] In one or more embodiments of the present specification, when determining whether the frame point cloud is a key frame, the previous frame key frame, that is, the previous key frame, can be determined first, and it can be determined whether the distance between the frame point cloud and the previous key frame is greater than a preset update distance threshold. If so, the frame point cloud is determined to be a key frame; if not, the frame point cloud is determined to be a non-key frame.

[0121] In one or more embodiments of the present specification, when determining whether the frame point cloud is a key frame, the server may also determine the previous frame key frame, and determine whether the angle difference between the heading angle of the frame point cloud and the previous key frame is greater than a preset angle difference threshold. If so, the frame point cloud is determined to be a key frame; if not, the frame point cloud is determined to be a non-key frame.

[0122] In one or more embodiments of the present specification, when a point cloud frame is determined to be a keyframe based on at least one of the distance and heading angle difference between the point cloud frame and the previous keyframe, the server may determine, for each geometric model in the matching point cloud frame, other geometric models of the same type as the geometric model from other geometric models in the matching point clouds of other frames as similar models of the geometric model. Furthermore, based on the initial values ​​of the relative poses between each pair of matching point clouds and the mass points and vectors of the geometric models in each matching point cloud, the server may determine the distances between each geometric model in each matching point cloud and its similar model, thereby determining the static object pairs that match each matching point cloud based on the distances between each geometric model in each matching point cloud and its similar model.

[0123] To ensure more accurate matching results, in one or more embodiments of this specification, when the frame point cloud is a keyframe, the server may further, for each geometric model in each frame's matching point cloud, identify other geometric models of the same type as the geometric model from other geometric models in other frames' matching point clouds, as similar models of the geometric model. Furthermore, based on the initial values ​​of the relative poses between each pair of matching point clouds and the mass points and vectors of the geometric models in each matching point cloud, the server determines the distances and angles between each geometric model in each matching point cloud and its similar model. Furthermore, based on the distances and angles between each geometric model in each matching point cloud and its similar model, the server determines the static object pairs that match each matching point cloud.

[0124] In one or more embodiments of the present specification, when the frame point cloud is a non-keyframe, the server may, for each geometric model in the frame point cloud, identify other geometric models of the same type as the geometric model from other geometric models in matching point clouds of other frames as similar models of the geometric model. Furthermore, based on the initial values ​​of the relative poses between the frame point cloud and the other matching point clouds, and the mass points and vectors of the geometric models in each matching point cloud, the server may determine the distances between each geometric model in the frame point cloud and each similar model. Based on the distances between each geometric model in the frame point cloud and each similar model, the server may determine the static object pairs that match the frame point cloud and each other matching point cloud.

[0125] Similarly, to achieve more accurate matching results, in one or more embodiments of this specification, when the frame point cloud is a non-keyframe, the server may, for each geometric model in the frame point cloud, identify other geometric models of the same type as the geometric model from other geometric models in other matching point clouds, as similar models of the geometric model. Furthermore, based on the initial values ​​of the relative poses between the frame point cloud and the other matching point clouds, and the mass points and vectors of the geometric models in each matching point cloud, the server determines the distances and angles between each geometric model in the frame point cloud and its respective similar models. Based on the distances and angles between each geometric model in the frame point cloud and its respective similar models, the server determines the static object pairs that match the frame point cloud and each other matching point cloud.

[0126] In one or more embodiments of the present specification, when constructing the optimization objective function in step S104, the server may construct the optimization objective function based on the distance between the particle of each geometric model in each static target pair to another geometric model, and the angle between the vectors of each geometric model in each static target pair.

[0127] In one or more embodiments of this specification, the optimization objective function may be specifically as follows:

[0128] A=min∑r(h(T i , f m ), h(T j , f n ))

[0129] Among them, r(h(T i , f m ), h(T j , f n )) represents the residual determined based on the distance and angle between the geometric model pair of the i-th frame matching point cloud and the j-th frame matching point cloud. The i-th frame matching point cloud and the j-th frame matching point cloud are a pair of matching point clouds, that is, a pair of point clouds with a geometric model pair. The sum of the residuals of all matching point cloud pairs is A. By solving the minimum value of the objective function, that is, the minimum value of A, the optimized pose of at least part of the matching point cloud can be obtained. In addition, h(T i , f m ) represents the structure corresponding to the matching point cloud of the i-th frame, which at least includes the data of each geometric model in the matching point cloud of the i-th frame (such as the type of each geometric model, particle coordinates, vectors, etc., the original radar points of the rod-shaped geometric model, etc.), and f m Indicates the type of static target corresponding to each geometric model in the i-th frame point cloud, T i Indicates the relative pose between the matching point cloud of the i-th frame and the first frame point cloud for pose optimization. j , f n) represents the structure corresponding to the j-th frame matching point cloud, which at least includes the data of each geometric model in the j-th frame matching point cloud (similar to the j-th frame matching point cloud), and f n Indicates the type of static target corresponding to each geometric model in the matching point cloud of the jth frame. j Indicates the relative pose between the matching point cloud of the j-th frame and the first frame point cloud for pose optimization.

[0130] It should be noted that when performing frame-to-frame matching, the i-th frame matching point cloud and the j-th frame matching point cloud are two adjacent frame point clouds, and one frame is the currently optimized frame point cloud when optimizing each frame point cloud frame by frame. When performing frame-to-image matching, the i-th frame matching point cloud and the j-th frame matching point cloud can be two non-adjacent frame point clouds.

[0131] In one or more embodiments of the present specification, a plurality of matching point cloud pairs may be determined based on the determined geometric model pairs. A matching point cloud pair includes at least one pair of matching geometric models between two frames of matching point clouds, ie, a geometric model pair.

[0132] Based on this, the objective function can also be expressed as:

[0133] A=min∑H m

[0134] H m =∑r(M iq , M js )

[0135] Among them, H m M represents the residual corresponding to the two matching point clouds, that is, the residual corresponding to the mth point cloud pair, determined based on the angles and distances between all geometric model pairs between the two matching point clouds. iq Indicates the qth geometric model in the i-th frame matching point cloud, M js Indicates the qth geometric model in the jth frame matching point cloud, M iq With M js That is, a pair of geometric models between the matching point cloud of the i-th frame and the matching point cloud of the j-th frame. iq , M js ) represents the residual corresponding to the geometric model pair determined based on the angle and distance between the geometric model pair.

[0136] When the geometric model pair is a pair of rod-shaped geometric models:

[0137]

[0138] Wherein, L represents the weight value corresponding to the qth rod-shaped geometric model and the sth rod-shaped geometric model, which can be determined based on the number of points corresponding to the qth rod-shaped geometric model and the sth rod-shaped geometric model. For example, the weight value can be obtained by taking the average, maximum value, minimum value, etc. of the number of points of the two rod-shaped geometric models. iq represents the coordinates of the mass point of the q-th rod-shaped geometric model in the matching point cloud of the i-th frame, P js Represents the coordinates of the mass point of the s-th rod-shaped geometric model in the j-th frame matching point cloud. represents the vector of the q-th rod-shaped geometric model in the matching point cloud of the i-th frame, The vector representing the s-th rod-shaped geometric model in the matching point cloud of the j-th frame. This item can realize the constraint on the angle between rod-shaped geometric models and is the expression value of the angle between rod-shaped geometric models. and The distance constraint between rod-shaped geometric models can be realized as the expression value of the distance.

[0139] When the geometric model pair is a pair of planar geometric models:

[0140]

[0141] Wherein, S represents the weight value corresponding to the pair of plane geometric models, which can be determined based on the area corresponding to the qth plane geometric model and the sth plane geometric model. For example, the average of the areas of the two can be used. iq 、b iq 、c iq d iq represents the plane parameters of the qth plane geometric model in the i-th frame matching point cloud, a js 、b js 、c js d js Represents the plane parameters of the s-th plane geometric model in the j-th frame matching point cloud. js 、y js 、z js represents the coordinates of the mass point of the sth plane geometric model of the jth frame matching point cloud, x iq 、y iq 、z iq Represents the coordinates of the mass point of the qth plane geometric model of the matching point cloud in the i-th frame. iq Represents the vector of the qth plane geometric model in the i-th frame matching point cloud, Represents the vector of the s-th plane geometric model in the matching point cloud of the j-th frame. This item can realize the constraint of the angle between plane geometric models, which is the expression value of the angle between plane geometric models. iq x js +b iq yjs +c qi z js +d iq |with|a js x iq +b js y iq +c js z iq +d js |Then the constraint on the distance between plane geometric models can be realized, which is the expression value of the distance between the plane geometric models.

[0142] Since each geometric model pair matched between a pair of matching point clouds may include both rod-shaped geometric model pairs and plane geometric model pairs, then H i It can also be expressed as:

[0143] H i =∑r(t iq , t js )+∑r(f iq , f js )

[0144] in,

[0145]

[0146]

[0147] Among them, t iq represents the qth rod-shaped geometric model in the i-th frame matching point cloud, t js Indicates that the s-th rod-shaped geometric model in the j-th frame matching point cloud is a geometric model pair. iq , t js ) represents the residual corresponding to the geometric model pair determined based on the angle and distance between the geometric model pair. iq represents the qth plane geometric model in the i-th frame matching point cloud, t js Represents the sth plane geometric model in the jth frame matching point cloud. r(t iq , t js ) and the corresponding formula r(f iq , f js ) The explanation of other symbols in the formula corresponding to r(M iq , M js ) corresponding to the two formulas.

[0148] Of course, instead of calculating the residuals in units of frames and then summing them up, different pairs of geometric models can be directly numbered, and then the residuals of the geometric model pairs can be calculated based on the distance and angle between the geometric model pairs as a whole, and then summed up to obtain the value of A corresponding to the objective function.

[0149] Then, the objective function can also be expressed as:

[0150] A=min∑r(W e , O e )

[0151] Among them, r(W e , O e ) represents the geometric model pair based on all matching point clouds, the e-th geometric model pair, the geometric model W e With geometric model O e The residuals determined by the angle and distance between the two points. The sum of the residuals corresponding to the geometric models of all matching point clouds is the complete expression of the objective function. And calculate r(W e , O e ) can be adapted to different geometric model pairs. Specifically, see the description of the residual formula corresponding to the rod-shaped geometric model and the plane geometric model respectively.

[0152] In one or more embodiments of the present specification, during frame-to-frame matching, in step S106, the server may adjust the pose of at least a portion of the matched point cloud with the goal of minimizing the objective function to obtain the optimized pose of at least a portion of the matched point cloud. Specifically, the server may optimize the pose of the point cloud frame based on the initial value of the relative pose between the point cloud frame and the previous point cloud frame with the goal of minimizing the objective function to obtain the optimized pose of the point cloud frame. In other words, only the pose of the point cloud frame is optimized.

[0153] In addition, in one or more embodiments of the present specification, when performing frame-by-frame matching, the server may, after obtaining each matched static object pair, perform linear optimization on the initial values ​​of the relative poses between each static object pair and each matched point cloud using a linear optimization algorithm, thereby re-determining the initial values ​​of the relative poses between the point cloud of the current frame and the point cloud of the previous frame. Specifically, the relative poses between the matching point clouds of each frame are optimized based on the initial values, and the optimized relative poses are used as the initial values.

[0154] In one or more embodiments of this specification, the linear optimization algorithm may be a Random Sample Consensus (RANSAC) algorithm. Of course, other linear optimization algorithms may also be used, and this specification does not limit this.

[0155] Then, in one or more embodiments of the present specification, in step S106, with the goal of minimizing the optimization objective function, the pose of the frame point cloud is adjusted based on the initial value of the relative pose between the frame point cloud and the previous frame point cloud. When the optimized pose of the frame point cloud is obtained, the initial value used may be the initial value optimized by the random sampling consensus algorithm.

[0156] In one or more embodiments of the present specification, when performing frame-image matching, in step S106, when adjusting the pose of at least a portion of the matching point clouds with the goal of minimizing the optimization objective function to obtain the optimized pose of at least a portion of the matching point clouds, the server may, when the frame point cloud is a keyframe, adjust the pose of each matching point cloud based on the initial value of the relative pose between each frame of matching point clouds with the goal of minimizing the optimization objective function to obtain the optimized pose of each matching point cloud. That is, the pose of all matching point clouds is optimized based on the initial value of the relative pose between each two frame point clouds in all matching point clouds.

[0157] In one or more embodiments of the present specification, when the frame point cloud is a non-keyframe, the server may optimize the minimum objective function and adjust the pose of the frame point cloud based on the initial relative pose values ​​between the frame point cloud and other matching point clouds to obtain the optimized pose of the frame point cloud. In other words, the pose of the frame point cloud is optimized based only on the initial relative pose values ​​between each pair of the frame point cloud and other matching point clouds.

[0158] In addition, in one or more embodiments of the present specification, the server may also adjust the pose of each matching point cloud based on the initial relative pose values ​​between the matching point clouds of each frame, with the goal of minimizing the optimization objective function, when the frame point cloud is a keyframe, to obtain an optimized pose for each matching point cloud. When the frame point cloud is a non-keyframe, the pose of any matching point cloud determined based on the frame point cloud is not adjusted, and steps S100 to S106 are continued for the next frame point cloud.

[0159] Furthermore, in order to reduce the amount of calculation, in one or more embodiments of the present specification, the server can also optimize each frame of the collected point cloud frame by frame. When optimizing to the frame, it first determines whether the frame point cloud is a key frame. When the judgment result is yes, steps S100 to S106 are executed. When the judgment result is no, step S100 is executed without executing steps S102 to S106, and the next frame point cloud can be optimized.

[0160] In one or more embodiments of the present specification, when performing frame-image matching, the server may not determine whether the frame point cloud is a key frame, but may directly use the frame point cloud as a key frame and execute subsequent steps.

[0161] In one or more embodiments of this specification, calculating the distance between geometric models based on mass points is merely an example. The distance between geometric models can also be calculated based on multiple points, not just mass points. For example, for each geometric model, the model can be sampled to obtain its expression points, and then represented using the expression points and vectors. The distance between the geometric models can then be determined based on the expression points and vectors of each geometric model.

[0162] In addition, in one or more embodiments of the steps of this specification, when executing the pose optimization method, both frame-by-frame matching and frame-by-image matching can be performed. The server can then determine the two-frame matching point clouds during frame-by-frame matching to obtain static target pairs for the two-frame point clouds, construct an optimization objective function, and optimize frame by frame to obtain the optimized poses of all point clouds. Then, based on the poses obtained through frame-by-frame matching optimization, frame-by-image matching is performed, and multi-frame matching point clouds are again determined to obtain static target pairs for each frame matching point cloud. The optimization objective function is then constructed again, and optimization is performed frame by frame to obtain further optimized poses for all point clouds.

[0163] Of course, when performing frame-to-frame matching, linear optimization can be performed first, followed by nonlinear optimization based on the optimization objective function. The pose of each frame point cloud obtained by nonlinear optimization is used as the final optimized pose for frame-to-frame matching, and then frame-to-frame matching is performed.

[0164] After linear optimization, the initial values ​​between each frame's point cloud are optimized and updated, serving as the re-determined initial values. Nonlinear optimization is then performed based on these re-determined initial values ​​to obtain the second optimized relative pose between each frame's point cloud. Furthermore, the second optimized relative pose between each frame's point cloud serves as the re-determined initial value for frame-image matching, and the optimization objective function constructed based on the frame-image matching results yields the final optimized pose for each frame's point cloud.

[0165] In one or more embodiments of the present specification, after the initial value is re-determined, that is, after the initial value is updated, each matching geometric model pair may be re-determined based on the new initial value.

[0166] Figure 2This is a schematic diagram of a pose optimization process provided in this specification. As shown in the figure, for each frame point cloud that has been collected, the server can perform feature extraction on each frame point cloud in turn, determine each static target in the frame point cloud, and then match each static target based on the distance between each static target, or the distance and angle between each static target, to determine each static target pair. Then, frame matching is performed, and based on each static target pair, linear optimization is performed to obtain the linearly optimized pose of the frame point cloud. After obtaining the linearly optimized pose of all frame point clouds, nonlinear optimization is performed. After obtaining the nonlinearly optimized pose of all frame point clouds, frame matching can be performed. The server can determine whether the frame point cloud is a key frame in turn for each frame point cloud. If so, the optimization objective function of the bundle adjustment is based on the optimization objective function of the bundle adjustment (the specific optimization process can refer to the above description of the key frame in this specification). If not, the optimization objective function of the bundle adjustment is based on the optimization objective function of the bundle adjustment (again, the specific optimization process can refer to the above description). After optimizing the point cloud of each frame, a sufficiently accurate pose of the optimized point cloud of all frames can be obtained.

[0167] In one or more embodiments of the present specification, specifically, the server may determine the previous frame point cloud of the frame point cloud from several frame point clouds preceding the frame point cloud, and use the previous frame point cloud and the frame point cloud as matching point clouds. Thereafter, the server may determine at least the distance between static targets belonging to the two frame matching point clouds and of the same type based on the initial value of the relative pose between the two frame matching point clouds, and determine each matching static target pair based on at least the distance between static targets belonging to the same type but belonging to different frame matching point clouds. Thereafter, the server may linearly optimize the initial values ​​between the frame matching point clouds based on the initial values ​​of the relative pose between each static target pair and each matching point cloud, using a random sampling consensus algorithm, and re-determine the initial value of the relative pose between the frame point cloud and the previous frame point cloud as the initial optimization value.

[0168] After optimizing frame by frame and obtaining the initial optimization values ​​corresponding to all frame point clouds used to construct the map, the server can further perform nonlinear optimization frame by frame based on the initial optimization values. Specifically, the server can re-determine the matching static target pairs based on the obtained initial optimization values, and construct the first optimization objective function of nonlinear optimization based on the distance and angle between each static target pair. And with the goal of minimizing the optimization objective function, adjust the pose of the frame point cloud to obtain the optimized pose of the frame point cloud. The specific form of the optimization objective function can be referred to the above description of this specification, and this specification will not repeat it here.

[0169] In one or more embodiments of this specification, after obtaining the optimized poses corresponding to all frame point clouds used to construct a map through nonlinear optimization frame by frame, the server can use the relative poses between the frame point clouds as initial values ​​for secondary optimization. Frame-to-map matching can then be performed based on the obtained initial values ​​for secondary optimization.

[0170] In one or more embodiments of the present disclosure, during frame-to-frame image matching, the server may continue to optimize the pose on a frame-by-frame basis. First, the server may re-determine the matching point cloud. Specifically, based on a preset interval, the server may determine several frame point clouds optimized before the current frame point cloud and the current frame point cloud as the newly determined matching point cloud. The server may then determine whether the current frame point cloud is a keyframe based on at least one of the distance and heading angle difference between the current frame point cloud and the previous keyframe.

[0171] In one or more embodiments of the present specification, when the frame point cloud is a keyframe, the server may determine, for each geometric model in each frame matching point cloud, other geometric models of the same type as the geometric model from other geometric models in other frame matching point clouds, as similar models of the geometric model. Furthermore, based on the initial values ​​of the relative poses between the two matching point clouds, the mass points and vectors of the geometric models in each matching point cloud, the server may determine the distances and angles between each geometric model in each matching point cloud and its respective similar models, thereby determining the static target pairs matched between each matching point cloud based on the distances and angles between each geometric model in each matching point cloud and its respective similar models. When the frame point cloud is a non-keyframe, the server may determine, for each geometric model in the frame point cloud, other geometric models of the same type as the geometric model from other geometric models in other frame matching point clouds, as similar models of the geometric model. Based on the initial values ​​of the relative poses between the frame point cloud and other matching point clouds, and the mass points and vectors of the geometric models in each matching point cloud, the distances and angles between each geometric model in the frame point cloud and its corresponding models are determined. Based on the distances and angles between each geometric model in the frame point cloud and its corresponding models, the static object pairs matched between the frame point cloud and other matching point clouds are determined.

[0172] The server then constructs a nonlinear optimization objective function for bundle adjustment (BA) based on the distances and angles between each pair of static targets, as the second optimization objective function. Ultimately, the pose of at least a portion of the matching point cloud is adjusted to obtain the final optimized pose of at least a portion of the matching point cloud, with the goal of minimizing the second optimization objective function.

[0173] It should be noted that the first optimization objective function and the second optimization objective function are formally consistent with the above-mentioned optimization objective function. The only difference is the number of matching point clouds involved in the calculation. In one or more embodiments of this specification, nonlinear optimization can specifically adopt nonlinear least squares optimization, for example, the Levenberg-Marquardt method (LM) or the Gauss-Newton method.

[0174] In this specification, static objects can be identified based on semantic features contained in point clouds, and static object pairs can be determined by matching based on these static objects. This matching based on static object pairs requires minimal computation and ensures matching accuracy. Furthermore, different methods are used to calculate the distance and angle between two static objects of the same type but belonging to different point clouds. This allows constraints to be placed on at least some of the six degrees of freedom of the pose of the two point clouds from different dimensions, resulting in a more accurate relative pose between the optimized point clouds. When the relative pose between two point clouds containing a matching static object pair is accurate, the geometric model overlap between the two matching point clouds increases after rotating and / or translating one of the point clouds based on this relative pose. By further constructing an objective function based on matching rod-shaped static objects, constraints can be placed on the x- and y-coordinates, as well as the pose of the point cloud. By further constructing an objective function based on matching planar static objects, constraints can be placed on the height, z-coordinate, pitch, and roll angles of the point cloud. Based on these constraints, the pose of each frame point cloud obtained by final optimization can be made more accurate.

[0175] In one or more embodiments of this specification, the pose of each point cloud frame ultimately optimized using the pose optimization method provided herein can be used to further optimize the global pose of each point cloud frame. After the pose of each point cloud frame is ultimately optimized, the server can perform loop closure detection and establish loop closure constraints between the point clouds. Subsequently, the server can perform graph optimization using the pose of each point cloud frame ultimately optimized using the pose optimization method as the first constraint and the loop closure constraint obtained from the loop closure detection as the second constraint.

[0176] In one or more embodiments of the present specification, graph optimization may also be performed based on the position determined based on GPS data as a third constraint and the relative pose between point clouds of each frame determined based on IMU data as a fourth constraint.

[0177] The above is a posture optimization method provided by one or more embodiments of this specification. Based on the same idea, this specification also provides a corresponding posture optimization device, such as Figure 3 shown.

[0178] Figure 3This is a schematic diagram of a posture optimization device provided in this specification, which includes:

[0179] The feature extraction module 200 is used to extract features from each frame of point cloud according to the order in which each frame of point cloud has been collected, and determine each static object and the type of each static object in the frame of point cloud;

[0180] A determination module 201 is configured to determine at least one frame of point cloud and the frame of point cloud from among a plurality of frame point clouds optimized before the frame of point cloud, as a matching point cloud;

[0181] A matching module 202 is configured to determine matched pairs of static objects based on at least the distances between static objects of the same type and belonging to matching point clouds of different frames, and to construct an optimization objective function based on the distances and angles between the pairs of static objects;

[0182] The optimization module 203 is configured to adjust the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain an optimized pose of the at least portion of the matching point cloud.

[0183] Optionally, the determination module 201 is further configured to determine a previous frame point cloud of the frame point cloud from several frame point clouds preceding the frame point cloud, and use the previous frame point cloud and the frame point cloud as matching point clouds.

[0184] Optionally, the matching module 202 is also used to determine the geometric model of each static target in each matching point cloud, and for each geometric model in each matching point cloud, determine the mass point of the geometric model and the vector that uniquely identifies the direction of the geometric model. For each geometric model in the frame point cloud, determine other geometric models of the same type as the geometric model from other geometric models of the previous frame point cloud as similar models of the geometric model. According to the initial value of the relative posture between the frame point cloud and the previous frame point cloud, the mass points and vectors of each geometric model, determine the distance and angle between each geometric model and its respective similar models, and determine the matched static target pairs based on the determined distances and angles.

[0185] Optionally, the determining module 201 is further configured to determine, according to a preset interval, from a plurality of frame point clouds optimized before the frame point cloud, a plurality of frame point clouds and the frame point cloud as matching point clouds.

[0186] Optionally, the matching module 202 is also used to determine the geometric model of each static target in each matching point cloud, and for each geometric model in each matching point cloud, determine the mass point of the geometric model and the vector that uniquely identifies the direction of the geometric model. When the frame point cloud is determined to be a key frame based on at least one of the distance and heading angle difference between the frame point cloud and the previous key frame, for each geometric model in each frame matching point cloud, determine other geometric models of the same type as the geometric model from other geometric models in other frame matching point clouds as similar models of the geometric model. According to the initial value of the relative pose between the pairwise matching point clouds and the mass points and vectors of the geometric models in each matching point cloud, determine the distance and angle between each geometric model of each matching point cloud and its respective similar models. According to the distance and angle between each geometric model of each matching point cloud and its respective similar models, determine each pair of static targets matched between each matching point cloud.

[0187] Optionally, the matching module 202 is also used to, when it is determined that the frame point cloud is a non-key frame based on at least one of the distance and heading angle difference between the frame point cloud and the previous key frame, determine, for each geometric model in the frame point cloud, other geometric models of the same type as the geometric model from other geometric models of other frame matching point clouds, as similar models of the geometric model; determine the distances and angles between each geometric model of the frame point cloud and each similar model based on the initial values ​​of the relative postures between the frame point cloud and other matching point clouds, the particles and vectors of the geometric models in each matching point cloud; and determine the static target pairs matched between the frame point cloud and each other matching point cloud based on the distances and angles between each geometric model in the frame point cloud and each similar model.

[0188] Optionally, the matching module 202 is further configured to construct an optimization objective function based on the distance between the mass point of each geometric model in each static target pair and the matched other geometric model, and the angle between the vectors of each geometric model in each static target pair.

[0189] The device shown also includes:

[0190] The updating module 204 is configured to perform linear optimization on the initial value by using a random sampling consensus algorithm, and re-determine the initial value of the relative pose between the point cloud of the current frame and the point cloud of the previous frame.

[0191] Optionally, the optimization module 203 is also used to adjust the pose of the frame point cloud based on the initial value of the relative pose between the frame point cloud and the previous frame point cloud with the goal of minimizing the optimization objective function, so as to obtain the optimized pose of the frame point cloud.

[0192] Optionally, the optimization module 203 is also used to, when the frame point cloud is a key frame, take the optimization objective function as the minimum and, based on the initial values ​​of the relative poses between the matching point clouds of each frame, adjust the pose of each matching point cloud to obtain the optimized pose of each matching point cloud; when the frame point cloud is a non-key frame, take the optimization objective function as the minimum and, based on the initial values ​​of the relative poses between the frame point cloud and other matching point clouds, adjust the pose of the frame point cloud to obtain the optimized pose of the frame point cloud.

[0193] This specification also provides a computer-readable storage medium, which stores a computer program that can be used to execute the above Figure 1 Provided pose optimization method.

[0194] This manual also provides Figure 4 The structural diagram of the electronic device shown in FIG. Figure 4 As shown, at the hardware level, the electronic device includes a processor, an internal bus, a memory, and a non-volatile memory, and may also include other hardware required for the business. The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it to achieve the above Figure 1 Provided pose optimization method.

[0195] It should be noted that all actions of obtaining signals, information or data in this manual are performed in compliance with the relevant data protection laws and policies of the country where they are located and with the authorization given by the owner of the corresponding device.

[0196] Of course, in addition to software implementation, this specification does not exclude other implementation methods, such as logic devices or a combination of software and hardware, etc. That is to say, the execution subject of the following processing flow is not limited to each logic unit, but can also be hardware or logic devices.

[0197] In the 1990s, technological improvements could be clearly distinguished as either hardware improvements (for example, improvements to circuit structures like diodes, transistors, and switches) or software improvements (improvements to process flows). However, with the advancement of technology, many process flow improvements today can now be considered direct improvements to hardware circuit structures. Designers almost always create the corresponding hardware circuit structure by programming the improved process flow into the hardware circuit. Therefore, it cannot be said that a process flow improvement cannot be implemented using hardware modules. For example, a programmable logic device (PLD), such as a field programmable gate array (FPGA), is an integrated circuit whose logical function is determined by user programming. Designers can "integrate" a digital system on a PLD through their own programming, without having to hire a chip manufacturer to design and manufacture a dedicated integrated circuit chip. Moreover, nowadays, instead of manually fabricating integrated circuit chips, this programming is mostly done using "logic compiler" software. This is similar to the software compiler used when developing programs. Before compilation, the original code must also be written in a specific programming language, called a hardware description language (HDL). There is not just one HDL, but many, such as ABEL (Advanced Boolean Expression Language), AHDL (Altera Hardware Description Language), Confluence, CUPL (Cornell University Programming Language), HDCal, JHDL (Java Hardware Description Language), Lava, Lola, MyHDL, PALASM, RHDL (Ruby Hardware Description Language), etc. The most commonly used ones are VHDL (Very-High-Speed ​​Integrated Circuit Hardware Description Language) and Verilog. Those skilled in the art will also understand that by simply programming the method flow in one of these hardware description languages ​​and then programming it into an integrated circuit, a hardware circuit that implements the logic method flow can be easily obtained.

[0198] The controller can be implemented in any suitable manner. For example, the controller can take the form of a microprocessor or processor and a computer-readable medium storing computer-readable program code (e.g., software or firmware) executable by the (micro)processor, logic gates, switches, application-specific integrated circuits (ASICs), programmable logic controllers, and embedded microcontrollers. Examples of controllers include, but are not limited to, the following microcontrollers: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20, and Silicone Labs C8051F320. The memory controller can also be implemented as part of the control logic of the memory. Those skilled in the art will also know that in addition to implementing the controller in a purely computer-readable program code format, the controller can be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers by logically programming the method steps. Therefore, such a controller can be considered a hardware component, and the devices included therein for implementing various functions can also be considered as structures within the hardware component. Or even, the devices for implementing various functions can be considered as both software modules that implement the method and structures within the hardware component.

[0199] The systems, devices, modules, or units described in the above embodiments may be implemented by computer chips or entities, or by products having certain functions. A typical implementation device is a computer. Specifically, the computer may be, for example, a personal computer, a laptop computer, a cellular phone, a camera phone, a smartphone, a personal digital assistant, a media player, a navigation device, an email device, a game console, a tablet computer, a wearable device, or a combination of any of these devices.

[0200] For the convenience of description, the above devices are described as being divided into various units according to their functions. Of course, when implementing this specification, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0201] It will be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0202] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts 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, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0203] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0204] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0205] In a typical configuration, a computing device includes one or more processors (CPUs), input / output interfaces, network interfaces, and memory.

[0206] Memory may include non-permanent storage in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. Memory is an example of a computer-readable medium.

[0207] Computer-readable media includes permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. The information can be computer-readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disc read-only memory (CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer-readable media does not include transitory computer-readable media (transitory media), such as modulated data signals and carrier waves.

[0208] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

[0209] Those skilled in the art will appreciate that the embodiments of this specification may be provided as methods, systems, or computer program products. Thus, this specification may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, this specification may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0210] This specification may be described in the general context of computer-executable instructions, such as program modules, executed by a computer. Generally, program modules include routines, programs, objects, components, data structures, and the like that perform specific tasks or implement specific abstract data types. This specification may also be practiced in distributed computing environments where tasks are performed by remote processing devices connected through a communications network. In a distributed computing environment, program modules may be located in both local and remote computer storage media, including storage devices.

[0211] The various embodiments in this specification are described in a progressive manner. Similar parts between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the system embodiments are generally similar to the method embodiments, so the description is relatively simple. For relevant parts, refer to the description of the method embodiments.

[0212] The foregoing is merely an example of the present invention and is not intended to limit the present invention. Various modifications and variations are possible for those skilled in the art. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be included within the scope of the claims of the present invention.

Claims

1. A posture optimization method, characterized in that: include: According to the acquisition order of each frame of point cloud, feature extraction is performed on each frame of point cloud in turn to determine each static target and the type of each static target in the frame of point cloud; Determine at least one frame point cloud and the frame point cloud from among several frame point clouds optimized before the current frame point cloud as matching point clouds; Determine the matched static object pairs based on at least the distances between static objects of the same type and belonging to different frame matching point clouds, and construct an optimization objective function based on the distances and angles between the static object pairs; Adjusting the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain an optimized pose of the at least portion of the matching point cloud; From among several frame point clouds optimized before the current frame point cloud, at least one frame point cloud and the current frame point cloud are determined as matching point clouds, specifically including: Determine a previous frame point cloud of the current frame point cloud from a plurality of frame point clouds preceding the current frame point cloud; The previous frame point cloud and the current frame point cloud are used as matching point clouds.

2. The method according to claim 1, wherein Determining the matched pairs of static objects based on at least the distances between static objects of the same type and belonging to the matching point clouds of different frames, specifically including: Determine the geometric model of each static target in each matching point cloud; For each geometric model in each matching point cloud, determine the mass point of the geometric model and the vector that uniquely identifies the direction of the geometric model; For each geometric model in the point cloud of the frame, determine other geometric models of the same type as the geometric model from other geometric models in the point cloud of the previous frame as similar models of the geometric model; Determine the distance and angle between each geometric model and its own similar model based on the initial value of the relative pose between the frame point cloud and the previous frame point cloud, and the mass points and vectors of each geometric model; According to the determined distances and angles, matching pairs of static targets are determined.

3. The method according to claim 1, wherein From among several frame point clouds optimized before the current frame point cloud, at least one frame point cloud and the current frame point cloud are determined as matching point clouds, specifically including: According to a preset interval, several frame point clouds and the frame point cloud are determined from several frame point clouds optimized before the current frame point cloud as matching point clouds.

4. The method according to claim 3, wherein Determining the matched pairs of static objects based on at least the distances between static objects of the same type and belonging to the matching point clouds of different frames, specifically including: Determine the geometric model of each static target in each matching point cloud; For each geometric model in each matching point cloud, determine the mass point of the geometric model and the vector that uniquely identifies the direction of the geometric model; When the frame point cloud is determined to be a key frame based on at least one of the distance and the heading angle difference between the frame point cloud and the previous key frame, for each geometric model in the matching point cloud of each frame, other geometric models of the same type as the geometric model are determined from other geometric models in the matching point clouds of other frames as similar models of the geometric model; According to the initial value of the relative pose between the two matching point clouds, the mass points and vectors of the geometric models in each matching point cloud, the distance and angle between each geometric model of each matching point cloud and its own similar model are determined; According to the distances and angles between the geometric models of each matching point cloud and their respective similar models, the static target pairs matched between the matching point clouds are determined.

5. The method according to claim 4, wherein The method further comprises: When the frame point cloud is determined to be a non-key frame based on at least one of the distance and the heading angle difference between the frame point cloud and the previous key frame, for each geometric model in the frame point cloud, other geometric models of the same type as the geometric model are determined from other geometric models in the matching point clouds of other frames as the same type of models of the geometric model; According to the initial values ​​of the relative poses between the frame point cloud and other matching point clouds, and the mass points and vectors of the geometric models in each matching point cloud, the distances and angles between each geometric model of the frame point cloud and its own similar models are determined; According to the distance and angle between each geometric model in the frame point cloud and its respective similar models, the static target pairs matched between the frame point cloud and other matching point clouds are determined.

6. The method according to claim 2 or 5, characterized in that According to the distance and angle between each pair of static targets, an optimization objective function is constructed, which specifically includes: An optimization objective function is constructed according to the distance between the mass point of each geometric model in each static target pair and the matching other geometric model, and the angle between the vectors of each geometric model in each static target pair.

7. The method according to claim 2, wherein Before constructing the optimization objective function based on the distances and angles between each pair of static targets, the method further includes: The initial value is linearly optimized by a random sampling consensus algorithm to re-determine the initial value of the relative pose between the point cloud of the frame and the point cloud of the previous frame.

8. The method according to claim 2 or 7, wherein: Adjusting the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain an optimized pose of the at least portion of the matching point cloud specifically includes: With the goal of minimizing the optimization objective function, the pose of the frame point cloud is adjusted based on the initial value of the relative pose between the frame point cloud and the previous frame point cloud to obtain the optimized pose of the frame point cloud.

9. The method according to claim 5, wherein Adjusting the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain an optimized pose of the at least portion of the matching point cloud specifically includes: When the point cloud of the frame is a key frame, the optimization objective function is minimized, and based on the initial value of the relative pose between the matching point clouds of each frame, the pose of each matching point cloud is adjusted to obtain the optimized pose of each matching point cloud; When the frame point cloud is a non-key frame, the optimization objective function is minimized. Based on the initial values ​​of the relative poses between the frame point cloud and other matching point clouds, the pose of the frame point cloud is adjusted to obtain the optimized pose of the frame point cloud.

10. A posture optimization device, characterized in that: include: A feature extraction module is used to extract features from each frame of point cloud according to the order in which each frame of point cloud has been collected, and to determine each static target and its type in the frame of point cloud; The determination module is used to determine at least one frame of point cloud from several frame point clouds optimized before the current frame point cloud and the frame point cloud as a matching point cloud, and to determine at least one frame of point cloud from several frame point clouds optimized before the current frame point cloud and the frame point cloud as a matching point cloud, specifically including: Determine a previous frame point cloud of the current frame point cloud from a plurality of frame point clouds preceding the current frame point cloud; The previous frame point cloud and the current frame point cloud are used as matching point clouds; a matching module for determining matched pairs of static objects based on at least the distances between static objects of the same type and belonging to matching point clouds of different frames, and constructing an optimization objective function based on the distances and angles between the pairs of static objects; The optimization module is used to adjust the pose of at least a portion of the matching point cloud with the goal of minimizing the optimization objective function to obtain the optimized pose of the at least portion of the matching point cloud.

11. A computer-readable storage medium, characterized in that The storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.

12. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the method according to any one of claims 1 to 9 is implemented.

Images