Loopback detection method and device and vehicle

Abstract

The invention relates to a loopback detection method and device and a vehicle, and the method comprises the steps: generating a multi-resolution space division tree based on stored point cloud data, the multi-resolution space division tree comprises at least one tree node, the tree node stores point cloud data, and the multi-resolution space division tree is a multilayer quadtree or octree; a point cloud frame is received, a coordinate set of to-be-registered point cloud points in the frame is acquired, a target underlying tree node is acquired based on the coordinate set, and the target underlying tree node comprises at least one coordinate in the coordinate set; judging whether the overlapping degree of the frame and the target underlying tree node meets a threshold value or not; and if the frame meets the threshold value, registering the frame and the multi-resolution space division tree, sequentially taking the optimal rotation translation parameter obtained by the tree nodes of the upper layer as the initial value of the tree nodes of the lower layer from the top layer to the bottom layer, and calculating the optimal rotation translation parameter of the tree nodes of the lower layer until the tree nodes of the bottom layer or registration fails.

Description

Technical Field

[0001] The invention relates to a loop detection method, a device and a vehicle, and belongs to the technical field of computer-aided driving. Background Art

[0002] In the vision-based SLAM (Simultaneous Localization and Mapping) system, due to the influence of cumulative errors, the vehicle trajectory will gradually deviate over time, and when it returns to the position it has been, there will be obvious deviations, especially in scenes lacking global GNSS (Global Navigation Satellite System) signals, such as underground garages and tunnels, the deviation will be greater. Loop detection can effectively eliminate the deviation. The current loop detection method is generally based on the visual bag-of-words model for registration. It is necessary to extract image features first and then match them according to feature descriptors. This feature matching method is easily affected by the size of the bag-of-words and / or lighting conditions. When the size of the bag-of-words is too small or the lighting conditions are poor, resulting in insufficient extracted features, it is easy to produce wrong matches. Once the matching is wrong, inconsistent mapping will occur, which will further affect the positioning and mapping accuracy. In addition, the feature point detection and matching module needs to be configured to realize the image feature extraction function. This module consumes a lot of computing resources and is therefore not a standard configuration module for current smart cars. Summary of the invention

[0003] In view of the shortcomings of the prior art, the present invention provides a loop detection method, device and vehicle, which utilize stored point cloud data to construct a multi-resolution space partition tree, can quickly discover loops, and combine the use of a registration algorithm to perform step-by-step iterative registration from coarse to fine. Without occupying additional computing resources, accurate registration can be achieved even in scenes where there is a lack of global positioning or the initial deviation is large, thereby obtaining good loop detection effects.

[0004] According to a first aspect of the present invention, a loop detection method is provided, comprising:

[0005] Generate a multi-resolution space partition tree based on the stored point cloud data, the multi-resolution space partition tree includes at least one tree node, the tree node stores the point cloud data, and the multi-resolution space partition tree is a multi-layer quadtree or octree;

[0006] Receive a point cloud frame, obtain a coordinate set of a point cloud point to be registered in the frame, and obtain a target bottom-level tree node based on the coordinate set, wherein the target bottom-level tree node includes at least one coordinate in the coordinate set;

[0007] Determine whether the overlap between the frame and the target bottom tree node meets a threshold;

[0008] If the threshold is met, the frame is aligned with the multi-resolution space partitioning tree, and the optimal rotation and translation parameters obtained from the previous tree node are used as the initial values ​​of the next tree node from the top layer to the bottom layer, and the optimal rotation and translation parameters of the next tree node are calculated until the bottom tree node or the alignment fails.

[0009] According to one embodiment, the point cloud data consists of a set of perceptual vector points.

[0010] According to one embodiment, the generating of a multi-resolution space partitioning tree based on the stored point cloud data, the multi-resolution space partitioning tree comprising at least one tree node, the tree node storing the point cloud data, comprises:

[0011] Based on the coordinates of each point cloud point in the point cloud data, determine one by one whether the coordinates of the point cloud point are included in the top-level tree node of the multi-resolution quadtree. If not, create a top-level tree node containing the point cloud point. If included, add the point cloud point to the top-level tree node, and then find out whether there is a child node containing the coordinates of the point cloud point in the next layer. If not, create a child node containing the point cloud point. If it already exists, add the point cloud point to the child node, and so on, until the bottom-level tree node.

[0012] According to one embodiment, the acquiring a target bottom-level tree node based on the coordinate set, wherein the target bottom-level tree node contains at least one coordinate in the coordinate set, comprises:

[0013] Based on the coordinates in the coordinate set, determine one by one whether the top-level tree nodes of the multi-resolution spatial partitioning tree have an inclusion relationship with the coordinates. If so, traverse the next layer of tree nodes to determine whether each node of this layer has an inclusion relationship with the coordinates, exclude nodes without an inclusion relationship, and further search for the next layer of tree nodes with an inclusion relationship, and so on, until the bottom layer of tree nodes, and obtain the bottom layer tree nodes that have an inclusion relationship with the coordinates.

[0014] According to one embodiment, the determining whether the overlap between the frame and the target bottom-level tree node satisfies a threshold comprises:

[0015] Determining the total number of point cloud points to be registered in the frame;

[0016] Counting the number of points to be registered contained in the target bottom-level tree node;

[0017] It is determined whether the ratio of the number of the to-be-registered points to the total number of the to-be-registered points is not less than a set threshold, and if so, it is determined that the threshold is met.

[0018] According to one embodiment, the registering of the frame with the multi-resolution space partitioning tree, sequentially using the optimal rotation and translation parameters obtained from the upper layer tree nodes as the initial values ​​of the next layer tree nodes from the top layer to the bottom layer, and calculating the optimal rotation and translation parameters of the next layer tree nodes until the bottom layer tree nodes or the registration fails, includes:

[0019] Each tree node in the multi-resolution space partitioning tree is used as a voxel in the NDT algorithm, and the mean and covariance of the normal distribution of the point cloud in each node are calculated;

[0020] Taking the top-level tree node of the multi-resolution spatial partitioning tree as a voxel, the Gauss-Newton algorithm is used to solve the optimization problem in the following formula to obtain the optimal estimated rotation and translation parameters (R, t) of the frame.

[0021]

[0022] Among them, R is the rotation matrix, t is the translation vector, and p j represents the point to be registered, n represents the number of points to be registered, u i and Σ i is the mean and covariance of the normal distribution within the voxel, and m is the number of successfully matched voxels;

[0023] The optimal rotation and translation parameters (R, t) obtained from the previous tree node are used as the initial values ​​of the Gauss-Newton iterative algorithm of the next tree node, and the optimization problem is continued to be solved with the next tree node as the voxel, and so on, until the bottom tree node or the registration fails.

[0024] According to one embodiment, the loop detection method further includes:

[0025] The sliding window is composed of historical point cloud frames;

[0026] Add the current point cloud frame to the sliding window and adjust all the poses in the sliding window;

[0027] When the number of frames in the sliding window reaches a set number threshold, a specified number of frames are moved out of the sliding window and added to the tree nodes of the multi-resolution space partitioning tree.

[0028] According to a second aspect of the present invention, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps of the loop detection method described in the first aspect of the present invention are performed.

[0029] According to a third aspect of the present invention, a loop detection device is provided, the device comprising a memory and a processor; the memory is used to store software applications; the processor is used to execute the software applications, and each program of the software application correspondingly executes each step of the loop detection method described in the first aspect of the present invention.

[0030] According to a fourth aspect of the present invention, a vehicle is provided, the vehicle comprising the loop detection device according to the third aspect of the present invention.

[0031] Technical features and beneficial effects of the present invention:

[0032] The present invention utilizes the stored point cloud data to construct a multi-resolution space partition tree, which can quickly detect loops. It combines the use of a multi-resolution registration algorithm to perform step-by-step iterative registration from coarse to fine. It is not easily affected by lighting or environmental textures, does not require additional computing resources, and can achieve accurate registration even in an environment lacking global positioning or with large initial deviations, thereby achieving good loop detection effects and ensuring the consistency and accuracy of visual mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 A schematic diagram of a loop detection method provided by an embodiment of the present invention.

[0034] Figure 2 A schematic diagram of a quadtree provided in an embodiment of the present invention that searches for bottom-level nodes with a containment relationship from top to bottom step by step. DETAILED DESCRIPTION

[0035] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with specific implementation methods.

[0036] A quadtree is a tree data structure that describes a two-dimensional space. Each node of a quadtree represents a square area. Each node can have four child nodes, corresponding to the four quadrants of the square area of ​​its parent node. Each node of an octree represents a cube space. Each node can have eight child nodes. The volume elements represented by the eight child nodes added together equal the cube of the parent node. An octree is an extension of a quadtree in three-dimensional space. Depending on the needs of the application, a quadtree or an octree can be constructed based on point cloud data. The following embodiments take a quadtree as an example to introduce the application of the present invention in the field of autonomous driving.

[0037] Embodiment 1:

[0038] like Figure 1 As shown, a loop detection method includes the following steps:

[0039] S100, constructing a multi-resolution quadtree based on the stored point cloud data, where the multi-resolution quadtree includes at least one tree node, and each tree node stores point cloud data.

[0040] Among them, the multi-resolution quadtree caches the world coordinate system point cloud data composed of a set of perception vector points. The multi-resolution quadtree divides the cached point cloud data into two-dimensional space according to geographic coordinates to form quadtree nodes. Each node of the quadtree corresponds to a square map. The top layer of the multi-resolution quadtree has only one node corresponding to the global map. Except for the leaf nodes at the bottom layer, each node of the quadtree can create four child nodes, and the four child nodes correspond to the sub-maps of the four quadrants of the square of its parent node. In the multi-resolution quadtree, tree nodes are only created for sub-maps containing point cloud data. The multi-resolution quadtree is layered according to the size of the map. Each layer has a resolution. The lower the level, the higher the resolution. Each node has a unique code. The multi-resolution quadtree can quickly search for the corresponding quadtree node according to the geographic coordinates or node code, thereby obtaining the point cloud data stored in the node.

[0041] For example, the resolution of a certain level of quadtree is block_size, and the node encoding is: (level, x, y), which represents the geographical range: x: {x*block_size~(x+1)*block_size}, y:

[0042] {y*block_size~(y+1)*block_size}. Its neighborhood, the left adjacent node is encoded as (level,x-1,y), the right adjacent node is encoded as (level,x+1,y), the upper node is encoded as (level,x,y-1), and the lower node is encoded as (level,x,y+1). If the node is a non-leaf node, the 4 child nodes contained in the node are encoded as follows: (level+1,2x,2y), (level+1,2x+1,2y), (level+1,2x,2y+1), (level+1,2x+1,2y+1). The geographical range of the child node is one quarter of its parent node, and its code can be directly inferred from the parent node code. Therefore, based on the multi-resolution quadtree, if the geographical coordinates are known, the code of the corresponding node can be quickly searched.

[0043] Wherein, S100 specifically includes the following steps:

[0044] S101, selecting point cloud points that have not yet been added to the multi-resolution quadtree from the stored point cloud data;

[0045] S102, calculating the top-level node of the quadtree that can contain the point cloud point according to the coordinates of the point cloud point, and if the top-level node does not exist, creating the top-level node, and if the top-level node already exists, adding the point cloud point to the top-level node;

[0046] S103, according to the coordinate range of the parent node of the upper layer, searching whether there is a child node in the next layer containing the coordinates of the point cloud point, if the child node does not exist, creating the child node, if the child node already exists, adding the point cloud point to the child node;

[0047] S104, determine whether the bottom layer has been reached, if not, execute S103, if yes, determine whether all stored point cloud data have been added to the multi-resolution quadtree, if not, return to S101, if yes, execute S200.

[0048] S200, receiving a point cloud frame, obtaining a coordinate set of the point cloud points to be registered in the frame, searching in the multi-resolution quadtree based on the coordinate set, and obtaining a bottom-level tree node that has an inclusion relationship with the coordinates in the coordinate set as a target bottom-level tree node.

[0049] Wherein, S200 specifically includes the following steps:

[0050] S201, receiving a point cloud frame, and obtaining a coordinate set of point cloud points to be registered in the frame;

[0051] S202, selecting the coordinates of a point cloud point that has not been retrieved in the coordinate set;

[0052] S203, determining whether the top node of the multi-resolution quadtree has a containment relationship with the coordinate, if not, proceeding to step S205, if yes, proceeding to the next layer;

[0053] S204, determine whether each node in this layer has a containment relationship with the coordinate. If there is no containment relationship, go to step S205. If there is a containment relationship, determine whether it is the bottom layer. If not, exclude the node without containment relationship and all its child nodes, and then go to the next layer of nodes with containment relationship and repeat the steps.

[0054] S204, if yes, obtain the code of the node that has a containment relationship with the coordinate;

[0055] S205, determining whether all coordinates in the coordinate set have been traversed, if not, returning to step S202, if yes, executing step S300.

[0056] It is worth mentioning that Figure 2As shown, the multi-resolution quadtree constructed in the present invention can quickly exclude nodes that have no containment relationship with the retrieval coordinates during the step-by-step traversal from top to bottom, thereby avoiding a large number of irrelevant child nodes from participating in the search and greatly improving the search efficiency. In addition, the multi-resolution quadtree of the present invention introduces a coding structure, which can quickly obtain the corresponding tree nodes through coding.

[0057] S300, determining whether the overlap between the frame and the target bottom tree node meets a threshold, if not, go to step S500, if yes, execute step S400.

[0058] Wherein, S300 specifically includes the following steps:

[0059] S301, determining the total number of point cloud points to be registered in the frame;

[0060] S302, query whether the coordinates of the point cloud point to be registered are included in the bottom tree node of the multi-resolution quadtree target. If included, mark the point to be registered as 1, and count the number of the points to be registered marked as 1.

[0061] S303, determine whether the ratio of the number of to-be-aligned points marked as 1 to the total number of to-be-aligned points is not lower than a set threshold, such as 30%. If so, it is determined that the threshold is met, a loop is detected, and then step S400 is executed. If not, go to step S500.

[0062] S400, using the NDT registration algorithm to align the frame with the multi-resolution quadtree, from the top layer to the bottom layer, the optimal rotation and translation parameters obtained from the previous layer of tree nodes are used as the initial values ​​of the next layer of tree nodes, and the optimal rotation and translation parameters of the next layer of tree nodes are calculated until the bottom layer of tree nodes or the registration fails.

[0063] Point cloud frame registration is the process of transforming the overlapping part of the point cloud into the same unified coordinate system by solving the transformation matrix between two point cloud frames with overlapping information or between a point cloud frame and an established target map. Among them, the registration algorithm based on NDT (Normal-Distributions Transform) assumes that the point cloud distribution in a certain voxel is a normal distribution, solves the optimal transformation, so that the sum of the probabilities of the point cloud to be registered in the normal distribution after transformation is maximized, and obtains the optimal solution for the pose, thereby converting the registration problem into a least squares problem. In this example, each node in the multi-resolution quadtree is used as a voxel in the NDT algorithm, and the mean and covariance of the point cloud in each node are calculated. For high-level nodes, their normal distribution is relatively dispersed, and for low-level nodes, their normal distribution is more concentrated. Through the iterative registration process from coarse to fine, correct registration can be achieved even in the case of large initial deviations.

[0064] Wherein, S400 specifically includes the following steps:

[0065] S401, each node in the multi-resolution quadtree is used as a voxel in the NDT algorithm,

[0066] Calculate the mean and covariance of the normal distribution of the point cloud within each node;

[0067] S402, taking the top node of the multi-resolution quadtree as a voxel, using the Gauss-Newton algorithm to solve the optimization problem in Formula 1, and obtaining the optimal estimated rotation and translation parameters (R, t) of the frame vector,

[0068]

[0069] Among them, R is the rotation matrix, t is the translation vector, and p j represents the point to be registered, such as the shape point of the lane line, n represents the number of points to be registered, u i and Σ i is the mean and covariance of the normal distribution within the voxel, m is the number of successfully matched voxels;

[0070] S403, the rotation and translation parameters (R, t) obtained from the previous layer of nodes are used as the initial values ​​of the Gauss-Newton iterative algorithm of the next layer of nodes, and the optimization problem of Formula 1 is continued to be solved with the next layer of nodes as voxels, and so on, until the bottom layer of tree nodes or the registration fails.

[0071] S500, adding the current point cloud frame into the sliding window, and performing a bundle adjustment on all the positions and postures in the sliding window to ensure the smoothness between the positions and postures in the sliding window.

[0072] S600, when the number of frames in the sliding window reaches a set number threshold, M (M is a positive integer, such as 20) frame vectors are moved out of the sliding window and added to the tree nodes of the multi-resolution quadtree.

[0073] It is worth mentioning that the traditional NDT algorithm has only one resolution, and the selection of resolution is very difficult. If it is too coarse, the registration effect will not reach the optimal one, and if it is too fine, it will easily fall into local details and cause registration failure. The present invention adopts an NDT registration algorithm based on a multi-resolution quadtree, which has multiple resolutions. Combined with the quadtree, iterative registration from the top layer to the bottom layer is performed, and the resolution is gradually improved. It can achieve gradual registration from coarse to fine, and has the advantages of both coarse registration and detailed registration. It will not fall into local details, and can achieve global optimization. It can overcome the influence of large initial deviation on registration and obtain good registration effect.

[0074] Embodiment 2:

[0075] A computer-readable storage medium stores a computer program, which, when executed by a processor, can perform the steps of the loop detection method of embodiment 1.

[0076] Embodiment 3:

[0077] A loop detection device includes: a memory and a processor; the memory is used to store software applications; the processor is used to execute the software applications, and each program of the software application correspondingly executes the steps in the loop detection method of embodiment 1.

[0078] Embodiment 4:

[0079] A vehicle comprises the loop detection device of the third embodiment.

[0080] The embodiment of the present invention utilizes the environment perception vector stored in the smart car to construct a multi-resolution quadtree, which can quickly detect loops. It is combined with a multi-resolution NDT registration algorithm to perform step-by-step iterative registration from coarse to fine. It is not easily affected by illumination or environmental texture, does not require additional computing resources, and can achieve accurate registration even in the case of large deviations, thereby obtaining a good loop detection effect.

[0081] It should be understood that the above is only an exemplary embodiment of the present invention, and the protection scope of the present invention is not limited thereto. Those skilled in the art can easily conceive of modifications, replacements or adaptive changes without departing from the principle and essence of the present invention, which should all be covered within the protection scope of the present invention.

Claims

1. A loop detection method, characterized in that: include: Generate a multi-resolution space partition tree based on the stored point cloud data, the multi-resolution space partition tree includes at least one tree node, the tree node stores the point cloud data, and the multi-resolution space partition tree is a multi-layer quadtree or octree; Receive a point cloud frame, obtain a coordinate set of a point cloud point to be registered in the frame, and obtain a target bottom-level tree node based on the coordinate set, wherein the target bottom-level tree node includes at least one coordinate in the coordinate set; Determine whether the overlap between the frame and the target bottom tree node meets a threshold; If the threshold is met, the frame is aligned with the multi-resolution space partitioning tree, and the optimal rotation and translation parameters obtained from the previous tree node are used as the initial values ​​of the next tree node from the top layer to the bottom layer, and the optimal rotation and translation parameters of the next tree node are calculated until the bottom tree node or the alignment fails.

2. The loop detection method according to claim 1, characterized in that: The point cloud data consists of a set of perception vector points.

3. The loop detection method according to claim 1, wherein: The generating of a multi-resolution space partitioning tree based on the stored point cloud data, wherein the multi-resolution space partitioning tree comprises at least one tree node, wherein the tree node stores the point cloud data, comprises: Based on the coordinates of each point cloud point in the point cloud data, determine one by one whether the coordinates of the point cloud point are included in the top-level tree node of the multi-resolution quadtree. If not, create a top-level tree node containing the point cloud point. If included, add the point cloud point to the top-level tree node, and then find out whether there is a child node containing the coordinates of the point cloud point in the next layer. If not, create a child node containing the point cloud point. If it already exists, add the point cloud point to the child node, and so on, until the bottom-level tree node.

4. The loop detection method according to claim 1, wherein: The acquiring a target bottom-level tree node based on the coordinate set, wherein the target bottom-level tree node contains at least one coordinate in the coordinate set, comprises: Based on the coordinates in the coordinate set, determine one by one whether the top-level tree nodes of the multi-resolution spatial partitioning tree have an inclusion relationship with the coordinates. If so, traverse the next layer of tree nodes to determine whether each node of this layer has an inclusion relationship with the coordinates, exclude nodes without an inclusion relationship, and further search for the next layer of tree nodes with an inclusion relationship, and so on, until the bottom layer of tree nodes, and obtain the bottom layer tree nodes that have an inclusion relationship with the coordinates.

5. The loop detection method according to claim 1, wherein: The determining whether the overlap between the frame and the target bottom tree node satisfies a threshold comprises: Determining the total number of point cloud points to be registered in the frame; Counting the number of points to be registered contained in the target bottom-level tree node; It is determined whether the ratio of the number of the to-be-registered points to the total number of the to-be-registered points is not less than a set threshold, and if so, it is determined that the threshold is met.

6. The loop detection method according to claim 1, wherein: The registering of the frame with the multi-resolution space partitioning tree, taking the optimal rotation and translation parameters obtained from the upper layer tree nodes as the initial values ​​of the next layer tree nodes from the top layer to the bottom layer, and calculating the optimal rotation and translation parameters of the next layer tree nodes until the bottom layer tree nodes or the registration fails, includes: Each tree node in the multi-resolution space partitioning tree is used as a voxel in the NDT algorithm, and the mean and covariance of the normal distribution of the point cloud in each node are calculated; Taking the top-level tree node of the multi-resolution spatial partitioning tree as a voxel, the Gauss-Newton algorithm is used to solve the optimization problem in the following formula to obtain the optimal estimated rotation and translation parameters (R, t) of the frame. Among them, R is the rotation matrix, t is the translation vector, and p j represents the point to be registered, n represents the number of points to be registered, u i and Σ i is the mean and covariance of the normal distribution within the voxel, and m is the number of successfully matched voxels; The optimal rotation and translation parameters (R, t) obtained from the previous tree node are used as the initial values ​​of the Gauss-Newton iterative algorithm of the next tree node, and the optimization problem is continued to be solved with the next tree node as the voxel, and so on, until the bottom tree node or the registration fails.

7. The loop detection method according to claim 1, wherein: Also includes: The sliding window is composed of historical point cloud frames; Add the current point cloud frame to the sliding window and adjust all the poses in the sliding window; When the number of frames in the sliding window reaches a set number threshold, a specified number of frames are moved out of the sliding window and added to the tree nodes of the multi-resolution space partitioning tree.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the loop detection method according to any one of claims 1 to 7 is performed.

9. A loop detection device, characterized in that: The device comprises a memory and a processor; The memory is used to store software applications; The processor is used to execute the software application, and each program of the software application correspondingly executes the loop detection method according to any one of claims 1 to 7.

10. A vehicle, characterized in that: The vehicle comprises the loop detection device according to claim 9.

Images