A pose determination method and apparatus

Abstract

The application discloses a pose determination method and device, and relates to the technical field of mobile robots. A specific embodiment of the method comprises the following steps: dividing a reference point cloud into a plurality of grids; calculating the mean value and the covariance matrix of the reference point cloud in the normal distribution in the grid; performing eigenvalue decomposition on the covariance matrix to obtain the eigenvalue of the grid; transforming the target point cloud based on the current pose to obtain the grid in which the target point cloud is located; determining an effective grid in the grid in which the target point cloud is located according to the eigenvalue of the grid; and calculating the value of the target function according to the mean value, the covariance matrix and the target point cloud in the effective grid. When a preset termination condition is met, the current pose is determined as the target pose; otherwise, the current pose is updated based on the value of the target function, and the transformation of the target point cloud based on the current pose is performed. The embodiment can reduce the calculation amount and improve the positioning accuracy.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mobile robots, and particularly relates to a pose determination method and device. BACKGROUND

[0002] For mobile robots such as autonomous vehicles, a positioning system is a necessary condition to ensure normal operation. Based on the difference of positioning sensors, the positioning method can be divided into satellite positioning, laser radar positioning and visual positioning. In particular, due to good adaptability to urban building structures, laser radar positioning is widely used, including methods based on point cloud matching. The method based on point cloud matching is to match the target point cloud obtained by real-time scanning of the laser radar with the reference point cloud established in advance to obtain the pose of the mobile robot. The currently used point cloud matching method includes NDT (Normal Distributions Transform).

[0003] However, when determining the pose, this method not only uses points such as lines and surfaces with obvious features, but also uses points such as points and spheres with more invalid features, resulting in a large amount of calculation and reduced positioning accuracy. SUMMARY

[0004] Therefore, the embodiments of the present application provide a pose determination method and device, which can avoid using points and spheres with more invalid features, thereby reducing the amount of calculation and improving the positioning accuracy.

[0005] In a first aspect, the embodiments of the present application provide a pose determination method, comprising:

[0006] dividing the reference point cloud into a plurality of grids;

[0007] calculating the mean and covariance matrix of the reference point cloud in the grid under the normal distribution;

[0008] performing eigenvalue decomposition on the covariance matrix to obtain the eigenvalue of the grid;

[0009] transforming the target point cloud based on the current pose to obtain the grid in which the target point cloud is located;

[0010] determining an effective grid in the grid in which the target point cloud is located according to the eigenvalue of the grid; wherein the effective grid is composed of a surface grid with a distribution feature of a surface and / or a line grid with a distribution feature of a line;

[0011] The objective function is calculated based on the mean, the covariance matrix, and the target point cloud in the effective grid. When a preset termination condition is met, the current pose is determined to be the target pose. Otherwise, the current pose is updated based on the value of the objective function, and the transformation of the target point cloud based on the current pose is performed.

[0012] In a second aspect, embodiments of the present invention provide a pose determination device, comprising:

[0013] The partitioning module is configured to divide the reference point cloud into multiple grids; and to calculate the mean and covariance matrix of the reference point cloud in the grids under a normal distribution.

[0014] The eigenvalue decomposition module is configured to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid.

[0015] The transformation module is configured to transform the target point cloud based on the current pose to obtain the mesh in which the target point cloud is located;

[0016] The determination module is configured to determine a valid grid in the grid containing the target point cloud based on the feature values ​​of the grid; wherein the valid grid is composed of a surface grid with a distribution feature of surfaces and / or a line grid with a distribution feature of lines; calculate the value of an objective function based on the mean, the covariance matrix, and the target point cloud in the valid grid; when a preset termination condition is met, determine the current pose as the target pose; otherwise, update the current pose based on the value of the objective function and perform the transformation of the target point cloud based on the current pose.

[0017] Thirdly, embodiments of the present invention provide an electronic device, including:

[0018] One or more processors;

[0019] Storage device for storing one or more programs.

[0020] When the one or more programs are executed by the one or more processors, the one or more processors perform the method as described in any of the above embodiments.

[0021] Fourthly, embodiments of the present invention provide a computer-readable medium having a computer program stored thereon, which, when executed by a processor, implements the method described in any of the above embodiments.

[0022] One embodiment of the above invention has the following advantages or beneficial effects: by eigenvalue decomposition of the covariance matrix, line grids and surface grids are selected in the grid where the target point cloud is located, and only line grids and surface grids with obvious features are used for matching, avoiding the use of points, spheres and other points with many invalid features, which can reduce the amount of computation and improve the positioning accuracy.

[0023] The further effects of the aforementioned unconventional alternative methods will be explained below in conjunction with specific implementation methods. Attached Figure Description

[0024] The accompanying drawings are provided to better understand the invention and are not intended to unduly limit the scope of the invention. Wherein:

[0025] Figure 1 This is a flowchart of a pose determination method provided in one embodiment of the present invention;

[0026] Figure 2 This is a flowchart of a pose determination method provided in another embodiment of the present invention;

[0027] Figure 3 This is a schematic diagram of a pose determination device provided in one embodiment of the present invention;

[0028] Figure 4 This is a schematic diagram of the structure of a computer system suitable for implementing terminal devices or servers of the present invention. Detailed Implementation

[0029] The following description, in conjunction with the accompanying drawings, illustrates exemplary embodiments of the present invention, including various details to aid understanding. These details should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the invention. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0030] Existing methods use points and spheres, which have many invalid features, when determining pose, resulting in a large amount of computation and low positioning accuracy.

[0031] In view of this, such as Figure 1 As shown, an embodiment of the present invention provides a pose determination method, including:

[0032] Step 101: Divide the reference point cloud into multiple grids.

[0033] The reference point cloud is a pre-built point cloud map. Smaller grid sizes result in higher positioning accuracy but higher computational cost, while larger grid sizes reduce computational cost but lower positioning accuracy. Therefore, in practical applications, the grid size can be determined based on business requirements.

[0034] Step 102: Calculate the mean and covariance matrix of the reference point cloud in the grid under a normal distribution.

[0035] Based on the reference point cloud in the grid, calculate the normal distribution parameters of the grid, including the mean and covariance matrix.

[0036] Step 103: Perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid.

[0037] The number of eigenvalues ​​and eigenvectors depends on the specific business scenario. For example, in a 3D scenario, it can be decomposed into 3 eigenvalues ​​and 3 eigenvectors, while in a 2D scenario, it can be decomposed into 2 eigenvalues ​​and 2 eigenvectors. Therefore, different methods can be used to perform eigenvalue decomposition on the covariance matrix according to the needs of the scenario. For ease of description, the following embodiments will use a 3D scenario as an example.

[0038] Step 104: Transform the target point cloud based on the current pose to obtain the mesh containing the target point cloud.

[0039] The first transformation uses a pre-estimated initial pose, and subsequent iterations update the initial pose until the termination condition is met. It should be noted that the execution order of the steps is not unique; for example, step 104 can be executed after step 103 or before step 102.

[0040] Step 105: Determine the effective grid in the grid where the target point cloud is located based on the feature values ​​of the grid; wherein, the effective grid consists of surface grids with surface distribution characteristics and / or line grids with line distribution characteristics.

[0041] Eigenvalues ​​can reflect the distribution characteristics of point clouds along the eigenvector direction. Therefore, based on eigenvalues ​​and their corresponding eigenvectors, line meshes and surface meshes can be selected.

[0042] Step 106: Calculate the value of the objective function based on the mean, covariance matrix, and target point cloud in the effective grid.

[0043] Step 107: Determine whether the preset termination condition is met. If yes, proceed to step 108. Otherwise, update the current pose based on the value of the objective function and proceed to step 104.

[0044] Step 108: Determine the current pose as the target pose.

[0045] The termination condition can be that the number of iterations reaches a certain threshold, or that the value of the objective function reaches a set threshold. This embodiment of the invention does not limit this.

[0046] This invention uses eigenvalue decomposition of the covariance matrix to filter out line grids and surface grids in the grid where the target point cloud is located. Only line grids and surface grids with obvious features are used for matching, avoiding the use of points, spheres and other points with many invalid features, which can reduce the amount of computation and improve the positioning accuracy.

[0047] To ensure positioning accuracy, preferably, the number of points in the grid is generally not less than 6.

[0048] In one embodiment of the present invention, eigenvalue decomposition is performed on the covariance matrix to obtain the eigenvalues ​​of the grid and the corresponding eigenvectors, including:

[0049] Based on the first formula, the covariance matrix is ​​eigenvalued to obtain the eigenvalues ​​of the grid and the corresponding eigenvectors.

[0050] The first formula includes:

[0051]

[0052] In this context, Σ represents the covariance matrix, λ1, λ2, and λ3 represent the eigenvalues, q1, q2, and q3 represent the eigenvectors corresponding to the eigenvalues, and q1, q2, and q3 are pairwise orthogonal.

[0053] To ensure the accuracy of line mesh and surface mesh recognition, the feature vectors are pairwise orthogonal. Through this embodiment of the invention, feature values ​​and feature vectors in a 3D scene can be obtained; similarly, feature values ​​and feature vectors in other dimensional scenes can also be obtained.

[0054] In one embodiment of the present invention, determining a valid grid within the grid containing the target point cloud based on the grid's feature values ​​includes:

[0055] Based on the eigenvalues ​​of the grid and the corresponding eigenvectors, calculate the line eigenvalues ​​and surface eigenvalues ​​of each grid in which the target point cloud is located;

[0056] Based on line feature values ​​and surface feature values, determine the line grid and surface grid in multiple grids where the target point cloud is located;

[0057] Determine the effective grid within the line grid and surface grid containing the target point cloud.

[0058] This invention allows for the selection of line and surface meshes from multiple meshes using feature values. Compared to point cloud distributions in the form of points or spheres, point clouds in line and surface meshes possess more significant features, thereby improving pose accuracy. Furthermore, eliminating point and spherical point cloud distributions reduces computational load and improves pose determination efficiency.

[0059] Considering that when only ground points are available, it is impossible to have good constraints in the horizontal direction, in one embodiment of the present invention,

[0060] Eigenvalues ​​of the grid are obtained by performing eigenvalue decomposition on the covariance matrix, including:

[0061] Eigenvalue decomposition of the covariance matrix yields the eigenvalues ​​of the grid and the corresponding eigenvectors.

[0062] Within the line and surface grids containing the target point cloud, determine the valid grids, including:

[0063] Based on the eigenvalues ​​of the surface mesh and the corresponding eigenvectors, the surface mesh containing the target point cloud is divided into ground mesh and non-ground mesh;

[0064] Determine the effective grids among the line grid, ground grid, and non-ground grids containing the target point cloud.

[0065] This invention determines whether a mesh belongs to a line mesh or a surface mesh by using eigenvalues. Based on eigenvalues ​​and eigenvectors, the surface mesh is further divided into ground meshes and non-ground meshes, thus ensuring that the effective mesh includes not only line meshes and ground meshes but also non-ground meshes, thereby improving positioning accuracy. It should be noted that the above method for determining the effective mesh is applicable not only to 3D scenes but also to 2D scenes and other scenarios.

[0066] In practical applications, the target point cloud can be transformed first, and then the type of the mesh can be determined based on the feature values ​​of the mesh. Alternatively, the type of the mesh can be determined first based on the feature values ​​of the mesh, and then the target point cloud can be transformed.

[0067] In one embodiment of the present invention, the line feature values ​​and surface feature values ​​of each grid in which the target point cloud is located are calculated based on the feature values ​​of the grid, including:

[0068] According to the second formula, calculate the line feature values ​​of each grid in the target point cloud;

[0069] According to the third formula, calculate the surface feature values ​​of each grid in the target point cloud;

[0070] The second formula includes:

[0071]

[0072] The third formula includes:

[0073]

[0074] Where c is used to characterize the line eigenvalue, p is used to characterize the surface eigenvalue, λ1, λ2 and λ3 are used to characterize the eigenvalues ​​respectively, and λ1≤λ2≤λ3.

[0075] Among them, 0≤c<1, the larger the value of c, the more the distribution characteristics tend to be linear; 0≤p<1, the larger the value of p, the more the distribution characteristics tend to be surface-like.

[0076] This invention can accurately describe the distribution characteristics of the grid in the direction of the feature vector through λ1, λ2 and λ3, thereby accurately classifying the grid type and improving positioning accuracy.

[0077] In practical applications, line meshes and surface meshes can be filtered by sorting the c and p values. Alternatively, a preset threshold can be used to filter them; for example, meshes with a c value greater than 0.8 are considered line meshes. Another approach is to first calculate the c value of the mesh to filter out line meshes from multiple meshes, then calculate the p value of the remaining meshes to filter out surface meshes, and vice versa. Of course, both the c and p values ​​can be calculated, and the mesh type can be determined by comparing the c and p values. The calculation methods for c and p values ​​are not limited to equations (2) and (3); for example, c = λ3 - λ2.

[0078] The p-value is similar, p = λ2 - λ1.

[0079] In one embodiment of the present invention, determining line grids and surface grids in multiple grids containing the target point cloud based on line feature values ​​and surface feature values ​​includes:

[0080] When the line feature value is greater than the preset first threshold, the grid where the target point cloud is located is determined to be a line grid; when the surface feature threshold is greater than the preset second threshold, the grid where the target point cloud is located is determined to be a surface grid.

[0081] Considering that line meshes have larger line feature values ​​and area meshes have larger area feature values, this embodiment of the invention determines the mesh type using a preset first threshold and a second threshold. If the line feature value of a mesh is greater than the first threshold and the area feature value is greater than the second threshold, the mesh can be classified as either a line mesh or an area mesh. In practical applications, meshes with the highest line feature values ​​(top 1%) can also be classified as line meshes, and meshes with the highest area feature values ​​(top 2%) can be classified as area meshes, in descending order of value.

[0082] The embodiments of the present invention can comprehensively consider the line features and surface features of the mesh, thereby improving the accuracy of mesh division.

[0083] In one embodiment of the present invention, the surface grid containing the target point cloud is divided into a ground grid and a non-ground grid based on the feature values ​​of the surface grid and the feature vectors corresponding to the feature values, including:

[0084] Calculate the angle between the eigenvector corresponding to λ1 and the ground normal vector. When the angle is less than the set angle threshold, the surface mesh is a ground mesh; otherwise, the surface mesh is a non-ground mesh.

[0085] Since λ1≤λ2≤λ3, the eigenvector corresponding to λ1 is the normal vector of the surface mesh. By calculating the angle between this eigenvector and the ground normal vector, we can determine whether the surface mesh is a ground mesh. Specifically, we can multiply the eigenvector corresponding to λ1 by the vector (0,0,1) and then divide by the magnitude of λ1 to determine whether the surface mesh is a ground mesh. In practical applications, we can also calculate the plane equation of the surface mesh. Based on the plane equation and the ground equation, we can determine whether the surface mesh is a ground mesh.

[0086] Through the embodiments of the present invention, the surface mesh can be further divided into ground mesh and non-ground mesh, ensuring that the effective mesh includes non-ground mesh, and avoiding the inability to effectively constrain the horizontal direction when there are only ground points.

[0087] In one embodiment of the present invention, determining the effective grid among the line grid, ground grid, and non-ground grid where the target point cloud is located includes:

[0088] Based on the line feature values ​​from largest to smallest, select the first number of line grids in the line grid where the target point cloud is located as valid grids;

[0089] Based on the surface feature values ​​from largest to smallest, select the second number of ground grids in the ground grid where the target point cloud is located as valid grids;

[0090] Based on the order of surface feature values ​​from largest to smallest, the third number of non-ground grids in the non-ground grids containing the target point cloud are selected as valid grids.

[0091] Considering that ground points alone cannot provide sufficient constraint in the horizontal direction, the effective mesh determined in this embodiment of the invention includes line meshes, ground meshes, and non-ground meshes. Given that a larger c value approximates a line and a larger p value approximates a surface, this embodiment selects the effective mesh in descending order of line and surface feature values. In practical applications, all line and surface meshes can also be considered as effective meshes.

[0092] The embodiments of the present invention further reduce the amount of computation and improve positioning accuracy through grid filtering.

[0093] Preferably, the second quantity is the same as the third quantity, and the first quantity is equal to the sum of the second and third quantities.

[0094] In practical applications, the first, second, and third quantities can also be in other proportional relationships, such as 1:1:1.

[0095] In one embodiment of the present invention, the objective function includes:

[0096]

[0097] Among them, score(P) vehicle ) is used to characterize the score of the current pose, t ki μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k ∑ is used to characterize the mean of the reference point cloud in the kth effective grid under a normal distribution. k M is used to characterize the covariance matrix of the reference point cloud of the k-th effective grid under a normal distribution. k Used to characterize the number of points in the target point cloud of the kth effective grid.

[0098] In this embodiment of the invention, the objective function includes the mean and covariance matrix of the reference point cloud in the effective grid under a normal distribution, as well as the coordinates of points in the target point cloud in the effective grid.

[0099] In practical applications, nonlinear optimization methods such as Newton's method and gradient descent can be used to optimize the score (P). vehicle ) Optimize to find a way to make score(P) vehicle The largest P vehicle .

[0100] The objective function is the probability density function of a normal distribution or a variation thereof. For example, the objective function can also be Equation (5).

[0101]

[0102] In the optimization process, this invention uses only effective grids and avoids using points or spherical point clouds with many invalid features, thereby improving positioning accuracy and efficiency.

[0103] like Figure 2 As shown, this embodiment of the invention uses an autonomous vehicle as an example to illustrate a pose determination method in a three-dimensional scene. The method includes:

[0104] Step 201: Divide the reference point cloud into multiple grids.

[0105] The area requiring localization is scanned using the onboard LiDAR of an autonomous vehicle to obtain a reference point cloud. This reference point cloud is then divided into a 50cm × 50cm × 50cm grid.

[0106] Step 202: Calculate the mean and covariance matrix of the reference point cloud in the grid under a normal distribution.

[0107] For grids with at least 6 points, calculate the mean coordinates of the reference point cloud in each grid under a normal distribution, and calculate the covariance matrix based on the mean coordinates.

[0108] Step 203: Perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid and the corresponding eigenvectors.

[0109] Based on equation (1), the covariance matrix is ​​decomposed to obtain the eigenvalues ​​λ1, λ2 and λ3 of the grid, and the corresponding eigenvectors q1, q2 and q3. q1, q2 and q3 are pairwise orthogonal.

[0110] Step 204: Transform the target point cloud based on the current pose to obtain the mesh containing the target point cloud.

[0111] When the target point cloud is obtained by scanning, the target point cloud is transformed according to the pre-estimated initial vehicle pose or the current pose determined by the previous iteration to obtain the grid where the target point cloud is located.

[0112] Step 205: Calculate the line and surface feature values ​​of each grid in which the target point cloud is located, based on the feature values ​​of the grid.

[0113] Based on equations (2) and (3), the c-value and p-value of the mesh are calculated.

[0114] Step 206: When the line feature value is greater than the preset first threshold, the grid where the target point cloud is located is determined to be a line grid; when the surface feature threshold is greater than the preset second threshold, the grid where the target point cloud is located is determined to be a surface grid.

[0115] Step 207: Calculate the angle between the eigenvector corresponding to λ1 and the ground normal vector. When the angle is less than the set angle threshold, the surface mesh is a ground mesh; otherwise, the surface mesh is a non-ground mesh.

[0116] Step 208: Select the first number of line grids as valid grids in the line grid where the target point cloud is located, according to the order of line feature values ​​from largest to smallest.

[0117] Select h line grids as valid grids in descending order of c value.

[0118] Step 209: Select the second number of ground grids as valid grids from the ground grids containing the target point cloud, in descending order of surface feature values.

[0119] Based on the p-values ​​from largest to smallest, u1 ground grids are selected as valid grids.

[0120] Step 210: Select the third number of non-ground grids as valid grids from the non-ground grids containing the target point cloud, according to the order of surface feature values ​​from largest to smallest.

[0121] Based on the p-values ​​from largest to smallest, select u2 non-ground grids as valid grids.

[0122] u1 = u2, h = u1 + u2.

[0123] Step 211: Calculate the value of the objective function based on the mean, covariance matrix, and target point cloud in the effective grid.

[0124] Step 212: Determine whether the termination condition is met. If yes, proceed to step 213. Otherwise, update the current pose based on the value of the objective function and proceed to step 204.

[0125] Step 213: Determine the current pose as the target pose.

[0126] The selected valid grid is used as the final grid to participate in the matching. The optimization iteration is performed based on the objective function of Equation (4) to obtain the final vehicle pose.

[0127] The embodiments of the present invention can filter out some point clouds that affect positioning accuracy, such as tree canopies, and select point clouds with distribution features of lines and surfaces, such as lampposts, tree trunks and walls. This not only improves calculation efficiency, but also improves positioning accuracy.

[0128] like Figure 3 As shown, an embodiment of the present invention provides a pose determination device, including:

[0129] The partitioning module 301 is configured to divide the reference point cloud into multiple grids; and to calculate the mean and covariance matrix of the reference point cloud in the grids under a normal distribution.

[0130] The eigenvalue decomposition module 302 is configured to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid.

[0131] The transformation module 303 is configured to transform the target point cloud based on the current pose to obtain the mesh where the target point cloud is located;

[0132] The determination module 304 is configured to determine the effective grid in the grid where the target point cloud is located based on the feature values ​​of the grid; wherein, the effective grid consists of surface grids with surface distribution features and / or line grids with line distribution features; the objective function is calculated based on the mean, covariance matrix and the target point cloud in the effective grid; when the preset termination condition is met, the current pose is determined to be the target pose; otherwise, the current pose is updated based on the objective function value and the transformation module 303 is triggered.

[0133] In one embodiment of the present invention, the feature decomposition module 302 is configured to perform feature decomposition on the covariance matrix based on the first formula to obtain the eigenvalues ​​of the grid and the eigenvectors corresponding to the eigenvalues.

[0134] The first formula includes:

[0135]

[0136] In this context, Σ represents the covariance matrix, λ1, λ2, and λ3 represent the eigenvalues, q1, q2, and q3 represent the eigenvectors corresponding to the eigenvalues, and q1, q2, and q3 are pairwise orthogonal.

[0137] In one embodiment of the present invention, the determining module 304 is configured to calculate the line feature value and surface feature value of each grid in which the target point cloud is located based on the feature value of the grid; determine the line grid and surface grid in the multiple grids in which the target point cloud is located based on the line feature value and surface feature value; and determine the valid grid in the line grid and surface grid in which the target point cloud is located.

[0138] In one embodiment of the present invention, the feature decomposition module 302 is configured to perform feature decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid and the eigenvectors corresponding to the eigenvalues; the determination module 304 is configured to divide the surface grid where the target point cloud is located into ground grids and non-ground grids according to the eigenvalues ​​of the surface grid and the eigenvectors corresponding to the eigenvalues; and determine the effective grids among the line grids, ground grids and non-ground grids where the target point cloud is located.

[0139] In one embodiment of the present invention, the determining module 304 is configured to calculate the line feature values ​​of each grid in which the target point cloud is located according to the second formula; and to calculate the surface feature values ​​of each grid in which the target point cloud is located according to the third formula.

[0140] The second formula includes:

[0141]

[0142] The third formula includes:

[0143]

[0144] Where c is used to characterize the line eigenvalue, p is used to characterize the surface eigenvalue, λ1, λ2 and λ3 are used to characterize the eigenvalues ​​respectively, and λ1≤λ2≤λ3.

[0145] In one embodiment of the present invention, the determining module 304 is configured to determine the grid where the target point cloud is located as a line grid when the line feature value is greater than a preset first threshold, and to determine the grid where the target point cloud is located as a surface grid when the surface feature threshold is greater than a preset second threshold.

[0146] In one embodiment of the present invention, the determining module 304 is configured to calculate the angle between the feature vector corresponding to λ1 and the ground normal vector. When the angle is less than a set angle threshold, the surface mesh is a ground mesh; otherwise, the surface mesh is a non-ground mesh.

[0147] In one embodiment of the present invention, the determining module 304 is configured to select a first number of line grids as valid grids in the line grid where the target point cloud is located, in descending order of line feature values; select a second number of ground grids as valid grids in the ground grid where the target point cloud is located, in descending order of surface feature values; and select a third number of non-ground grids as valid grids in the non-ground grid where the target point cloud is located, in descending order of surface feature values.

[0148] In one embodiment of the present invention, the objective function includes:

[0149]

[0150] Among them, score(P) vehicle ) is used to characterize the score of the current pose, t ki μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k ∑ is used to characterize the mean of the reference point cloud in the kth effective grid under a normal distribution. k M is used to characterize the covariance matrix of the reference point cloud of the k-th effective grid under a normal distribution. k Used to characterize the number of points in the target point cloud of the kth effective grid.

[0151] This invention provides an electronic device, comprising:

[0152] One or more processors;

[0153] Storage device for storing one or more programs.

[0154] When one or more programs are executed by one or more processors, the one or more processors implement the methods as described in any of the above embodiments.

[0155] This invention provides a computer-readable medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any of the above embodiments.

[0156] The following is for reference. Figure 4 It shows a schematic diagram of the structure of a computer system 400 suitable for implementing a terminal device of the present invention. Figure 4 The terminal device shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of the present invention.

[0157] like Figure 4As shown, the computer system 400 includes a central processing unit (CPU) 401, which can perform various appropriate actions and processes based on programs stored in read-only memory (ROM) 402 or programs loaded from storage section 408 into random access memory (RAM) 403. The RAM 403 also stores various programs and data required for the operation of the system 400. The CPU 401, ROM 402, and RAM 403 are interconnected via a bus 404. An input / output (I / O) interface 405 is also connected to the bus 404.

[0158] The following components are connected to I / O interface 405: an input section 406 including a keyboard, mouse, etc.; an output section 407 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and speakers, etc.; a storage section 408 including a hard disk, etc.; and a communication section 409 including a network interface card such as a LAN card, modem, etc. The communication section 409 performs communication processing via a network such as the Internet. A drive 410 is also connected to I / O interface 405 as needed. A removable medium 411, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on drive 410 as needed so that computer programs read from it can be installed into storage section 408 as needed.

[0159] In particular, according to the embodiments disclosed in this invention, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this invention include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 409, and / or installed from removable medium 411. When the computer program is executed by central processing unit (CPU) 401, it performs the functions defined above in the system of this invention.

[0160] It should be noted that the computer-readable medium shown in this invention can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In this invention, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. Computer-readable signal media can also be any computer-readable medium other than computer-readable storage media, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wireless, wire, optical fiber, RF, etc., or any suitable combination thereof.

[0161] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0162] The modules described in the embodiments of the present invention can be implemented in software or hardware. The described modules can also be housed in a processor; for example, a processor can be described as including a sending module, an acquisition module, a determining module, and a first processing module. The names of these modules do not necessarily limit the module itself; for example, the sending module can also be described as "a module that sends an image acquisition request to a connected server."

[0163] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can occur depending on design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A pose determination method, characterized in that, include: Divide the reference point cloud into multiple grids; Calculate the mean and covariance matrix of the reference point cloud in the grid under a normal distribution; The covariance matrix is ​​subjected to eigenvalue decomposition to obtain the eigenvalues ​​of the grid. The target point cloud is transformed based on the current pose to obtain the grid in which the target point cloud is located; Based on the feature values ​​of the grid, effective grids are determined in the grid where the target point cloud is located; wherein, the effective grids are composed of surface grids with surface distribution characteristics and / or line grids with line distribution characteristics; surface grids include ground grids and non-ground grids, the number of effective surface grids is the same as the number of effective non-ground grids, and the number of effective line grids is equal to the sum of the number of effective surface grids and the number of effective non-ground grids; The objective function is calculated based on the mean, the covariance matrix, and the target point cloud in the effective grid. When a preset termination condition is met, the current pose is determined to be the target pose. Otherwise, the current pose is updated based on the value of the objective function, and the transformation of the target point cloud based on the current pose is performed. The determination of valid grids includes: Calculate the angle between the normal vector of the face mesh and the ground normal vector. When the angle is less than a set angle threshold, the face mesh is a ground mesh; otherwise, the face mesh is a non-ground mesh. According to the order of the line feature values ​​of the grid from largest to smallest, a first number of line grids in the line grid where the target point cloud is located are selected as the effective grids; According to the order of the surface feature values ​​of the grid from largest to smallest, a second number of ground grids are selected as the effective grids in the ground grid where the target point cloud is located; According to the order of the surface feature values ​​from largest to smallest, the third number of non-ground grids in the non-ground grid where the target point cloud is located are selected as the effective grids.

2. The method as described in claim 1, characterized in that, Based on the feature values ​​of the grid, valid grids are determined within the grid containing the target point cloud, including: Based on the feature values ​​of the grid, calculate the line feature values ​​and surface feature values ​​of each grid in which the target point cloud is located; Based on the line feature value and the surface feature value, determine the line grid and the surface grid in the multiple grids where the target point cloud is located; The effective grid is determined within the line grid and surface grid where the target point cloud is located.

3. The method as described in claim 2, characterized in that, The covariance matrix is ​​subjected to eigenvalue decomposition to obtain the eigenvalues ​​of the grid, including: Eigenvalue decomposition is performed on the covariance matrix to obtain the eigenvalues ​​of the grid and the eigenvectors corresponding to the eigenvalues; Determining the effective grid within the line grid and surface grid containing the target point cloud includes: Based on the feature values ​​of the surface mesh and the feature vectors corresponding to the feature values, the surface mesh containing the target point cloud is divided into ground mesh and non-ground mesh; The effective grid is determined from the line grid, ground grid, and non-ground grid in which the target point cloud is located.

4. The method as described in claim 3, characterized in that, Eigenvalue decomposition is performed on the covariance matrix to obtain the eigenvalues ​​of the grid and the corresponding eigenvectors, including: Based on the first formula, the covariance matrix is ​​decomposed into eigenvalues ​​of the grid and the eigenvectors corresponding to the eigenvalues. The first formula includes: Wherein, ∑ is used to represent the covariance matrix, λ1, λ2 and λ3 are used to represent the eigenvalues, q1, q2 and q3 are used to represent the eigenvectors corresponding to the eigenvalues, and q1, q2 and q3 are pairwise orthogonal; Based on the feature values ​​of the grid, calculate the line feature values ​​and surface feature values ​​of each grid in which the target point cloud is located, including: calculating the line feature values ​​of each grid in which the target point cloud is located according to the second formula; According to the third formula, calculate the surface feature values ​​of each grid in which the target point cloud is located; The second formula includes: The third formula includes: Where c is used to characterize the line feature value, p is used to characterize the surface feature value, λ1, λ2 and λ3 are used to characterize the feature value respectively, and λ1≤λ2≤λ3.

5. The method as described in claim 4, characterized in that, Based on the line feature value and the surface feature value, the line grid and the surface grid are determined in the multiple grids containing the target point cloud, including: When the line feature value is greater than a preset first threshold, the grid where the target point cloud is located is determined to be a line grid; when the surface feature threshold is greater than a preset second threshold, the grid where the target point cloud is located is determined to be a surface grid.

6. The method as described in claim 1, characterized in that, The objective function includes: Among them, score(P) vehicle ) is used to characterize the score of the current pose, t ki μ is used to characterize the coordinates of the i-th point in the target point cloud of the k-th effective grid. k ∑ is used to characterize the mean of the reference point cloud in the kth effective grid under a normal distribution. k M is used to characterize the covariance matrix of the reference point cloud of the k-th effective grid under a normal distribution. k Used to characterize the number of points in the target point cloud of the kth effective grid.

7. A pose determination device, characterized in that, include: The partitioning module is configured to divide the reference point cloud into multiple grids; Calculate the mean and covariance matrix of the reference point cloud in the grid under a normal distribution; The eigenvalue decomposition module is configured to perform eigenvalue decomposition on the covariance matrix to obtain the eigenvalues ​​of the grid. The transformation module is configured to transform the target point cloud based on the current pose to obtain the mesh in which the target point cloud is located; The determination module is configured to determine effective grids in the grid containing the target point cloud based on the feature values ​​of the grid; wherein, the effective grids consist of surface grids with surface distribution characteristics and / or line grids with line distribution characteristics; the objective function is calculated based on the mean, the covariance matrix, and the target point cloud in the effective grids; when a preset termination condition is met, the current pose is determined to be the target pose; otherwise, the current pose is updated based on the value of the objective function, and the transformation module is triggered; the surface grids include ground grids and non-ground grids, the number of effective surface grids is the same as the number of effective non-ground grids, and the number of effective line grids is equal to the sum of the number of effective surface grids and the number of effective non-ground grids; The determination module is further configured to calculate the angle between the normal vector of the face grid and the ground normal vector. When the angle is less than a set angle threshold, the face grid is a ground grid; otherwise, the face grid is a non-ground grid. The module selects a first number of line grids as valid grids from the line grids containing the target point cloud, in descending order of their line feature values. It also selects a second number of ground grids as valid grids from the ground grids containing the target point cloud, in descending order of their face feature values. Finally, it selects a third number of non-ground grids as valid grids from the non-ground grids containing the target point cloud, in descending order of their face feature values.

8. An electronic device, characterized in that, include: One or more processors; Storage device for storing one or more programs. When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1-6.

Images