Lidar Mapping Optimization Method, Device, Electronic Device, and Storage Medium

Abstract

The present application discloses a method, apparatus, electronic device, and storage medium for optimizing lidar mapping. The method includes: obtaining point cloud data of a current frame and pose data of the current frame based on a lidar; performing loop detection based on the pose data of the current frame to obtain a loop frame corresponding to the current frame; obtaining the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of adjacent frames of the loop frame; constructing a loop constraint of a preset optimization model according to the point cloud data of the current frame, the pose data and point cloud data of the loop frame, and the pose data and point cloud data of the adjacent frames; determining optimized pose data according to the preset optimization model, so as to perform mapping by means of the optimized pose data. The present application constructs a loop constraint with richer information based on the point cloud data of the current frame, the pose data and point cloud data of the loop frame and its adjacent frames, thereby improving the convergence speed and optimization effect of the model.

Description

Technical Field

[0001] This application relates to the technical field of map construction, and in particular, to a method and device for optimizing lidar mapping, an electronic device, and a storage medium. Background Art

[0002] Currently, lidar mapping is mainly realized based on SLAM (Simultaneous Localization and Mapping). In the backend of lidar mapping, optimization methods such as g2o, GTSAM, or Ceres are generally used for optimization, and their principles are to add constraints of vertices and edges during the optimization process.

[0003] Currently, the constraint of the edge is generally to directly add the relative transformation pose obtained after matching two key frames into the edge constraint for subsequent optimization. However, for the case of a large closed loop, this method is very likely to cause a large cumulative error problem, and it is also not conducive to the convergence speed of the optimization iteration when the number of loop frames is small. Summary of the Invention

[0004] Embodiments of this application provide a method and device for optimizing lidar mapping, an electronic device, and a storage medium, so as to improve the convergence speed of the lidar mapping optimization model and improve the optimization effect.

[0005] Embodiments of this application adopt the following technical solutions:

[0006] In the first aspect, embodiments of this application provide a method for optimizing lidar mapping, where the method includes:

[0007] Obtain the point cloud data of the current frame and the pose data of the current frame based on the lidar;

[0008] Perform loop detection based on the pose data of the current frame to obtain the loop frame corresponding to the current frame;

[0009] Obtain the pose data of the loop frame, the point cloud data of the loop frame, the pose data of the adjacent frame corresponding to the loop frame, and the point cloud data of the adjacent frame;

[0010] Construct the loop constraint of a preset optimization model according to the point cloud data of the current frame, the pose data and the point cloud data of the loop frame, and the pose data and the point cloud data of the adjacent frame corresponding to the loop frame;

[0011] Determine the optimized pose data according to the preset optimization model, so as to perform mapping through the optimized pose data.

[0012] Optionally, performing loop detection based on the pose data of the current frame to obtain the loop frame corresponding to the current frame includes:

[0013] Obtaining the pose data of multiple historical frames;

[0014] Determining the distance between the current frame and each of the historical frames according to the pose data of the current frame and the pose data of the multiple historical frames;

[0015] Determining the loop frame among the multiple historical frames according to the distance between the current frame and each of the historical frames.

[0016] Optionally, the determining the loop frame among the multiple historical frames according to the distance between the current frame and each of the historical frames includes:

[0017] Comparing the distance between the current frame and each of the historical frames with a preset distance threshold;

[0018] If the distance between the current frame and the historical frame is less than the preset distance threshold, then taking the historical frame as the loop frame.

[0019] Optionally, there are multiple neighboring frames, and constructing the loop constraint of the preset optimization model according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, and the pose data of the neighboring frames corresponding to the loop frame and the point cloud data of the neighboring frames includes:

[0020] Constructing the loop constraint between the current frame and the loop frame, and the loop constraint between the current frame and each of the neighboring frames corresponding to the loop frame according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, and the pose data of the neighboring frames corresponding to the loop frame and the point cloud data of the neighboring frames;

[0021] Taking the loop constraint between the current frame and the loop frame, and the loop constraint between the current frame and each of the neighboring frames corresponding to the loop frame as the loop constraint of the preset optimization model.

[0022] Optionally, the loop constraint includes the first loop constraint between the current frame and the loop frame, the neighboring frame corresponding to the loop frame includes the next frame corresponding to the loop frame, and constructing the loop constraint of the preset optimization model according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, and the pose data of the neighboring frames corresponding to the loop frame and the point cloud data of the neighboring frames includes:

[0023] Match the point cloud data of the current frame with the point cloud data of the next frame corresponding to the loop closure frame to obtain the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame;

[0024] Construct a first loop closure constraint between the current frame and the loop closure frame according to the pose data of the loop closure frame, the pose data of the next frame corresponding to the loop closure frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame.

[0025] Optionally, the constructing a first loop closure constraint between the current frame and the loop closure frame according to the pose data of the loop closure frame, the pose data of the next frame corresponding to the loop closure frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame includes:

[0026] Invert the pose data of the loop closure frame to obtain the inverted pose data of the loop closure frame;

[0027] Multiply the inverted pose data of the loop closure frame, the pose data of the next frame corresponding to the loop closure frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame to obtain the first loop closure constraint between the current frame and the loop closure frame.

[0028] Optionally, the loop closure constraint further includes a second loop closure constraint between the current frame and the loop closure frame. After obtaining the pose data of the loop closure frame and the point cloud data of the loop closure frame, and the pose data of the neighboring frame corresponding to the loop closure frame and the point cloud data of the neighboring frame, the method further includes:

[0029] Match the point cloud data of the current frame with the point cloud data of the loop closure frame to obtain the relative transformation pose between the current frame and the loop closure frame;

[0030] Use the relative transformation pose between the current frame and the loop closure frame as the second loop closure constraint between the current frame and the loop closure frame.

[0031] In a second aspect, an embodiment of the present application further provides a lidar mapping optimization device, where the device includes:

[0032] A first acquisition unit, configured to acquire the point cloud data and the pose data of the current frame based on a lidar;

[0033] A loop closure detection unit, configured to perform loop closure detection based on the pose data of the current frame to obtain the loop closure frame corresponding to the current frame;

[0034] A second acquisition unit, configured to acquire the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the neighboring frame corresponding to the loop frame and the point cloud data of the neighboring frame;

[0035] A construction unit, configured to construct a loop constraint of a preset optimization model according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the neighboring frame corresponding to the loop frame and the point cloud data of the neighboring frame;

[0036] An optimization unit, configured to determine optimized pose data according to the preset optimization model, so as to perform mapping by using the optimized pose data.

[0037] In a third aspect, an embodiment of the present application further provides an electronic device, including:

[0038] A processor; and

[0039] A memory arranged to store computer-executable instructions, where the executable instructions, when executed, cause the processor to execute any one of the foregoing methods.

[0040] In a fourth aspect, an embodiment of the present application further provides a computer-readable storage medium, where the computer-readable storage medium stores one or more programs, and when the one or more programs are executed by an electronic device including a plurality of application programs, the electronic device is caused to execute any one of the foregoing methods.

[0041] At least one of the foregoing technical solutions adopted in the embodiments of the present application can achieve the following beneficial effects: In the lidar mapping optimization method of the embodiments of the present application, first, the point cloud data of the current frame and the pose data of the current frame are acquired based on the lidar; then, loop detection is performed based on the pose data of the current frame to obtain a loop frame corresponding to the current frame; then, the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the neighboring frame corresponding to the loop frame and the point cloud data of the neighboring frame are acquired; then, a loop constraint of a preset optimization model is constructed according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the neighboring frame corresponding to the loop frame and the point cloud data of the neighboring frame; finally, optimized pose data is determined according to the preset optimization model, so as to perform mapping by using the optimized pose data. The lidar mapping optimization method of the embodiments of the present application is based on the pose relationship between the current frame and the loop frame and the neighboring frame, and uses the point cloud data of the current frame, the pose data and the point cloud data of the loop frame and its neighboring frames to construct a loop constraint with richer information, and optimizes the pose data based on this, thereby improving the convergence speed of the optimization model and improving the optimization effect. Description of the Drawings

[0042] The accompanying drawings described herein are used to provide a further understanding of the present application and form a part of the present application. The schematic embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation of the present application. In the drawings:

[0043] Figure 1 is a schematic flow chart of a method for optimizing lidar mapping in an embodiment of the present application;

[0044] Figure 2 is a schematic structural diagram of a device for optimizing lidar mapping in an embodiment of the present application;

[0045] Figure 3 is a schematic structural diagram of an electronic device in an embodiment of the present application. Detailed embodiments

[0046] In order to make the objectives, technical solutions, and advantages of the present application clearer, the technical solutions of the present application will be clearly and completely described below in conjunction with the specific embodiments of the present application and the corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present application.

[0047] The following will describe in detail the technical solutions provided by each embodiment of the present application in conjunction with the drawings.

[0048] In a scenario of mapping based on visual SLAM, pose data is very important basic data. The estimation of pose is a recursive process, that is, the pose of the current frame is solved from the pose of the previous frame. Therefore, traditional pose constraints are all established with the previous frame. However, there is an error in each pose estimation. As the pose recursion progresses, the error also accumulates continuously, forming an accumulated error. This will lead to unreliable long-term estimation results, or it is impossible to construct a globally consistent trajectory and map.

[0049] To solve this problem, loop detection came into being. The key to loop detection is how to effectively detect that the lidar has passed through the same place. If a loop is detected, it will transmit the information to the backend for optimization processing. A loop is a more compact and accurate constraint than the backend. This constraint condition can form a topologically consistent trajectory map. If a closed loop can be detected and optimized, the result can be made more accurate.

[0050] Based on this, the embodiments of the present application provide a method for optimizing lidar mapping. As Figure 1 shown, a schematic flow chart of a method for optimizing lidar mapping in an embodiment of the present application is provided. The method at least includes the following steps S110 to S150:

[0051] Step S110: Obtain the point cloud data of the current frame and the pose data of the current frame based on the lidar.

[0052] The concepts of "current frame" and "loop closure frame" are involved in loop closure detection. In the embodiments of the present application, it is necessary to first determine the "current frame", so as to start loop closure detection at the position of the current frame, and then determine the "loop closure frame" corresponding to the "current frame".

[0053] The principle of determining the "current frame" is not to close the loop with frames that are too close. If the key frames are selected too close, the similarity between the two key frames will be too high, and the detected loop closure will not be very meaningful. Therefore, the frames used for loop closure detection are preferably sparse, not very similar to each other, and can cover the entire environment. For example, if the distance between two adjacent key frames is currently 2m, then loop closure detection can be started at the position 100 frames later, that is, about 200m. There is a high probability of detecting a loop closure at this position. Of course, specifically how to determine the "current frame" can be flexibly determined according to actual needs and is not specifically limited here.

[0054] After determining the current frame, the point cloud data of the current frame and the pose data of the current frame can be further obtained. The point cloud data of the current frame is the data directly collected based on the lidar, and the pose data of the current frame is obtained by solving the pose based on the previous frame and accumulates the pose estimation errors of the previous frames.

[0055] Step S120: Perform loop closure detection based on the pose data of the current frame to obtain the loop closure frame corresponding to the current frame.

[0056] After determining the current frame, loop closure detection can be performed based on the pose data of the current frame to obtain the loop closure frame corresponding to the current frame. Here, there may be one or more loop closure frames. The specific loop closure detection method can be flexibly set according to actual needs, such as using methods combining feature points and bag of words, such as ORB-SLAM, VINS-Mono, etc.

[0057] Step S130: Obtain the pose data and point cloud data of the loop closure frame, as well as the pose data and point cloud data of the adjacent frames corresponding to the loop closure frame.

[0058] After detecting the loop closure frame, it is also necessary to further obtain the pose data and point cloud data of the loop closure frame. Since the loop closure frame is also a key frame in the entire loop closure trajectory, the corresponding pose data and point cloud data can also be obtained based on the lidar.

[0059] Step S140: Construct a loop closure constraint for a preset optimization model based on the point cloud data of the current frame, the pose data of the loop closure frame and the point cloud data of the loop closure frame, as well as the pose data of the neighboring frames corresponding to the loop closure frame and the point cloud data of the neighboring frames.

[0060] The construction of traditional loop closure constraints is to directly match the point cloud data of two frames to obtain the transformation pose relationship between the frames. However, when the direct matching result has a large error, it will lead to a decrease in the convergence speed of the optimization model and the optimization effect is not good.

[0061] Based on this, in the embodiment of the present application, when constructing loop closure constraints, not only the point cloud data of the current frame, the point cloud data of the loop closure frame and the point cloud data of the neighboring frames are needed, but also the pose data of the loop closure frame and the pose data of the neighboring frames are needed. Based on the pose relationship between the current frame and the loop closure frame and the neighboring frames, another form of loop closure constraint with richer information is constructed, and the constructed loop closure constraint is added as an edge constraint to the preset optimization model for optimization processing, so as to be able to make up for the problem of large matching errors in the traditional method to a certain extent, and then improve the convergence speed of the optimization model. Improve the optimization effect.

[0062] Step S150: Determine the optimized pose data according to the preset optimization model, so as to perform mapping through the optimized pose data.

[0063] The preset optimization model here can be, for example, any one of g2o, GTSAM or Ceres. By optimizing and solving the preset optimization model, the optimized pose data of each key frame in the entire loop closure trajectory can be obtained, providing more accurate data support for subsequent map construction.

[0064] Taking g2o as an example, before performing optimization and solution, the information that needs to be provided to g2o includes vertex constraints, edge constraints and basic parameter settings, etc. Among them, the vertex constraints refer to the constraints of the pose of each key frame, and the edge constraints include the loop closure constraints obtained in the previous steps. The basic parameter settings can include the setting of the number of iterations, etc.

[0065] The lidar mapping optimization method of the embodiment of the present application is based on the pose relationship between the current frame and the loop closure frame and the neighboring frames, uses the point cloud data of the current frame, the pose data and the point cloud data of the loop closure frame and its neighboring frames to construct a loop closure constraint with richer information, and optimizes the pose data based on this, thereby improving the convergence speed of the optimization model and improving the optimization effect.

[0066] In an embodiment of the present application, the loop closure detection based on the pose data of the current frame to obtain the loop closure frame corresponding to the current frame includes: obtaining the pose data of multiple historical frames; determining the distances between the current frame and each of the historical frames according to the pose data of the current frame and the pose data of the multiple historical frames; and determining the loop closure frame from the multiple historical frames according to the distances between the current frame and each of the historical frames.

[0067] In the embodiment of the present application, when determining the loop closure frame, the pose data of multiple historical frames before the current frame can be obtained first. As mentioned above, the current frame will not form a loop with frames that are too close. Therefore, when initially selecting historical frames, historical frames that are separated from the current frame by more than a certain frame number threshold can be used as the selected historical frames, that is, not all historical frames before the current frame need to be used as the selected historical frames to match the current frame.

[0068] After obtaining the pose data of the above-mentioned multiple historical frames, since the pose data will specifically include the position coordinates of the frame, the position coordinates of the current frame are calculated with the position coordinates of each historical frame, so that historical frames that are relatively close to the current frame can be obtained as the loop closure frame.

[0069] In an embodiment of the present application, the determining the loop closure frame from the multiple historical frames according to the distances between the current frame and each of the historical frames includes: comparing the distances between the current frame and each of the historical frames with a preset distance threshold; if the distance between the current frame and the historical frame is less than the preset distance threshold, then taking the historical frame as the loop closure frame.

[0070] As in the foregoing embodiment, after obtaining the distances between the current frame and each historical frame, the obtained multiple distances can be respectively compared with a preset distance threshold set in advance. If the distance is less than the distance threshold, it means that the distance between the historical frame and the current frame is close enough, and it can be considered that the historical frame is the loop closure frame corresponding to the current frame; otherwise, it is not a loop closure frame.

[0071] The above-mentioned preset distance threshold can be flexibly set according to the actual scenario and actual requirements. For example, it can be set to 5m. When the distance between the historical frame and the current frame is less than 5m, it is considered a loop closure frame. Therefore, in this case, more than one loop closure frame may be determined. To reduce the influence of distance error, subsequent loop closure constraints can be constructed for each loop closure frame.

[0072] In an embodiment of the present application, there are multiple adjacent frames. The loop closure constraint for constructing a preset optimization model based on the point cloud data of the current frame, the pose data of the loop closure frame, the point cloud data of the loop closure frame, as well as the pose data of the adjacent frames corresponding to the loop closure frame and the point cloud data of the adjacent frames includes: constructing a loop closure constraint between the current frame and the loop closure frame, and a loop closure constraint between the current frame and each of the adjacent frames corresponding to the loop closure frame, based on the point cloud data of the current frame, the pose data of the loop closure frame, the point cloud data of the loop closure frame, as well as the pose data of the adjacent frames corresponding to the loop closure frame and the point cloud data of the adjacent frames; and using the loop closure constraint between the current frame and the loop closure frame, and the loop closure constraint between the current frame and each of the adjacent frames corresponding to the loop closure frame, as the loop closure constraint of the preset optimization model.

[0073] Based on the loop closure frame determined in the foregoing embodiment, a loop closure constraint between the current frame and the loop closure frame can be constructed using the point cloud data of the current frame, the pose data of the loop closure frame, and the point cloud data of the loop closure frame, as well as the pose data of the adjacent frames corresponding to the loop closure frame and the point cloud data of the adjacent frames. Since the data of several adjacent frames corresponding to the loop closure frame is also relatively similar to the loop closure frame, a loop closure constraint between the current frame and the adjacent frames corresponding to the loop closure frame can be further constructed.

[0074] The number of adjacent frames can be flexibly set according to actual needs. For example, for the current frame m and the loop closure frame n, the two adjacent frames before and after it can be used as adjacent frames, including adjacent frame n - 1, adjacent frame n - 2, adjacent frame n + 1, and adjacent frame n + 2. Then, the loop closure constraints between m and n, n - 1, n - 2, n + 1, and n + 2 are calculated respectively.

[0075] In an embodiment of the present application, the loop closure constraint includes a first loop closure constraint between the current frame and the loop closure frame. The adjacent frame corresponding to the loop closure frame includes the next frame corresponding to the loop closure frame. The loop closure constraint for constructing a preset optimization model based on the point cloud data of the current frame, the pose data of the loop closure frame, the point cloud data of the loop closure frame, as well as the pose data of the adjacent frame corresponding to the loop closure frame and the point cloud data of the adjacent frame includes: matching the point cloud data of the current frame with the point cloud data of the next frame corresponding to the loop closure frame to obtain the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame; and constructing the first loop closure constraint between the current frame and the loop closure frame based on the pose data of the loop closure frame, the pose data of the next frame corresponding to the loop closure frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame.

[0076] The embodiment of this application takes the loop constraint between the current frame and the loop frame as an example to further illustrate the specific construction process of the loop constraint. For the current frame m and the loop frame n, when constructing the loop constraint between m and n, it is equivalent to calculating the transformation pose T between m and n m,n , and the core of the embodiment of this application lies in how to reconstruct the transformation pose T between m and n m,n .

[0077] Specifically, first match the point cloud data of the current frame m with the point cloud data of the next frame n + 1 corresponding to the loop frame, so as to obtain the transformation pose T between m and n + 1 m,n+1 , and then further combine the pose data T of the loop frame n n , the pose data T of the next frame n + 1 corresponding to the loop frame n+1 to recalculate the transformation pose T between m and n m,n , and use this as the first loop constraint between m and n

[0078] It can be seen that the first loop constraint between the reconstructed current frame and the loop frame contains both T n , T n+1 and T m,n+1 . Therefore, adding this constraint to the optimization model can make up for the matching error caused by the direct matching method between the original m and n. In addition, when the cumulative pose estimation error of T n , T n+1 is relatively large, it can also be compensated by the first loop constraint constructed above, thereby improving the convergence speed and optimization effect of the model

[0079] In an embodiment of this application, constructing the first loop constraint between the current frame and the loop frame according to the pose data of the loop frame, the pose data of the next frame corresponding to the loop frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop frame includes: inverting the pose data of the loop frame to obtain the inverted pose data of the loop frame; multiplying the inverted pose data of the loop frame, the pose data of the next frame corresponding to the loop frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop frame to obtain the first loop constraint between the current frame and the loop frame

[0080] The first loop constraint between the current frame m and the loop frame n constructed in the embodiment of this application can be specifically expressed in the following form

[0081] T m,n = T n .inverse() * T n+1 * T m,n+1 , (1)

[0082] Among them, inverse() represents the inverse of T n Taking the inverse, the above formula reconstructs the first loop constraint between m and n by successive multiplication. The error existing in each step part of the formula can be compensated by continuous optimization after adding the optimization model.

[0083] Similarly, when calculating the loop constraints between the current frame m and the adjacent frames n - 1, n - 2, n + 1, and n + 2 respectively, the above logic can also be adopted. For example:

[0084] T m,n-1 = T n-1 .inverse() * T n * T m,n , (2)

[0085] T m,n-2 = T n-2 .inverse() * T n-1 * T m,n-1 , (3)

[0086] T m,n+1 = T n+1 .inverse() * T n * T m,n , (4)

[0087] T m,n+2 = T n+2 .inverse() * T n+1 * T m,n+1 , (5)

[0088] In an embodiment of the present application, the loop constraint further includes a second loop constraint between the current frame and the loop frame. After obtaining the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the adjacent frame corresponding to the loop frame and the point cloud data of the adjacent frame, the method further includes: matching the point cloud data of the current frame with the point cloud data of the loop frame to obtain the relative transformation pose between the current frame and the loop frame; using the relative transformation pose between the current frame and the loop frame as the second loop constraint between the current frame and the loop frame.

[0089] In the embodiment of the present application, a second loop constraint between the current frame m and the loop frame n is further constructed. The second loop constraint can be regarded as obtained by directly matching the point cloud data of the current frame and the loop frame. Specifically, based on the acquisition result of the lidar, the point cloud data of the current frame m and the point cloud data of the loop frame n are obtained, and then matching algorithms such as NDT (Normal Distribution Transformation) or ICP (Iterative Closest Point) can be used to calculate the relative transformation pose T between the current frame m and the loop frame n m,n , which is used as the second loop constraint.

[0090] Similarly, the second loop constraint between the current frame m and each adjacent frame n-1, n-2, n+1, and n+2 can be further calculated, that is, the point cloud data of the current frame m is respectively matched with the point cloud data of each adjacent frame n-1, n-2, n+1, and n+2, and the transformation poses T m,n-1 , T m,n-2 , T m,n+1 and T m,n+2 between the current frame m and each adjacent frame n-1, n-2, n+1, and n+2 are respectively obtained.

[0091] Based on the above embodiments, the present application constructs two forms of loop constraints between the current frame and the loop frame and between the current frame and each adjacent frame respectively. One form is the transformation pose calculated based on the above formulas (1)-(5), that is, the first loop constraint, and the other form is the transformation pose directly obtained by matching the point cloud data between two frames, that is, the second loop constraint. Thereby, the convergence speed of the model can be greatly improved, and the optimization effect can be improved.

[0092] The embodiment of the present application also provides a lidar mapping optimization device 200, as Figure 2 shown, which provides a structural schematic diagram of a lidar mapping optimization device in the embodiment of the present application. The device 200 includes: a first acquisition unit 210, a loop detection unit 220, a second acquisition unit 230, a construction unit 240, and an optimization unit 250, wherein:

[0093] The first acquisition unit 210 is configured to acquire the point cloud data and the pose data of the current frame based on the lidar;

[0094] The loop detection unit 220 is configured to perform loop detection based on the pose data of the current frame to obtain the loop frame corresponding to the current frame;

[0095] A second acquisition unit 230, configured to acquire the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of an adjacent frame corresponding to the loop frame and the point cloud data of the adjacent frame;

[0096] A construction unit 240, configured to construct a loop constraint of a preset optimization model according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of an adjacent frame corresponding to the loop frame and the point cloud data of the adjacent frame;

[0097] An optimization unit 250, configured to determine optimized pose data according to the preset optimization model, so as to perform mapping by using the optimized pose data.

[0098] In an embodiment of the present application, the loop detection unit 220 is specifically configured to: acquire the pose data of multiple historical frames; determine the distance between the current frame and each of the historical frames according to the pose data of the current frame and the pose data of the multiple historical frames; and determine the loop frame from the multiple historical frames according to the distance between the current frame and each of the historical frames.

[0099] In an embodiment of the present application, the loop detection unit 220 is specifically configured to: compare the distance between the current frame and each of the historical frames with a preset distance threshold; and if the distance between the current frame and the historical frame is less than the preset distance threshold, use the historical frame as the loop frame.

[0100] In an embodiment of the present application, there are multiple adjacent frames, and the construction unit 240 is specifically configured to: construct a loop constraint between the current frame and the loop frame, and a loop constraint between the current frame and each adjacent frame corresponding to the loop frame according to the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of an adjacent frame corresponding to the loop frame and the point cloud data of the adjacent frame; and use the loop constraint between the current frame and the loop frame, and the loop constraint between the current frame and each adjacent frame corresponding to the loop frame as the loop constraint of the preset optimization model.

[0101] In one embodiment of the present application, the loop closure constraint includes a first loop closure constraint between the current frame and the loop closure frame. The neighboring frame corresponding to the loop closure frame includes the next frame corresponding to the loop closure frame. The construction unit 240 is specifically configured to: match the point cloud data of the current frame with the point cloud data of the next frame corresponding to the loop closure frame to obtain the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame; construct the first loop closure constraint between the current frame and the loop closure frame according to the pose data of the loop closure frame, the pose data of the next frame corresponding to the loop closure frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame.

[0102] In one embodiment of the present application, the construction unit 240 is specifically configured to: invert the pose data of the loop closure frame to obtain the inverted pose data of the loop closure frame; multiply the inverted pose data of the loop closure frame, the pose data of the next frame corresponding to the loop closure frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop closure frame to obtain the first loop closure constraint between the current frame and the loop closure frame.

[0103] In one embodiment of the present application, the loop closure constraint further includes a second loop closure constraint between the current frame and the loop closure frame. The device further includes: a matching unit, configured to match the point cloud data of the current frame with the point cloud data of the loop closure frame to obtain the relative transformation pose between the current frame and the loop closure frame; use the relative transformation pose between the current frame and the loop closure frame as the second loop closure constraint between the current frame and the loop closure frame.

[0104] It can be understood that the above lidar mapping optimization device can implement each step of the lidar mapping optimization method provided in the foregoing embodiment. The relevant explanations regarding the lidar mapping optimization method are applicable to the lidar mapping optimization device and will not be elaborated here.

[0105] Figure 3 is a schematic structural diagram of an electronic device according to an embodiment of the present application. Please refer to Figure 3 , at the hardware level, the electronic device includes a processor, and optionally also includes an internal bus, a network interface, and a memory. Among them, the memory may include a memory, such as a high-speed random access memory (Random-Access Memory, RAM), and may also include a non-volatile memory, such as at least one disk memory, etc. Of course, the electronic device may also include other hardware required for other services.

[0106] The processor, network interface, and memory can be interconnected through an internal bus, which can be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, an EISA (Extended Industry Standard Architecture) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 3 only a bidirectional arrow is used in

[0107] Memory, used to store programs. Specifically, the program can include program code, and the program code includes computer operation instructions. The memory can include a memory and a non-volatile memory, and provide instructions and data to the processor.

[0108] The processor reads the corresponding computer program from the non-volatile memory into the memory and then runs it, forming a lidar mapping optimization device at the logical level. The processor executes the program stored in the memory and is specifically used to perform the following operations:

[0109] Obtain the point cloud data and pose data of the current frame based on the lidar;

[0110] Perform loop detection based on the pose data of the current frame to obtain the loop frame corresponding to the current frame;

[0111] Obtain the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of the adjacent frame corresponding to the loop frame;

[0112] Construct a loop constraint of a preset optimization model according to the point cloud data of the current frame, the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of the adjacent frame corresponding to the loop frame;

[0113] Determine the optimized pose data according to the preset optimization model to perform mapping through the optimized pose data.

[0114] The above is as in this application Figure 1The method executed by the lidar mapping optimization device disclosed in the illustrated embodiment can be applied to or implemented by a processor. The processor may be an integrated circuit chip with signal processing capabilities. During implementation, the steps of the above method can be completed through the integrated logic circuit of the hardware in the processor or instructions in the form of software. The above processor may be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it may also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. It can implement or execute the various methods, steps, and logic block diagrams disclosed in the embodiments of the present application. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. The steps of the method disclosed in combination with the embodiments of the present application can be directly embodied as being executed by a hardware decoding processor or executed by a combination of hardware and software modules in the decoding processor. The software module may be located in a mature storage medium in the art such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory, and the processor reads the information in the memory and combines its hardware to complete the steps of the above method.

[0115] The electronic device can also execute Figure 1 the method executed by the lidar mapping optimization device in Figure 1 the illustrated embodiment and implement the functions of the lidar mapping optimization device in

[0116] Embodiments of the present application also propose a computer-readable storage medium that stores one or more programs. The one or more programs include instructions that, when executed by an electronic device including multiple application programs, can enable the electronic device to execute Figure 1 the method executed by the lidar mapping optimization device in the illustrated embodiment and specifically used to execute:

[0117] Obtain the point cloud data of the current frame and the pose data of the current frame based on the lidar;

[0118] Perform loop detection based on the pose data of the current frame to obtain the loop frame corresponding to the current frame;

[0119] Obtain the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the neighboring frames corresponding to the loop frame and the point cloud data of the neighboring frames;

[0120] Construct loop constraints for a preset optimization model based on the point cloud data of the current frame, the pose data of the loop frame and the point cloud data of the loop frame, as well as the pose data of the neighboring frames corresponding to the loop frame and the point cloud data of the neighboring frames;

[0121] Determine the optimized pose data according to the preset optimization model, so as to perform mapping through the optimized pose data.

[0122] Those skilled in the art should understand that the embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memory, CD-ROM, optical memory, etc.) containing computer-usable program code.

[0123] The present invention is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, and the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for realizing the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 or multiple blocks.

[0124] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including instruction means, and the instruction means realizes the functions specified in one process Figure 1 one process or multiple processes and / or blocks Figure 1 or multiple blocks.

[0125] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide means for realizing the functions specified in one process Figure 1Steps of the function specified in one or more processes and / or blocks Figure 1 Steps of the function specified in one or more blocks

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

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

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

[0129] It should also be noted that the term "comprising", "including" or any other variant thereof is intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that includes a series of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the statement "comprising a..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus that includes the element.

[0130] Those skilled in the art should understand that the embodiments of the present application may be provided as a method, system, or computer program product. Therefore, the present application may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0131] The above are only embodiments of the present application and are not intended to limit the present application. For those skilled in the art, various changes and modifications can be made to the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included within the scope of the claims of the present application.

Claims

1. A method for optimizing lidar mapping, wherein, The method includes: Obtaining point cloud data of the current frame and pose data of the current frame based on a lidar; Performing loop detection based on the pose data of the current frame to obtain a loop frame corresponding to the current frame; Obtaining the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of an adjacent frame corresponding to the loop frame; Constructing loop constraints of a preset optimization model according to the point cloud data of the current frame, the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of the adjacent frame corresponding to the loop frame; Determining optimized pose data according to the preset optimization model, so as to perform mapping through the optimized pose data; The loop constraints include a first loop constraint between the current frame and the loop frame, the adjacent frame corresponding to the loop frame includes the next frame corresponding to the loop frame, and constructing the loop constraints of the preset optimization model according to the point cloud data of the current frame, the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of the adjacent frame corresponding to the loop frame includes: Matching the point cloud data of the current frame with the point cloud data of the next frame corresponding to the loop frame to obtain a relative transformation pose between the current frame and the next frame corresponding to the loop frame; Constructing the first loop constraint between the current frame and the loop frame according to the pose data of the loop frame, the pose data of the next frame corresponding to the loop frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop frame.

2. The method according to claim 1, wherein The performing loop detection based on the pose data of the current frame to obtain a loop frame corresponding to the current frame includes: Obtaining the pose data of multiple historical frames; Determining the distances between the current frame and each of the historical frames according to the pose data of the current frame and the pose data of the multiple historical frames; Determining the loop frame from the multiple historical frames according to the distances between the current frame and each of the historical frames.

3. The method according to claim 2, wherein, The determining the loop frame from the multiple historical frames according to the distances between the current frame and each of the historical frames includes: Comparing the distances between the current frame and each of the historical frames with a preset distance threshold; If the distance between the current frame and the historical frame is less than the preset distance threshold, then taking the historical frame as the loop frame.

4. The method according to claim 1, wherein, There are multiple adjacent frames, and constructing the loop constraints of the preset optimization model according to the point cloud data of the current frame, the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of the adjacent frames corresponding to the loop frame includes: Constructing loop constraints between the current frame and the loop frame, and loop constraints between the current frame and each of the adjacent frames corresponding to the loop frame according to the point cloud data of the current frame, the pose data and point cloud data of the loop frame, as well as the pose data and point cloud data of the adjacent frames corresponding to the loop frame; Take the loop constraint between the current frame and the loop frame, and the loop constraints between the current frame and each adjacent frame corresponding to the loop frame as the loop constraints of the preset optimization model.

5. The method according to claim 1, wherein The constructing the first loop constraint between the current frame and the loop frame according to the pose data of the loop frame, the pose data of the next frame corresponding to the loop frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop frame includes: Invert the pose data of the loop frame to obtain the inverted pose data of the loop frame; Multiply the inverted pose data of the loop frame, the pose data of the next frame corresponding to the loop frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop frame to obtain the first loop constraint between the current frame and the loop frame.

6. The method according to claim 1, wherein The loop constraint further includes a second loop constraint between the current frame and the loop frame. After obtaining the pose data of the loop frame and the point cloud data of the loop frame, and the pose data of the adjacent frame corresponding to the loop frame and the point cloud data of the adjacent frame, the method further includes: Match the point cloud data of the current frame with the point cloud data of the loop frame to obtain the relative transformation pose between the current frame and the loop frame; Take the relative transformation pose between the current frame and the loop frame as the second loop constraint between the current frame and the loop frame.

7. A lidar mapping optimization device, wherein, The device includes: A first acquisition unit for acquiring the point cloud data and the pose data of the current frame based on a lidar; A loop detection unit for performing loop detection based on the pose data of the current frame to obtain the loop frame corresponding to the current frame; A second acquisition unit for acquiring the pose data and the point cloud data of the loop frame, and the pose data and the point cloud data of the adjacent frame corresponding to the loop frame; A construction unit for constructing the loop constraints of the preset optimization model according to the point cloud data of the current frame, the pose data and the point cloud data of the loop frame, and the pose data and the point cloud data of the adjacent frame corresponding to the loop frame; An optimization unit for determining the optimized pose data according to the preset optimization model to perform mapping with the optimized pose data; The loop constraint includes a first loop constraint between the current frame and the loop frame, and the adjacent frame corresponding to the loop frame includes the next frame corresponding to the loop frame. The construction unit is specifically configured to: Match the point cloud data of the current frame with the point cloud data of the next frame corresponding to the loop frame to obtain the relative transformation pose between the current frame and the next frame corresponding to the loop frame; Construct the first loop constraint between the current frame and the loop frame according to the pose data of the loop frame, the pose data of the next frame corresponding to the loop frame, and the relative transformation pose between the current frame and the next frame corresponding to the loop frame.

8. An electronic device, including: A processor; And A memory arranged to store computer-executable instructions which, when executed, cause the processor to perform the method according to any one of claims 1 to 6.

9. A computer-readable storage medium storing one or more programs which, when executed by an electronic device including a plurality of application programs, cause the electronic device to perform the method according to any one of claims 1 to 6.

Images