A point cloud matching method, device, apparatus, and storage medium

Abstract

The present disclosure provides a point cloud matching method, device and equipment, and a storage medium, relates to the technical field of automatic driving, in particular to the technical field of automatic driving perception, data labeling and the like. The specific implementation scheme is: obtaining obstacle point clouds at multiple time points; constructing a factor graph by using the obstacle point clouds at the multiple time points, obstacle poses and obstacle bounding boxes; optimizing the factor graph; and matching the obstacle point clouds at the multiple time points based on the optimized factor graph. The present disclosure can improve the accuracy of point cloud matching of target obstacles.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of automatic driving, in particular to the technical field of automatic driving perception, data labeling, etc. BACKGROUND

[0002] Laser radar is a common sensor of an automatic driving vehicle, but because of the occlusion of the field of view and the sparse sampling of the laser radar, it can only obtain a fragmentary and sparse point cloud of one side of the target obstacle. In the scenarios of automatic driving perception, data labeling, and digital asset production, in order to obtain as complete and dense point cloud as possible, it is necessary to match and fuse the point clouds of the same obstacle at multiple times (or multiple frames), so as to obtain the complete point cloud of the obstacle. How to match the point clouds of the same obstacle at different times (or different frames) is a technical problem to be solved. SUMMARY

[0003] The present disclosure provides a point cloud matching method, device, equipment, and storage medium.

[0004] According to an aspect of the present disclosure, a point cloud matching method is provided, comprising:

[0005] obtaining obstacle point clouds at multiple times;

[0006] constructing a factor graph by using the obstacle point clouds at the multiple times, an obstacle pose, and an obstacle bounding box;

[0007] optimizing the factor graph;

[0008] matching the obstacle point clouds at the multiple times based on the optimized factor graph.

[0009] According to another aspect of the present disclosure, a point cloud matching device is provided, comprising:

[0010] a point cloud obtaining module configured to obtain obstacle point clouds at multiple times;

[0011] a factor graph constructing module configured to construct a factor graph by using the obstacle point clouds at the multiple times, an obstacle pose, and an obstacle bounding box;

[0012] a factor graph optimizing module configured to optimize the factor graph;

[0013] a matching module configured to match the obstacle point clouds at the multiple times based on the optimized factor graph.

[0014] According to another aspect of the present disclosure, an electronic device is provided, comprising:

[0015] at least one processor; and

[0016] a memory communicatively connected with the at least one processor; wherein

[0017] The memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method of any of the embodiments of the present disclosure.

[0018] According to another aspect of the present disclosure, a non-transitory computer-readable storage medium storing computer instructions is provided, wherein the computer instructions are used to enable the computer to perform the method according to any of the embodiments of the present disclosure.

[0019] According to another aspect of the present disclosure, a computer program product is provided, comprising a computer program which, when executed by a processor, implements the method according to any of the embodiments of the present disclosure.

[0020] The present disclosure can achieve point cloud matching assisted by the obstacle bounding box by taking the obstacle bounding box as a constraint factor when performing point cloud matching, thereby improving the accuracy of point cloud matching.

[0021] It should be understood that the content described in this part is not intended to identify key or important features of the embodiments of the present disclosure, nor to limit the scope of the present disclosure. Other features of the present disclosure will become apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0022] The accompanying drawings are used to better understand the present scheme and do not limit the present disclosure. Among them:

[0023] Figure 1 is a schematic flow chart of a point cloud matching method according to an embodiment of the present disclosure;

[0024] Figure 2 is a schematic diagram of factor graph optimization principle;

[0025] Figure 3 is an example one of the factor graph constructed according to an embodiment of the present disclosure;

[0026] Figure 4 is an example two of the factor graph constructed according to an embodiment of the present disclosure;

[0027] Figure 5 is a structural schematic diagram of a point cloud matching device 500 according to an embodiment of the present disclosure;

[0028] Figure 6 shows a schematic block diagram of an example electronic device 600 that can be used to implement embodiments of the present disclosure. DETAILED DESCRIPTION

[0029] Exemplary embodiments of the present disclosure are described herein with reference to the accompanying drawings, which are meant to be exemplary in nature, and include various details intended to facilitate understanding of the present disclosure. Thus, it should be apparent to those skilled in the art that various modifications can be made in the embodiments described without departing from the scope of the present disclosure. Likewise, the description herein is meant to be illustrative only and does not limit the scope of the present disclosure. For clarity and the sake of brevity, descriptions of well-known functions and constructions are omitted.

[0030] The "and / or" of the embodiments of the present disclosure means that there can be three relationships, for example, A and / or B can mean that A exists alone, A and B exist together, and B exists alone. The term "at least one" herein means any one of a plurality or any combination of at least two of a plurality, for example, including at least one of A, B, and C can mean including any one or more elements selected from the set consisting of A, B, and C. The terms "first", "second", herein mean to refer to a plurality of similar technical terms and to distinguish them, and are not meant to be limited in order or limited to only two, for example, the first feature and the second feature refer to two categories / two features, the first feature can be one or more, and the second feature can also be one or more.

[0031] Laser radar is a common sensor for autonomous vehicles, but due to occlusion and sparse sampling of laser radar, it can only obtain incomplete and sparse point clouds on one side of the target obstacle. In the scenarios of autonomous driving perception, data labeling, and digital asset production, in order to obtain as complete and dense point clouds as possible, it is necessary to match and fuse multiple frames of incomplete point clouds of the vehicle. There are some point cloud matching algorithms currently, which are used to match point clouds of the same obstacle at different times (or different frames).

[0032] The main purpose of the point cloud matching algorithm is to find a transformation matrix to determine the spatial relationship between the point clouds of the same obstacle at different times (or different frames), so as to realize the comparison, alignment and merging of point cloud data of the same obstacle in three-dimensional space to generate complete point cloud data. Point cloud matching algorithms are widely used in computer vision, robot navigation and 3D modeling fields. The following are several common point cloud matching algorithms:

[0033] (1) Iterative Closest Point (ICP) algorithm. The ICP algorithm is an iterative least squares optimization algorithm, which mainly matches the nearest neighbor points between two point clouds, calculates the rotation and translation matrix (the rotation and translation matrix can form the pose transformation matrix between point clouds at different times), and aligns the two sets of point clouds. The advantage of this algorithm is simple and easy to implement, and is suitable for point cloud data that has been close to registration, but it is sensitive to the initial position and is easy to fall into a local optimal solution.

[0034] (2) Normal Distributions Transform (NDT) algorithm. NDT algorithm divides the point cloud space into grids, constructs a normal distribution model in each grid, and aligns the point cloud based on the probability model. NDT algorithm performs well in noisy environment and is commonly used in SLAM systems, but has large computational complexity and is suitable for regular environment.

[0035] (3) Feature-based Matching algorithm. This method first extracts features from the point cloud, such as Fast Point Feature Histograms (FPFH), Signature of Histograms of OrienTations (SHOT), etc., and then matches based on the feature points. This method has good robustness to local structure of point cloud and is suitable for large attitude changes, but has large computational complexity for feature extraction.

[0036] (4) Deep learning method. In recent years, point cloud matching algorithms based on deep learning, such as PointNet, Dynamic Graph CNN (DGCNN), Fast Global Registration (FGR), etc., extract point cloud features through neural network and perform global matching. This kind of algorithm can learn the complex geometric structure of point cloud and has strong robustness to noise and missing data.

[0037] Although these algorithms have slightly different principles, they are all based on the geometric features of point cloud for matching, so when the geometric features of point cloud degenerate, the effect of these algorithms is not good.

[0038] Based on this, the embodiments of the present disclosure propose a point cloud matching method. Figure 1 is a schematic flow chart of a point cloud matching method proposed by the embodiments of the present disclosure, which comprises:

[0039] S110, obtaining obstacle point clouds at multiple time instants;

[0040] S120, constructing a factor graph using the obstacle point clouds at the multiple time instants, obstacle poses, and obstacle bounding boxes;

[0041] S130, optimizing the factor graph;

[0042] S140, matching the obstacle point clouds at the multiple time instants based on the optimized factor graph.

[0043] In some examples, step S110 comprises:

[0044] The laser radar system of the ego vehicle is used to acquire point clouds at multiple adjacent time instants respectively;

[0045] Based on the point clouds acquired at the multiple adjacent time instants respectively, the obstacle point clouds of the same obstacle in the multiple time instants are determined.

[0046] The ego vehicle can also be referred to as a host vehicle, and the ego vehicle / host vehicle can be an autonomous vehicle.

[0047] In some examples, the laser radar system of the ego vehicle acquires point clouds of the surrounding environment at multiple continuous time instants respectively. For example, point clouds are acquired at a k time instant (or a time instant corresponding to a kth frame), a k+1 time instant (or a time instant corresponding to a k+1th frame), a k+2 time instant (or a time instant corresponding to a k+2th frame), and so on. The k time instant, the k+1 time instant, the k+2 time instant, and so on, exist in a time sequence, for example, the k time instant is the earliest time instant. The point cloud acquired at the k time instant is referred to as a kth frame point cloud, the point cloud acquired at the k+1 time instant is referred to as a k+1th frame point cloud, the point cloud acquired at the k+2 time instant is referred to as a k+2th frame point cloud, and so on.

[0048] In some examples, a detection model (such as a 3D detection model) is used for the division of the point clouds. The 3D detection model is a neural network model used for target detection in three-dimensional space, which can detect and locate objects from 3D point clouds or other three-dimensional data. In an embodiment of the present disclosure, the 3D detection model is used to identify the point clouds of the dynamic obstacles in the point clouds acquired at the multiple time instants based on 3D bounding boxes. Then, the point clouds of the same obstacles are determined from the point clouds acquired at the multiple time instants.

[0049] In some examples, a Multi-Object Tracking (MOT) algorithm is used to identify the obstacle point clouds of the same obstacle. The MOT algorithm is an algorithm used for tracking multiple targets in video or point cloud data, which can continuously detect and identify multiple targets in a scene in time series data, and assign a unique ID to each target to maintain target consistency across frames. The MOT algorithm is used in an embodiment of the present disclosure to associate the dynamic obstacles of adjacent frames, acquire the continuous correspondence of the dynamic obstacles between frames, and thus determine the obstacle point clouds of the same obstacle in the factor graph, thereby creating conditions for constructing the factor graph.

[0050] For example, the point cloud at time k includes obstacle point clouds corresponding to obstacle 1, obstacle 2, and obstacle 3; the dynamic point cloud at time k+1 includes obstacle point clouds corresponding to obstacle 1, obstacle 2, and obstacle 4. Based on the MOT algorithm, it can be determined that the obstacle point cloud corresponding to obstacle 1 in the point cloud at time k and the obstacle point cloud corresponding to obstacle 1 in the point cloud at time k+1, and assign a unique ID to the two obstacle point clouds, such as ID = 1; it can also be determined that the obstacle point cloud corresponding to obstacle 2 in the point cloud at time k and the obstacle point cloud corresponding to obstacle 2 in the point cloud at time k+1, and assign a unique ID to the two obstacle point clouds, such as ID = 2.

[0051] The main goal of the point cloud matching algorithm proposed by the embodiments of the present disclosure is to find a transformation matrix containing the information of a rotation matrix R and a translation vector t, so that the point cloud at the previous time in the adjacent time and the point cloud at the later time in the adjacent time for the same obstacle are aligned as much as possible under the transformation of the transformation matrix.

[0052] In some embodiments, based on the information of the same obstacle at different times, the factor graph can be constructed by using the above step S120. In the factor graph, the obstacle point clouds of the same obstacle at multiple times are used to form point cloud geometric feature constraints on the transformation matrix; and the obstacle point cloud and the obstacle bounding box are used to form bounding box constraints on the transformation matrix. Based on the aforementioned point cloud geometric feature constraints and bounding box constraints, the factor graph is optimized, so as to realize the matching of the point clouds of the same obstacle at different times, and the accuracy of the point cloud matching can be improved.

[0053] The principle of factor graph optimization (FGO) is based on graph theory and probabilistic graphical models, and is mainly used to process large-scale nonlinear optimization problems. The core idea is to describe the relationship and constraints between multiple variables through a factor graph, and then solve each variable by using a nonlinear optimization algorithm. The factor graph is composed of two basic concepts: nodes and edges. Figure 2 is a schematic diagram of the principle of factor graph optimization, as Figure 2 shown, the circular and elliptical shapes represent nodes. Among them, the circular shape represents a variable node, that is, a node that needs to be solved, such as the position of a vehicle; the elliptical shape represents a constraint node, which represents an observation constraint, such as a common road sign. The line connecting the nodes represents an edge, which represents the constraint condition between the nodes connected by the edge.

[0054] When optimizing the factor graph, the embodiments of the present disclosure can use a nonlinear optimization method to iterate the numerical values of the nodes in the factor graph multiple times until the convergence condition is met; the convergence condition includes at least one of the following:

[0055] (1) the number of iterations is greater than or equal to a threshold value;

[0056] (2) the numerical value of the adjusted node satisfies a constraint condition.

[0057] The nonlinear optimization algorithm includes at least one of the following: a gradient descent algorithm, a Newton algorithm, a quasi-Newton algorithm, etc. Among them, the optimization direction can also be called a search direction. In the case of a gradient descent algorithm, the optimization direction or search direction can be a negative gradient direction or a gradient direction, and the step size can be the magnitude of the adjustment.

[0058] By using the above nonlinear optimization method to iteratively optimize the factor graph, the speed of factor graph optimization can be improved, thereby improving the speed and efficiency of point cloud matching.

[0059] The optimization process includes multiple iterations. In each iteration process, the numerical value in the node is adjusted, and the adjusted node is calculated to determine whether the numerical value of the adjusted node satisfies the constraint condition. If not, the next iteration is performed, i.e., the numerical value in the node is adjusted again, and the numerical value of the adjusted node is determined again. The iteration is repeated until the convergence condition is met, and the optimization process is ended.

[0060] When the optimization process is completed, if the numerical value of the node satisfies the constraint condition, it is considered that the optimization of the factor graph is successful. If the number of iterations reaches the threshold value, but the constraint condition is still not satisfied, it is considered that the optimization of the factor graph fails.

[0061] The embodiments of the present disclosure utilize the obstacle point clouds, obstacle poses, and obstacle bounding boxes at multiple time points to construct and optimize a factor graph, and realize matching of obstacle point clouds at different time points based on the optimized factor graph. The geometric constraints between obstacle point clouds at different time points and the detection constraints of the 3D detection module can be combined to realize stable matching of obstacle point clouds.

[0062] In some examples, constructing the factor graph includes:

[0063] Constructing multiple nodes in the factor graph, the multiple nodes including: obstacle point clouds, obstacle poses, and obstacle bounding boxes at each time point;

[0064] Constructing constraint conditions between the nodes, the constraint conditions including: a first constraint condition between obstacle point clouds and obstacle poses at adjacent time points, and a constraint condition between at least two of the obstacle point clouds, the obstacle poses, and the obstacle bounding boxes;

[0065] Determining the factor graph based on the multiple nodes and the constraint conditions.

[0066] This embodiment utilizes static point clouds, obstacle point clouds, obstacle poses, and vehicle poses at various times to construct a factor map, and performs vehicle localization based on the factor map; it can comprehensively utilize the geometric feature constraints (corresponding to the first constraint condition) and bounding box constraints (corresponding to the second constraint condition) of the point cloud for point cloud matching, thereby improving the accuracy of point cloud matching.

[0067] Figure 3 This is an example of a factor graph constructed according to an embodiment of this disclosure. For example... Figure 3 As shown, the nodes in this factor graph include the following:

[0068] Obstacle point cloud, obstacle pose and obstacle bounding box

[0069] The obstacle point cloud (P) at time k k ), obstacle pose (T) k ) and obstacle bounding box (x k );

[0070] The obstacle point cloud (P) at time k+1 k+1 ), obstacle pose (T) k+1 ) and obstacle bounding box (x k+1 ).

[0071] The factor graph displays nodes at two adjacent time points. In some implementations, the factor graph includes nodes at multiple consecutive time points, where there are multiple sets of adjacent time points. For example, the node at time k and the node at time k+1 form a factor graph, the node at time k+1 and the node at time k+2 form a factor graph, and so on; in one example, during point cloud matching calculations, the factor graph formed by each set of adjacent time point nodes is optimized. Figure 3 This is merely an example and does not represent a limitation on the number of times corresponding to nodes in the factor graph. Furthermore, the symbols used to represent each node in this embodiment are merely illustrative and not intended to limit this disclosure.

[0072] like Figure 3 As shown, the constraints in this factor graph include a first constraint, and also constraints between at least two items in the obstacle point cloud, obstacle pose, and obstacle bounding box. The first constraint (e.g.) Figure 3 In this context, e1) can represent the constraint factor between obstacle point clouds and obstacle poses at adjacent time points, including the obstacle point clouds and obstacle poses at earlier and later time points in adjacent time periods. The first constraint condition can be called a geometric feature constraint, and the constraint conditions between the obstacle point cloud, obstacle pose, and at least two items in the obstacle bounding box can be called bounding box constraints, as follows:

[0073] (I) Point Cloud Geometric Feature Constraints

[0074] In some examples, the first constraint condition is constructed, including:

[0075] determining a first pose transformation matrix of the obstacle point cloud at the adjacent time based on the obstacle pose at the adjacent time;

[0076] transforming the obstacle point cloud at the former time in the adjacent time by using the pose transformation matrix to obtain a first intermediate value;

[0077] constructing the first constraint condition, the first constraint condition including: a difference value between the first intermediate value and the obstacle point cloud at the latter time in the adjacent time is less than or equal to a first threshold value.

[0078] Taking the adjacent times k and k+1 as examples, the time k is the former time, and the time k+1 is the latter time. In some examples, the first constraint condition is expressed as:

[0079]

[0080] wherein e1 approaches 0;

[0081] P k denotes the obstacle point cloud at the time k, P k+1 denotes the obstacle point cloud at the time k+1;

[0082] T k denotes the obstacle pose at the time k, T k+1 denotes the obstacle pose at the time k+1;

[0083] T k+1,k denotes the first pose transformation matrix of the obstacle point cloud at the adjacent time, that is, the pose transformation matrix of the obstacle point cloud (or obstacle) from the time k to the time k+1; T k+1,k may be determined according to T k and T k+1 ;

[0084] ||.|| 2 denotes the square of the second norm;

[0085] i denotes the identification of the point in the point cloud.

[0086] As shown in the above formula (1), the first intermediate value is equal to T k+1,k P k ; the square of the second norm of the difference value between the first intermediate value and the obstacle point cloud at the latter time approaches 0, that is, the difference value between the first intermediate value and the obstacle point cloud at the latter time is less than or equal to the first threshold value.

[0087] It can be seen that the first constraint condition uses the obstacle point cloud at adjacent time of the same obstacle to establish geometric constraints between the obstacle point clouds at adjacent time, which is used as a constraint condition for matching point clouds at different time.

[0088] (ii) bounding box constraint

[0089] As Figure 3 In the above, the constraint condition between at least two of the obstacle point cloud, the obstacle pose and the obstacle bounding box includes at least one of a second constraint condition (such as e2 in Figure 3 ) and a third constraint condition (such as e3 in Figure 3 ). Wherein, the second constraint condition is a constraint condition between the obstacle bounding box and the obstacle point cloud at the same time, which can be called a bounding box constraint of the same frame; the third constraint condition is a constraint condition between the obstacle bounding box and the obstacle point cloud at different time, which can be called a bounding box constraint between different frames. The following are described in detail respectively:

[0090] 1. Bounding box constraint of the same frame:

[0091] In some examples, the second constraint condition includes: each point in the obstacle point cloud is within the range of the obstacle bounding box at the same time.

[0092] In some examples, the second constraint condition is expressed as:

[0093]

[0094] Wherein, e2 approaches to 0;

[0095] x k represents the obstacle bounding box at time k;

[0096] represents the i-th point in the obstacle point cloud at time k, such as including the three-dimensional coordinates of the point, the coordinate system of the three-dimensional coordinates is the coordinate system with the point cloud center point as the origin;

[0097] represents the coordinate system of the i-th point in the obstacle point cloud at time k is converted to the coordinate system with the center point of the obstacle bounding box as the origin;

[0098] represents the distance between the i-th point in the k-th frame point cloud and the center point of the obstacle bounding box;

[0099] S represents the size of the obstacle bounding box, including three values representing length, width and height;

[0100] 0.5S represents the distance between the obstacle bounding box and the center point;

[0101] Ideally, the i-th point in the k-th frame of point cloud is within the obstacle bounding box, i.e. If the i-th point in the k-th frame of point cloud is outside the obstacle bounding box, then

[0102] The principle of setting the second constraint condition is to find points (i.e., points outside the obstacle bounding box) and adjust them to approach 0.

[0103] 2. Bounding box constraint between different frames:

[0104] As shown in Figure 3 , the third constraint condition includes:

[0105] The constraint condition between the obstacle pose and the obstacle bounding box at the previous time and the obstacle pose and the obstacle point cloud at the next time, and / or the constraint condition between the obstacle pose and the obstacle point cloud at the previous time and the obstacle pose and the obstacle bounding box at the next time.

[0106] In an example, the third constraint condition is constructed, including:

[0107] Based on the obstacle pose at the adjacent time, determine the first pose transformation matrix and the second pose transformation matrix of the obstacle point cloud at the adjacent time;

[0108] Transform the obstacle bounding box at the previous time using the first pose transformation matrix to obtain the first transformed bounding box; each point in the obstacle point cloud at the next time is within the first transformed bounding box; and / or,

[0109] Transform the obstacle bounding box at the next time using the second pose transformation matrix to obtain the second transformed bounding box; each point in the obstacle point cloud at the previous time is within the second transformed bounding box.

[0110] In an example as shown in First part: , the third constraint condition includes at least one of the following two parts:

[0111] The constraint condition between T k , T k+1 , x k and P k+1 , as represented by equation (3-1); and,

[0112] The constraint condition between T k , T k+1 , x k+1 and P k , as represented by equation (3-2).

[0113] Specifically, it includes the following parts:

[0114] Second part:

[0115]

[0116] where e 3-1 approaches 0;

[0117] T k represents the obstacle pose at time k, T k+1 represents the obstacle pose at time k+1;

[0118] T k+1,k represents the first pose transformation matrix of the obstacle point cloud, that is, the pose transformation matrix of the obstacle point cloud (or obstacle) from time k to time k+1; T k+1,k can be determined according to T k and T k+1 ;

[0119] x k represents the obstacle bounding box at time k;

[0120] represents the i-th point in the obstacle point cloud at time k+1;

[0121] i represents the identification of the point in the point cloud.

[0122] As formula (3-1), T k+1,k is used to transform x k , that is, to obtain the first transformed bounding box T k+1,k x k ; ideally, the bounding box obtained after transformation (that is, the first transformed bounding box T k+1,k x k ) should be able to wrap the obstacle point cloud at time k+1, that is, each point in the obstacle point cloud at time k+1 is within the first transformed bounding box, that is, If the i-th point in the point cloud at time k+1 is outside the first transformed bounding box, then

[0123] If , then

[0124] If , then:

[0125]

[0126] The principle of setting the third constraint condition is to find the point (that is, the point outside the first transformed bounding box) of , and by adjusting, the part that exceeds approaches 0.

[0127] Figure 4

[0128]

[0129] where e 3-2 approaches 0;

[0130] T k represents the obstacle pose at time k, T k+1 represents the obstacle pose at time k+1;

[0131] T kk+1 represents the second pose transformation matrix of the obstacle point cloud, that is, the pose transformation matrix of the obstacle point cloud (or obstacle) from time k+1 to time k; T k,k+1 can be determined according to T k and T k+1 , the second pose transformation matrix T k,k+1 is the inverse matrix of the first pose transformation matrix T k+1,k ;

[0132] x k+1 represents the obstacle bounding box at time k+1;

[0133] represents the i-th point in the obstacle point cloud at time k;

[0134] i represents the identification of the point in the point cloud.

[0135] As formula (3-2), T k,k+1 is used to transform x k+1 , that is, to obtain the second transformed bounding box T k,k+1 x k+1 ; ideally, the bounding box obtained after transformation (that is, the second transformed bounding box T k,k+1 x k+1 ) should be able to wrap the obstacle point cloud at time k, that is, each point in the obstacle point cloud at time k is within the range of the second transformed bounding box, that is, If the i-th point in the point cloud at time k is outside the second transformed bounding box, then

[0136] If then

[0137] If then:

[0138]

[0139] The principle of setting the third constraint condition is to find The point (that is, the point outside the bounding box of the obstacle) is determined, and the excess part is adjusted to approach 0.

[0140] As can be seen, the second and third constraints utilize the relationship between the obstacle point cloud and the obstacle bounding box to establish constraints between them. In this disclosure, the constraint between the obstacle point cloud and the obstacle bounding box at the same time is referred to as the second constraint, and the constraint between the obstacle point cloud and the obstacle bounding box at different times is referred to as the third constraint. The embodiments of this disclosure combine the second and third constraints with the first constraint, enabling the obstacle bounding box to be used as a matching factor, thereby improving the accuracy of point cloud matching.

[0141] In addition to the nodes and constraints mentioned above, the factor graph proposed in this embodiment may also include other nodes and constraints. In some implementations, the nodes in the factor graph may further include: obstacle velocities at various times; the constraints in the factor graph may further include at least one of a fourth constraint and a fifth constraint; wherein the fourth constraint includes constraints between obstacle velocities at adjacent times, and the fifth constraint includes constraints between obstacle poses and obstacle velocities at adjacent times.

[0142] Figure 4 This is Example 2 of a factor graph constructed according to an embodiment of this disclosure. Figure 4 As shown, the nodes in this factor graph may also include:

[0143] The obstacle velocity (v) at time k k ) and the obstacle velocity (v) at time k+1 k+1 The foregoing symbols are merely examples and are not intended to limit this disclosure.

[0144] like Figure 4 As shown, the constraints in this factor graph may also include a fourth constraint and / or a fifth constraint. In some examples, the fourth constraint includes: the difference in obstacle velocity between adjacent time points is less than or equal to a second threshold.

[0145] Taking adjacent times as k and k+1 as an example, time k is the earlier time and time k+1 is the later time. In some examples, this fourth constraint is expressed as:

[0146] e4 = v k+1 -v k (4)

[0147] Among them, e4 approaches the zero vector;

[0148] v k+1 Let v represent the velocity of the obstacle at time k+1. krepresents a velocity of the dynamic obstacle at the dynamic time instant k; the velocity can be a three-dimensional vector, and three elements in the three-dimensional vector respectively represent velocities of the dynamic obstacle in three coordinate axis directions.

[0149] As shown in the above formula (4), a difference between the velocity of the dynamic obstacle at the k+1 time instant and the velocity at the k time instant approaches a zero vector, i.e., a difference value of the velocities of the dynamic obstacle at adjacent time instants is less than

[0150] In some examples, the manner of constructing the fifth constraint condition comprises:

[0151] determining a third pose transformation matrix based on the obstacle velocity at the adjacent time instant and a time interval between the adjacent time instants;

[0152] transforming, based on the third pose transformation matrix, the obstacle pose at the prior time instant in the adjacent time instants to obtain a second intermediate value;

[0153] constructing a fifth constraint condition, the fifth constraint condition comprising: a difference value between the second intermediate value and the obstacle pose at the subsequent time instant in the adjacent time instants is less than or equal to a third threshold value.

[0154] As shown in the above formula (5), the third pose transformation matrix is equal to exp(v Figure 4 Taking the adjacent time instants k and k+1 as examples, the k time instant is the prior time instant, and the k+1 time instant is the subsequent time instant. In some examples, the fifth constraint condition is expressed as:

[0155] e5=(T k+1 ) -1 exp(v k △t)T k (5)

[0156] wherein e5 approaches a unit matrix;

[0157] T k represents the obstacle pose at the k time instant, and T k+1 represents the obstacle pose at the k+1 time instant; the obstacle pose can be a pose in a world coordinate system;

[0158] v k represents the obstacle velocity at the k time instant (approximating the obstacle velocity at the k+1 time instant);

[0159] △t represents a time interval between the two adjacent time instants;

[0160] exp(), representing mapping of a vector in the parentheses into a matrix;

[0161] As shown in the above formula (5), the third pose transformation matrix is equal to exp(v k△t); Based on the third pose transformation matrix, the obstacle pose at a previous time step is transformed to obtain the second intermediate value; the second intermediate value is equal to exp(v k △t)T. Ideally, the second intermediate value is equal to the obstacle pose at the later time step; therefore, in equation (5), the product of the inverse matrix of the obstacle pose at the later time step and the second intermediate value approaches the identity matrix, which means that the difference between the second intermediate value and the obstacle pose at the later time step is less than or equal to the third threshold.

[0162] The fourth and fifth constraints are based on kinematic principles. Combining the fourth and fifth constraints with the aforementioned constraints to jointly optimize the factor map can improve the accuracy and robustness of point cloud matching.

[0163] The above describes a method for LiDAR localization of a vehicle using factor graph optimization. Embodiments of this disclosure can also assign corresponding weights to each constraint and optimize the factor graph based on these weights.

[0164] For example, the overall constraints for the factor graph are set as follows:

[0165] E = w1e1 + w2e5 + w 3-1 e 3-1 +w 3-2 e 3-2 +w4e4+w5e5 (6)

[0166] Among them, w1, w2, w 3-1 and w 3-2 w4 and w5 represent the weights of the first, second, third, fourth, and fifth constraints, respectively. By setting the weights of each constraint, different levels of importance can be assigned to different constraints, allowing for targeted optimization based on their relative importance during factor graph optimization.

[0167] The following is an example illustrating multiple iterations:

[0168] by Figure 4 Taking the factor plot shown as an example, Figure 4 In the diagram, nodes represented by circles are constraint nodes, and nodes represented by ellipses are variable nodes. During factor graph optimization, the values ​​of constraint nodes remain unchanged, while the values ​​of variable nodes are adjusted in each iteration, ultimately ensuring that the adjusted nodes satisfy the constraints between them.

[0169] After constructing the factor graph, firstly, the fixed values of each constraint node are determined, and the initial values of each variable node are determined. In an example, the initial values of the obstacle poses are determined by the orientation and the center point position of the bounding box of the model detection, the obstacle velocities can be determined by the observation values or the experience values, and the initial values of the obstacle bounding boxes are determined by the orientation and the center point position of the detection box of the model detection.

[0170] In the first iteration process, the initial values of each variable node (such as the obstacle velocity, the obstacle pose, and the obstacle bounding box in the factor graph) are adjusted, and whether the first to fifth constraint conditions are satisfied between the adjusted variable nodes and the constraint nodes is determined; if all or part of the constraint conditions are not satisfied, the second iteration process is entered; Figure 5

[0171] In the second iteration process, the values of each variable node (the values after the adjustment in the first iteration process) are continuously adjusted, and whether the first to fifth constraint conditions are satisfied between the adjusted variable nodes and the constraint nodes is determined; if all or part of the constraint conditions are not satisfied, the third iteration process is entered;

[0172] Until the first to fifth constraint conditions are satisfied between the adjusted variable nodes and the constraint nodes, it is considered that the optimization of the factor graph is successful, and based on the obstacle pose (including the direction and the position) after the last adjustment, the matching relationship of the obstacle point clouds at different time points can be determined, so that the matching of the obstacle point clouds is realized. If the number of iterations reaches the preset number threshold, and all of the first to fifth constraint conditions are not satisfied between the nodes, it is considered that the optimization of the factor graph fails.

[0173] In the factor graph optimization process, the values of the plurality of variable nodes can be adjusted based on a nonlinear optimization algorithm to obtain the values of the plurality of adjusted variable nodes, which can include: determining the optimization direction and the step length corresponding to the plurality of variable nodes based on the nonlinear optimization algorithm and the plurality of constraint conditions in the factor graph; and obtaining the values of the optimized variable nodes based on the optimization direction and the step length. The nonlinear optimization algorithm includes at least one of the following: a gradient descent algorithm, a Newton algorithm, a quasi-Newton algorithm, and the like. The optimization direction can also be referred to as a search direction. In the case where the optimization algorithm is the gradient descent algorithm, the optimization direction or the search direction can be a negative gradient direction or a gradient direction, and the step length can be the adjustment amplitude.

[0174] ​Whether the constraint condition is satisfied can be determined according to the threshold corresponding to the constraint condition. The one or more thresholds corresponding to each constraint condition can be set according to actual conditions, and the one or more thresholds corresponding to the one or more constraint conditions can be the same or different, which is not limited herein. In addition, setting different weights for the constraint conditions can be embodied by setting different thresholds corresponding to the constraint conditions; if the threshold corresponding to a constraint condition is set to be relatively strict, it means that the weight of the constraint condition is relatively high; if the threshold corresponding to a constraint condition is set to be relatively loose, it means that the weight of the constraint condition is relatively low.

[0175] The present disclosure also provides a point cloud matching device, Figure 6 is a structural schematic diagram of a point cloud matching device 500 according to an embodiment of the present disclosure, comprising:

[0176] The point cloud acquisition module 510 is configured to acquire obstacle point clouds at multiple time points.

[0177] The factor graph construction module 520 is configured to construct a factor graph by using the obstacle point clouds at the multiple time points, the obstacle poses, and the obstacle bounding boxes.

[0178] The factor graph optimization module 530 is configured to optimize the factor graph.

[0179] The matching module 540 is configured to match the obstacle point clouds at the multiple time points based on the optimized factor graph.

[0180] In some embodiments, the factor graph construction module 520 is configured to:

[0181] construct multiple nodes in the factor graph, wherein the multiple nodes include the obstacle point clouds at the multiple time points, the obstacle poses, and the obstacle bounding boxes;

[0182] construct constraint conditions between the nodes, wherein the constraint conditions include a first constraint condition between the obstacle point clouds and the obstacle poses at adjacent time points, and a constraint condition between at least two of the obstacle point clouds, the obstacle poses, and the obstacle bounding boxes;

[0183] determine the factor graph based on the multiple nodes and the constraint conditions.

[0184] In some embodiments, the factor graph construction module 520 is configured to:

[0185] determine a first pose transformation matrix of the obstacle point clouds at adjacent time points based on the obstacle poses at the adjacent time points;

[0186] transform the obstacle point clouds at a previous time point in the adjacent time points by using the first pose transformation matrix to obtain a first intermediate value;

[0187] The first constraint condition comprises a difference value between the first intermediate value and the obstacle point cloud at the subsequent moment in the adjacent moment being less than or equal to a first threshold value.

[0188] In some embodiments, the constraint condition between at least two of the obstacle point cloud, the obstacle pose, and the obstacle bounding box comprises:

[0189] At least one of the second constraint condition and the third constraint condition; wherein,

[0190] The second constraint condition is a constraint condition between the obstacle bounding box and the obstacle point cloud at the same moment;

[0191] The third constraint condition comprises a constraint condition between the obstacle pose and the obstacle bounding box at the preceding moment in the adjacent moment and the obstacle pose and the obstacle point cloud at the subsequent moment in the adjacent moment, and / or a constraint condition between the obstacle pose and the obstacle point cloud at the preceding moment in the adjacent moment and the obstacle pose and the obstacle bounding box at the subsequent moment in the adjacent moment.

[0192] In some embodiments, the second constraint condition comprises that each point in the obstacle point cloud at the same moment is within the range of the obstacle bounding box.

[0193] In some embodiments, the factor graph construction module 520 is configured to:

[0194] Determine, based on the obstacle pose at the adjacent moment, a first pose transformation matrix and a second pose transformation matrix of the obstacle point cloud at the adjacent moment;

[0195] Transform, using the first pose transformation matrix, the obstacle bounding box at the preceding moment to obtain a first transformed bounding box, each point in the obstacle point cloud at the subsequent moment being within the range of the first transformed bounding box; and / or

[0196] Transform, using the second pose transformation matrix, the obstacle bounding box at the subsequent moment to obtain a second transformed bounding box, each point in the obstacle point cloud at the preceding moment being within the range of the second transformed bounding box.

[0197] In some embodiments, the node further comprises an obstacle velocity at each moment.

[0198] The constraint condition further comprises at least one of a fourth constraint condition and a fifth constraint condition; wherein,

[0199] The fourth constraint condition comprises a constraint condition between the obstacle velocities at the adjacent moments;

[0200] The fifth constraint condition comprises a constraint condition between the obstacle pose and the obstacle velocity at the adjacent moments.

[0201] In some embodiments, the fourth constraint condition comprises: a difference value of the obstacle velocities of the adjacent time instants is less than or equal to a second threshold value.

[0202] In some embodiments, the factor graph constructing module 520 is configured to:

[0203] determine a third pose transformation matrix based on the obstacle velocities of the adjacent time instants and the time interval between the adjacent time instants;

[0204] transform the obstacle pose of the preceding time instant among the adjacent time instants based on the third pose transformation matrix to obtain a second intermediate value;

[0205] construct a fifth constraint condition comprising: a difference value between the second intermediate value and the obstacle pose of the subsequent time instant among the adjacent time instants is less than or equal to a third threshold value.

[0206] In some embodiments, the point cloud obtaining module 510 is configured to:

[0207] obtain point clouds at a plurality of adjacent time instants respectively by using a laser radar system of the ego vehicle;

[0208] determine obstacle point clouds of the same obstacle in the plurality of time instants based on the point clouds obtained at the plurality of adjacent time instants respectively.

[0209] In some embodiments, the factor graph optimizing module 530 is configured to:

[0210] perform multiple iterations on the numerical values of the nodes in the factor graph by using a nonlinear optimization method until a convergence condition is met; the convergence condition comprises at least one of:

[0211] the number of iterations is greater than or equal to a threshold value;

[0212] the adjusted numerical values of the nodes satisfy the constraint conditions.

[0213] In some embodiments, the factor graph optimizing module 530 is configured to:

[0214] set corresponding weights for the first constraint condition, the second constraint condition, the third constraint condition, the fourth constraint condition and the fifth constraint condition respectively;

[0215] optimize the factor graph based on the respective weights.

[0216] The specific functions and examples of the modules and sub-modules of the apparatuses of the embodiments of the present disclosure are described above in the corresponding steps of the method embodiments, which will not be described here again.

[0217] In the technical solutions of the present disclosure, the acquisition, storage and application of the personal information of the user comply with relevant laws and regulations and do not violate public order and good customs.

[0218] According to embodiments of the present disclosure, the present disclosure also provides an electronic device, a readable storage medium and a computer program product.

[0219] Figure 6 A schematic block diagram of an example electronic device 600 that can be used to implement embodiments of the present disclosure is shown. The electronic device is intended to represent various forms of digital computers, such as laptops, desktops, tablets, personal digital assistants, servers, blade servers, mainframes, and other appropriate computers. The electronic device can also represent various forms of mobile devices, such as personal digital assistants, cellular telephones, smartphones, wearable devices, and other similar computing devices. The components shown here, their connections and relationships, and their functions, are meant to be examples only, and are not meant to limit implementations of the present disclosure described and / or claimed in this document.

[0220] As shown in ​ The device 600 includes a computing unit 601 that can perform various appropriate actions and processes in accordance with a computer program stored in a read-only memory (ROM) 602 or a computer program loaded into a random access memory (RAM) 603 from a storage unit 608. Various programs and data required for the operation of the device 600 can also be stored in the RAM 603. The computing unit 601, the ROM 602, and the RAM 603 are connected to each other through a bus 604. An input / output (I / O) interface 605 is also connected to the bus 604.

[0221] Various components in the device 600 are connected to the I / O interface 605, including an input unit 606 such as a keyboard, a mouse, etc., an output unit 607 such as various types of displays, a speaker, etc., a storage unit 608 such as a magnetic disk, an optical disk, etc., and a communication unit 609 such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 609 allows the device 600 to exchange data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0222] The computing unit 601 can be various general and / or special purpose processing components with processing and computing capabilities. Some examples of the computing unit 601 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various specialized artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 601 performs various methods and processes described above, such as the detection method. For example, in some embodiments, the detection method can be implemented as a computer software program tangibly embodied in a machine-readable medium, such as the storage unit 608. In some embodiments, part or all of the computer program can be loaded and / or installed onto the device 600 via the ROM 602 and / or the communication unit 609. When the computer program is loaded onto the RAM 603 and executed by the computing unit 601, one or more steps of the detection method described above can be performed. Alternatively, in other embodiments, the computing unit 601 can be configured to perform the detection method by any other suitable means, such as by means of firmware.

[0223] Various implementations of the systems and techniques described above can be realized in digital electronic circuitry, integrated circuitry, a field programmable gate array (FPGA), an application specific integrated circuit (ASIC), a system on a chip (SOC), a programmable logic device (PLD), a computer hardware, firmware, software, and / or combinations thereof. These various implementations can include implementation in one or more computer programs that are executable and / or interpretable on a programmable system including at least one programmable processor, which can be special or general purpose, coupled to receive data and instructions from, and to transmit data and instructions to, a storage system, at least one input device, and at least one output device.

[0224] Program code for carrying out methods of the present disclosure can be written in any combination of one or more programming languages. The program code can be provided to a processor or controller of a general purpose computer, special purpose computer, or other programmable data processing apparatus to produce a machine, such that the program code, when executed by the processor or controller, produces the functions / operations specified in the flowcharts and / or the block diagrams. The program code can be executed entirely on a machine, partially on a machine, partially on a machine and partially on a remote machine or entirely on a remote machine or server.

[0225] In the context of this disclosure, a machine-readable medium can be a tangible medium that contains or stores a program for use by or in connection with an instruction execution system, apparatus, or device. The machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can include but is not limited to an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples of the machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0226] To provide for interaction with a user, the systems and techniques described here can be implemented on a computer having a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user and a keyboard and a pointing device (e.g., a mouse or a trackball) by which the user can provide input to the computer. Other kinds of devices can be used to provide for interaction with a user as well; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form, including acoustic, speech, or tactile input.

[0227] The systems and techniques described here can be implemented in a computing system that includes a back end component (e.g., as a data server), or that includes a middleware component (e.g., an application server), or that includes a front end component (e.g., a user computer having a graphical user interface or a Web browser through which a user can interact with an implementation of the systems and techniques described here), or any combination of such back end, middleware, or front end components. The components of the system can be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include a local area network (LAN), a wide area network (WAN), and the Internet.

[0228] The computer system can include clients and servers. A client and server are generally remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a client-server relationship to each other. The server can be a cloud server, a server of a distributed system, or a server combined with a blockchain.

[0229] It should be understood that the various forms of flow shown above can be re-ordered, steps added or removed, etc. For example, the steps recited in the present disclosure can be performed in parallel, in series, in a different order, etc., as long as the desired results of the technology disclosed in the present disclosure are achieved, which is not limited herein.

[0230] The above detailed description does not constitute a limitation of the protection scope of the present disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent replacements, and improvements made within the principles of the present disclosure shall be included in the protection scope of the present disclosure.

Claims

1. A method for point cloud matching, comprising: obtaining a plurality of obstacle point clouds at different time instants; constructing a plurality of nodes and constraints between the nodes using the plurality of obstacle point clouds, obstacle poses and obstacle bounding boxes, and determining a factor graph based on the plurality of nodes and the constraints; optimizing the factor graph; matching the plurality of obstacle point clouds based on the optimized factor graph; wherein the plurality of nodes comprise the obstacle point clouds, obstacle poses and obstacle bounding boxes at different time instants, and the constraints comprise a first constraint and at least one of a second constraint and a third constraint; constructing the first constraint comprises: determining a first pose transformation matrix of the obstacle point clouds at a neighboring time instant based on the obstacle pose at the neighboring time instant; transforming the obstacle point cloud at a preceding time instant in the neighboring time instant using the first pose transformation matrix to obtain a first intermediate value; and the first constraint is that a difference value between the first intermediate value and the obstacle point cloud at a following time instant in the neighboring time instant is less than or equal to a first threshold; the second constraint is a constraint between the obstacle bounding box and the obstacle point cloud at the same time instant, and the third constraint comprises a constraint between the obstacle pose and the obstacle bounding box at the preceding time instant in the neighboring time instant and the obstacle point cloud and the obstacle pose at the following time instant in the neighboring time instant, and / or a constraint between the obstacle pose and the obstacle point cloud at the preceding time instant in the neighboring time instant and the obstacle bounding box and the obstacle pose at the following time instant in the neighboring time instant.

2. The method of claim 1, wherein, the second constraint comprises that each point in the obstacle point cloud at the same time instant is within the obstacle bounding box.

3. The method of claim 1, wherein, constructing the third constraint comprises: determining a first pose transformation matrix and a second pose transformation matrix of the obstacle point clouds at the neighboring time instant based on the obstacle pose at the neighboring time instant; transforming the obstacle bounding box at the preceding time instant using the first pose transformation matrix to obtain a first transformed bounding box, and each point in the obstacle point cloud at the following time instant is within the first transformed bounding box; and / or transforming the obstacle bounding box at the following time instant using the second pose transformation matrix to obtain a second transformed bounding box, and each point in the obstacle point cloud at the preceding time instant is within the second transformed bounding box. 4.The method of any one of claims 1-3, wherein the nodes further comprise obstacle velocities at different time instants; the constraints further comprise at least one of a fourth constraint and a fifth constraint; wherein the fourth constraint comprises a constraint between the obstacle velocities at neighboring time instants; the fifth constraint comprises a constraint between the obstacle pose and the obstacle velocity at the neighboring time instants.

5. The method of claim 4, wherein, the fourth constraint comprises that a difference value between the obstacle velocities at the neighboring time instants is less than or equal to a second threshold.

6. The method of claim 4, wherein, constructing the fifth constraint comprises: determine a third pose transformation matrix based on the obstacle speed at the adjacent time and the time interval of the adjacent time; transform the obstacle pose at a previous time in the adjacent time based on the third pose transformation matrix to obtain a second intermediate value; construct the fifth constraint condition, the fifth constraint condition comprising: a difference value between the second intermediate value and an obstacle pose at a later time in the adjacent time being less than or equal to a third threshold value.

7. The method of claim 1, wherein, the obtaining of the obstacle point cloud at multiple times comprises: obtaining point clouds at multiple adjacent times by using a laser radar system of a host vehicle; determining the obstacle point cloud of a same obstacle at the multiple times based on the point clouds obtained at the multiple adjacent times.

8. The method of claim 4, wherein, the optimization of the factor graph comprises: iterating the numerical values of the nodes in the factor graph by using a nonlinear optimization method until a convergence condition is met; the convergence condition comprising at least one of the following: the number of iterations being greater than or equal to a threshold value; the numerical values of the adjusted nodes satisfying the constraint conditions.

9. The method of claim 8, wherein, the optimization of the factor graph comprises: setting corresponding weights for the first constraint condition, the second constraint condition, the third constraint condition, the fourth constraint condition and the fifth constraint condition respectively; optimizing the factor graph based on the weights.

10. A point cloud matching apparatus, comprising: a point cloud obtaining module configured to obtain obstacle point clouds at multiple times; a factor graph constructing module configured to construct multiple nodes and constraint conditions between the nodes by using the obstacle point clouds at the multiple times, obstacle poses and obstacle bounding boxes, to determine a factor graph based on the multiple nodes and the constraint conditions, to determine a first pose transformation matrix of the obstacle point cloud at adjacent times based on the obstacle poses at the adjacent times, to transform the obstacle point cloud at a previous time in the adjacent time by using the first pose transformation matrix to obtain a first intermediate value, and to construct a first constraint condition; a factor graph optimizing module configured to optimize the factor graph; a matching module configured to match the obstacle point clouds at the multiple times based on the optimized factor graph; wherein the multiple nodes comprise the obstacle point clouds, obstacle poses and obstacle bounding boxes at the multiple times; the constraint conditions comprise the first constraint condition, and at least one of a second constraint condition and a third constraint condition; the first constraint condition is that a difference value between the first intermediate value and an obstacle point cloud at a later time in the adjacent time is less than or equal to a first threshold value; the second constraint condition is a constraint condition between the obstacle bounding box and the obstacle point cloud at a same time; the third constraint condition comprises a constraint condition between an obstacle pose and an obstacle bounding box at a previous time in adjacent times, and an obstacle pose and an obstacle point cloud at a later time in the adjacent times, and / or a constraint condition between an obstacle pose and an obstacle point cloud at a previous time in adjacent times, and an obstacle pose and an obstacle bounding box at a later time in the adjacent times.

11. The apparatus of claim 10, wherein, The second constraint condition comprises: each point in the obstacle point cloud is within the range of the obstacle bounding box at the same time.

12. The apparatus of claim 10, wherein, The factor graph construction module is configured to: determine, based on the obstacle poses of adjacent time instants, a first pose transformation matrix and a second pose transformation matrix of the obstacle point cloud at the adjacent time instants; transform, by using the first pose transformation matrix, the obstacle bounding box at the preceding time instant to obtain a first transformed bounding box; each point in the obstacle point cloud at the subsequent time instant is within the range of the first transformed bounding box; and / or transform, by using the second pose transformation matrix, the obstacle bounding box at the subsequent time instant to obtain a second transformed bounding box; each point in the obstacle point cloud at the preceding time instant is within the range of the second transformed bounding box.

13. The apparatus of any of claims 10-12, wherein, The node further comprises: obstacle velocities at each time instant; The constraint condition further comprises at least one of a fourth constraint condition and a fifth constraint condition; wherein The fourth constraint condition comprises a constraint condition between the obstacle velocities of adjacent time instants; The fifth constraint condition comprises a constraint condition between the obstacle poses and the obstacle velocities of the adjacent time instants.

14. The apparatus of claim 13, wherein, The fourth constraint condition comprises: a difference value of the obstacle velocities of adjacent time instants is less than or equal to a second threshold value.

15. The apparatus of claim 13, wherein, The factor graph construction module is configured to: determine, based on the obstacle velocities of the adjacent time instants and a time interval of the adjacent time instants, a third pose transformation matrix; transform, based on the third pose transformation matrix, the obstacle pose at the preceding time instant among the adjacent time instants to obtain a second intermediate value; construct the fifth constraint condition, which comprises: a difference value of the second intermediate value and the obstacle pose at the subsequent time instant among the adjacent time instants is less than or equal to a third threshold value.

16. The apparatus of claim 10, wherein, The point cloud acquisition module is configured to: acquire, by using a laser radar system of a host vehicle, point clouds at a plurality of adjacent time instants respectively; determine, based on the point clouds acquired at the plurality of adjacent time instants respectively, the obstacle point cloud of a same obstacle among the plurality of time instants.

17. The apparatus of claim 13, wherein, The factor graph optimization module is configured to: perform, by using a nonlinear optimization method, multiple iterations on the numerical values of the nodes in the factor graph until a convergence condition is met; the convergence condition comprises at least one of: an iteration number is greater than or equal to a threshold value; the numerical values of the adjusted nodes satisfy the constraint condition.

18. The apparatus of claim 17, wherein, The factor graph optimization module is configured to: set corresponding weights for the first constraint condition, the second constraint condition, the third constraint condition, the fourth constraint condition and the fifth constraint condition respectively; optimize the factor graph based on the weights.

19. An electronic device comprising: at least one processor; and a memory connected to the at least one processor in communication; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the method of any one of claims 1-9.

20. A non-transitory computer readable storage medium having stored thereon computer instructions, wherein, The computer instructions are used to enable the computer to perform the method of any one of claims 1-9. The computer instructions are used to enable the computer to perform the method of any one of claims 1-9.

21. A computer program product comprising a computer program which, when executed by a processor, implements the method according to any one of claims 1-9.

Images