Loop detection method, point cloud map construction method, electronic device and storage medium

Abstract

The present invention relates to a loop detection method, a point cloud map construction method, an electronic device and a storage medium. The method constructs a main object descriptor with a main object as the center, and only requires that the main object positions of two frames are close and one or more main objects in the same scene can be observed. Compared with the method of establishing a Scan Context descriptor with a laser radar as the center in the prior art, the descriptor provided by the present invention that does not rely on the observation position can solve the problem that the Scan Context descriptor cannot perform loop detection when the positions of the two frames do not overlap, expands the scope of application of loop detection, and improves the robustness of loop detection. Further, since the relative posture between the current key frame and the historical key frame finally calculated includes the offset and rotation angle in the x and y directions, when the result obtained by the loop detection is sent to the back end for global target map optimization, a better posture optimization result can be obtained, which can improve the robustness of the entire SLAM system and the accuracy of positioning and mapping.

Description

Technical Field

[0001] The present invention relates to the technical field of map construction, and in particular to a loop detection method, a point cloud map construction method, an electronic device and a storage medium. Background Art

[0002] SLAM (Simultaneous Localization And Mapping) technology is widely used in autonomous driving, drones, robots, VR (Virtual Reality) and AR (Augmented Reality). SLAM refers to the process in which a device moves from an unknown location in an unknown environment, locates itself according to the data collected by sensors and the constructed environment map, and constructs an incremental map based on its own positioning.

[0003] The currently popular 3D SLAM framework is mainly based on the graph optimization method, which includes three modules: front-end, back-end and loop detection. The front-end collects sensor data, processes the data and outputs odometer information; the back-end mainly constructs a graph structure with vehicle posture as nodes and relative posture as edges for global posture optimization.

[0004] Loop detection is a very important part of the SLAM system. In the SLAM system, sensor data noise and the algorithm itself will cause certain errors in the vehicle posture estimated by the front end. The error will accumulate over time. When the vehicle completes a circular movement, the path calculated by the SLAM algorithm will not be completely closed due to the accumulated error. At this time, the loop detection module of the SLAM system is required to identify the loop and calculate the accurate relative posture, and send this relative posture to the back end to optimize the vehicle posture.

[0005] Scan Context is one of the most popular loop detection technologies. Scan Context literally means "scanning context" in English. Just like when we read, we need to understand the context to understand its meaning. When SLAM performs loop detection, it also needs to compare the "context" (previous data) to know whether we have walked to the same place as before (loop). This technology uses lidar as the center to establish a global descriptor called Scan Context, and performs loop detection through rotation matching. Compared with the direct matching method, it has higher efficiency, accuracy and recall rate, but there are still some defects.

[0006] Related technologies propose a scene recognition technology based on 3D point cloud, which projects 3D point cloud to XY plane, establishes Scan Context as the descriptor of key frame with LiDAR as the center, and establishes multi-dimensional tree structure with ScanContext ring feature vector as node for subsequent fast search. When the device returns to the coordinate position of a certain moment in history, multiple candidate frames are quickly searched in the multi-dimensional tree structure according to the ring feature vector of the current Scan Context, and the Scan Context is matched by rotation. If the highest matching similarity is greater than the preset value, it is considered that the loop is detected, and finally the matching posture information is sent to the backend. The method of establishing Scan Context with LiDAR as the center used by this technology requires that the current frame and the historical frame coincide in position to detect the loop, which will cause many loops to be missed; this technology can only calculate the rotation angle of the device between the two frames by rotating Scan Context matching, and cannot calculate the offset of the device in the x and y directions, which has certain limitations in actual use.

[0007] Related technologies also propose a laser SLAM loop detection system and method based on graph descriptors. This method uses semantic information extracted from point cloud data to form two graph descriptors, full graph descriptors and vertex descriptors, to represent point cloud frames and semantic objects. Compared with traditional algorithms that extract descriptors at the pixel level, the present invention can avoid the cumbersome task of normal vector calculation and the problems caused by large changes in viewing angles, and can detect loops more quickly and robustly. Although this technology uses semantic information to construct two descriptors and improves the robustness of scene recognition, it cannot accurately calculate the relative pose between the current frame and the loop frame.

[0008] Related technologies also propose a method for creating point cloud maps and identifying scenes based on static semantic information, including step 1: jointly projecting laser point cloud and semantic image to obtain semantic point cloud; step 2: filtering robust static information through semantic labels; step 3: extracting descriptors of robust static information and combining them into descriptors of the current scene; step 4: combining semantic point cloud with laser radar odometer to generate a global map, and relocating on the global map through point cloud registration. This technology extracts robust static information to establish descriptors, and the descriptors are directly matched to obtain the pose of the current frame. However, the descriptor content is strongly dependent on the position of the laser radar, which can easily cause mismatching of the descriptor, and the direct matching method will consume a lot of time, which has certain limitations in actual use.

[0009] In summary, although the Scan Context method and various improved methods based on Scan Context can quickly retrieve loop candidate frames and calculate the similarity between two frames, they require the coordinates of the two frames to basically coincide, and cannot identify scenes where the positions of two frames differ by a certain distance; secondly, such methods can only calculate the rotation angle of the current frame relative to the matching frame, and cannot calculate the offset of the X and Y coordinates between the two frames, which has certain limitations in actual use. Summary of the invention

[0010] In view of this, the purpose of the present invention is to provide a loop detection method, a point cloud map construction method, an electronic device and a storage medium to solve the problem that the loop detection method in the prior art requires that the coordinates of the two frames basically coincide and cannot identify scenes with a certain distance difference in position.

[0011] According to a first aspect of an embodiment of the present invention, a loop detection method is provided, including:

[0012] Obtaining a key frame constructed by a front end, and a main object determined from the key frame;

[0013] For any main object, construct a main object descriptor with the main object as the center;

[0014] Match the main object descriptor of the current key frame with the main object descriptors of the historical key frames in pairs;

[0015] According to the matching results, determine whether the current key frame and the historical key frame are in the same scene. If so, determine whether there is a loop edge between the current key frame and the historical key frame, and add the relative posture between the current key frame and the historical key frame as a constraint edge to the posture graph constructed by the backend, so that the backend can perform global posture optimization on the posture graph according to the constraint edge.

[0016] Preferably, the front end constructs a key frame by fusing and filtering the 3D point cloud in the current frame and the 3D point clouds in the previous four frames according to the matching results of the front end, and defines the pose of the current frame as the pose of the key frame, and defines the point cloud in the key frame as a submap; and / or,

[0017] The front end determines the main object from the key frame by the following method, including:

[0018] For any sub-map, obtain the semantic label of each point in the sub-map;

[0019] According to the semantic label, the main object point cloud is filtered out;

[0020] Cluster the filtered main object point cloud to obtain all the main objects of the sub-map;

[0021] For any main object, the X coordinate and Y coordinate of the main object are calculated according to the coordinate values ​​of all points in the main object.

[0022] Preferably, the constructing of the main object descriptor with the main object as the center includes:

[0023] For any main object, a polar coordinate system is established with the main object as the center and the line connecting the laser radar to the main object as the polar axis. The polar axis is divided into N r Divide the polar angle into N s Divide equally to divide the environment around the main object into N r ×N s grids, project the points in the submap downward to the grids, and calculate the average height of all points in each grid; where N r ≥2, N s ≥2;

[0024] The average height values ​​corresponding to each grid are combined to form an N r ×N s The matrix of N r ×N s The matrix is ​​determined as the main object descriptor of the main object.

[0025] Preferably, the method further comprises:

[0026] Deleting points in the grid whose height is lower than a height threshold, where the height threshold is determined based on an average height of ground points; and / or ignoring points that are not projected into the grid.

[0027] Preferably, the method further comprises:

[0028] The column vector obtained by averaging the main object descriptor by row is defined as the ring feature vector; the row vector obtained by averaging the main object descriptor by column is defined as the fan feature vector;

[0029] For any sub-map, save the main object position list, main object description sub-list, and fan feature vector list into the database according to the key frame sequence number;

[0030] The main object position list stores the X coordinate and Y coordinate of each main object in the sub-map;

[0031] The main object descriptor list stores the main object descriptor of each main object in the sub-map;

[0032] The feature vector list stores the fan feature vectors of each main object descriptor of the sub-map.

[0033] Preferably, the matching of the main object descriptors of the current key frame with the main object descriptors of the historical key frames in pairs includes:

[0034] Construct a multi-dimensional tree structure based on the ring feature vectors of historical key frames;

[0035] Create a candidate keyframe set;

[0036] Traverse the ring feature vectors of all main object descriptors in the current key frame, search for a preset number of similar nodes in the multidimensional tree structure for each ring feature vector, and add the key frame numbers corresponding to the similar nodes to the candidate key frame set;

[0037] The historical key frames corresponding to the key frame serial numbers in the candidate key frame set are taken as candidate key frames, and the main object descriptors of the current key frame and the main object descriptors of the candidate key frames are matched in pairs.

[0038] Preferably, the matching of the main object descriptors of the current key frame and the main object descriptors of the candidate key frames in pairs includes:

[0039] Get the fan feature vector list and main object descriptor list of the candidate keyframes;

[0040] Calculating a coarse offset required for aligning a main object descriptor of a current key frame with a main object descriptor of a candidate key frame according to the fan feature vectors in the fan feature vector list;

[0041] Constructing an optimization equation in an offset window centered on the coarse offset to calculate a fine offset required for aligning a main object descriptor of the current key frame with a main object descriptor of a candidate key frame;

[0042] The similarity between the main object descriptor of the current key frame and the main object descriptor of the candidate key frame is calculated according to the precise offset, and the similarity is stored in the matching result.

[0043] Preferably, the matching of the main object descriptors of the current key frame with the main object descriptors of the candidate key frames in pairs further includes:

[0044] Get the list of main object positions of candidate keyframes;

[0045] According to the X coordinate and Y coordinate of the main object in the current key frame in the submap, and the X coordinate and Y coordinate of the main object in the candidate key frame in the submap, calculate the relative pose between the main object of the current key frame and the main object of the candidate key frame, and store the relative pose and the similarity in the matching result in correspondence;

[0046] The relative position and posture at least includes: an offset in the X direction, an offset in the Y direction, and a rotation angle of the main object of the current key frame relative to the main object of the candidate key frame.

[0047] Preferably, judging whether the current key frame and the historical key frame are in the same scene according to the matching result includes:

[0048] Eliminate the similarities and corresponding relative postures in the matching results that are lower than the similarity threshold;

[0049] In the remaining matching results, for each main object descriptor of the current keyframe, find the main object descriptor with the highest similarity in the candidate keyframes, and filter out the corresponding similarities;

[0050] The relative poses corresponding to the filtered similarities are regarded as a set of three-dimensional points, mapped into three-dimensional space, and clustered to obtain multiple classes;

[0051] If there is at least one class with a number of points greater than the threshold, the current keyframe and the historical keyframe are determined to be in the same scene, and the average pose of all points in the class with the largest number of points is taken as the relative pose between the current keyframe and the historical keyframe;

[0052] If there is no class whose number of points is greater than the quantity threshold, it is determined that the current key frame and the historical key frame are not in the same scene, and there is no loop edge between the current key frame and the historical key frame.

[0053] Preferably, the method further comprises:

[0054] If it is determined that there is no loop edge between the current key frame and the historical key frame, all loop feature vectors of the current key frame are added to the multi-dimensional tree structure.

[0055] According to a second aspect of an embodiment of the present invention, a point cloud map construction method is provided, comprising at least:

[0056] Radar odometry based on main object features;

[0057] The loop detection method described above.

[0058] Preferably, the radar odometer based on main object features comprises:

[0059] Determine the main object from each frame of 3D point cloud;

[0060] After the main object is determined, the main object features are extracted, and the main object features at least include: the centroid coordinates of each main object, and the direction vector of each main object;

[0061] Each time a frame of 3D point cloud is obtained, the main object is matched with the previous frame of 3D point cloud, and the overall matching error is controlled to be minimum, and the relative position of each frame relative to the previous frame is calculated; the overall matching error is related to all the main object features in each frame;

[0062] The calculated relative poses are multiplied in sequence to obtain the pose of each frame in the world coordinate system.

[0063] According to a third aspect of an embodiment of the present invention, there is provided an electronic device, including:

[0064] A communication module, a processor and a memory, wherein the memory stores program instructions;

[0065] The processor is used to execute program instructions stored in the memory, execute the above-mentioned loop detection method, and / or the above-mentioned point cloud map construction method.

[0066] According to a fourth aspect of an embodiment of the present invention, there is provided a computer-readable storage medium having a rewritable computer program stored thereon;

[0067] When the computer program runs on a computer device, the computer device executes the above-mentioned loop closure detection method and / or the above-mentioned point cloud map construction method.

[0068] The technical solution provided by the embodiments of the present invention may have the following beneficial effects:

[0069] The technical solution provided by the present invention constructs a main object descriptor with the main object as the center. It only requires that the main object positions of the two frames are close and one or more main objects in the same scene can be observed. Compared with the method of establishing a Scan Context descriptor with the lidar as the center in the prior art, the descriptor provided by the present invention that is independent of the observation position can solve the problem that the Scan Context descriptor cannot perform loop detection when the positions of the two frames do not overlap, thereby expanding the scope of application of loop detection and improving the robustness of loop detection.

[0070] Furthermore, the technical solution provided by the present invention avoids the influence of mismatching by screening the matching results of the main object descriptors of the current key frame and the main object descriptors of the historical key frames. The relative pose between the current key frame and the historical key frame finally calculated includes the offset and rotation angle in the x and y directions. Compared with the prior art that cannot calculate the offset of the X and Y coordinates between the two frames, the technical solution provided by the present invention can obtain better pose optimization results when the results obtained by loop detection are sent to the back end for global target map optimization, which can improve the robustness of the entire SLAM system and the accuracy of positioning and mapping.

[0071] It is to be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0073] Figure 1 is a flow chart of a loop detection method according to an exemplary embodiment;

[0074] Figure 2 is a signal flow diagram of a point cloud map construction method according to an exemplary embodiment;

[0075] Figure 3 is a schematic diagram showing the construction of a main object descriptor according to an exemplary embodiment;

[0076] Figure 4 is a schematic diagram showing relative poses between a main object of a current key frame and a main object of a candidate key frame according to an exemplary embodiment;

[0077] Figure 5 It is a schematic diagram showing the result of pairwise matching between the main object descriptor of the current key frame and the main object descriptors of the candidate key frames according to an exemplary embodiment. DETAILED DESCRIPTION

[0078] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present invention. Instead, they are merely examples of devices and methods consistent with some aspects of the present invention as detailed in the appended claims.

[0079] It should be noted that the technical solution provided by the present invention requires the use of sensors on the vehicle to achieve simultaneous positioning and mapping, and use loop detection to optimize the posture.

[0080] “Vehicles” include but are not limited to: autonomous vehicles, drones, unmanned logistics vehicles, mobile robots, etc.; “Sensors” include but are not limited to: lidar, structured light cameras, millimeter wave radars and other sensors that can provide 3D point cloud information of the environmental space.

[0081] The data of the sensor needs to undergo an external parameter conversion of the sensor relative to the center of the vehicle to convert the sensor data to the center of the vehicle. The technical solution provided by the present invention assumes that the sensor data has undergone the external parameter conversion.

[0082] Some concepts mentioned in various embodiments of the present invention are summarized as follows:

[0083] Frame: Sensor data is read in the form of frames. Each frame is a complete packet of sensor data at a certain moment. The frame sending and receiving frequency is determined by the system used by the device.

[0084] Main object: It is the basis for realizing scene recognition, and is an object with certain specific semantics in the scene.

[0085] Main object descriptor: A form of representation of the category and distribution of the surrounding point cloud centered on the main object.

[0086] Coordinate system: The present invention uses a rectangular coordinate system and a polar coordinate system. The rectangular coordinate system uses the universal Cartesian rectangular coordinate system right-hand system (forward is the X axis, upward is the Z axis, and left is the Y axis); the polar coordinate system defines the positive direction of the polar angle as counterclockwise rotation from the polar axis.

[0087] Posture: It is the position and attitude of the vehicle in 2D space. The position is the coordinates in the x-direction and y-direction, and the attitude is the azimuth angle θ with the x-axis as the positive direction.

[0088] Observation position: A rectangular coordinate system is established with the center of the sensor as the origin, the positive direction of the vehicle as the X-axis, and the left as the Y-axis. The position of the main object in the coordinate system is called the observation position.

[0089] The specific implementation methods of each embodiment of the present invention are explained as follows:

[0090] Embodiment 1

[0091] Figure 1 is a flow chart of a loop detection method according to an exemplary embodiment. Figure 1 As shown, the method includes:

[0092] Step S11, obtaining a key frame constructed by the front end, and a main object determined from the key frame;

[0093] Step S12: for any main object, construct a main object descriptor with the main object as the center;

[0094] Step S13, matching the main object descriptors of the current key frame with the main object descriptors of the historical key frames in pairs;

[0095] Step S14: Based on the matching results, determine whether the current key frame and the historical key frame are in the same scene. If so, determine that there is a loop edge between the current key frame and the historical key frame, and add the relative pose between the current key frame and the historical key frame as a constraint edge to the pose graph constructed by the back end, so that the back end performs global pose optimization on the pose graph according to the constraint edge.

[0096] It should be noted that the technical solution provided in this embodiment is applicable to the field of SLAM technology and to the construction of point cloud maps. Figure 2 , the construction of point cloud map includes:

[0097] The front-end odometer completes real-time posture tracking based on the data frames output by the front-end sensors (including but not limited to: lidar, camera, IMU, GNSS, wheel speed encoder and other sensors), processes the input data frames, calculates the posture changes, and obtains the motion relationship between sensors. Based on the motion relationship between sensors, the map is initially constructed. This step is often the accumulation of sensor information, but as time accumulates, errors will accumulate.

[0098] Loop detection uses the similarity between data frames to determine whether the previous position has been reached, thereby eliminating cumulative errors and obtaining a globally consistent trajectory and map.

[0099] Back-end optimization and mapping: Based on the results of loop closure detection, the pose graph established by the front-end odometer is optimized, and a global target map (i.e., point cloud map) is established based on the optimized pose graph.

[0100] It is understandable that the technical solution provided in this embodiment improves the loop detection part in the process of constructing the point cloud map. Based on the loop detection method provided in this embodiment, combined with the existing front-end odometer and back-end optimization method, the construction of the point cloud map can be realized.

[0101] In specific practice, the "front end" in step S11 constructs a key frame by fusing and filtering the 3D point cloud in the current frame and the 3D point clouds in the previous four frames according to the matching results of the front end; and defines the pose of the current frame as the pose of the key frame, and defines the point cloud in the key frame as a submap;

[0102] The front end determines the main object from the key frame by the following method, including:

[0103] For any sub-map, obtain the semantic label of each point in the sub-map;

[0104] According to the semantic label, the main object point cloud is filtered out;

[0105] Cluster the filtered main object point cloud to obtain all the main objects of the sub-map;

[0106] For any main object, the X coordinate and Y coordinate of the main object are calculated according to the coordinate values ​​of all points in the main object.

[0107] Specifically, each time a sub-map is established, the trained point cloud semantic segmentation network RangeNet++ is used (other 3D point cloud semantic segmentation networks can also be used, including but not limited to: PointRCNN, PointNet++, etc.), or the semantic label of each point in the 3D point cloud is obtained by image semantic segmentation and then projected to the lidar coordinate system, or the image is collected by the camera, the image is processed by the image semantic segmentation network to obtain the semantic image, and then the semantic image is projected onto the 3D point cloud through external parameter conversion, and finally the 3D point cloud semantics is obtained.

[0108] After the main object point cloud is screened out, Euclidean clustering is performed (other point cloud clustering algorithms such as K-Means clustering algorithm and DBSCAN clustering algorithm can also be used, and a point cloud clustering method based on deep learning can also be used) to obtain multiple main objects, and the X coordinate and Y coordinate of each main object are calculated.

[0109] Preferably, for any main object, the coordinate values ​​of all points in the main object are averaged to obtain the X coordinate and Y coordinate of the main object.

[0110] In actual practice, the semantics of the main object in each keyframe is not necessarily one or several specific semantics. Different keyframes can determine different main objects according to the specific distribution of the point cloud.

[0111] Generally speaking, the selection of the main object should follow the following principles:

[0112] 1. The main object cannot be a moving object, such as a moving car or pedestrian, and the main object cannot be a common movable object, such as a roadblock or a stationary car;

[0113] 2. The main object should have multiple candidate targets in the key frame, and the number of 3D points that make up each candidate target should exceed a certain value;

[0114] 3. There should be no other objects in the direction of the laser radar to the main object to avoid selecting alternative targets that are easily blocked.

[0115] In actual practice, the main objects are usually street lamps, tree trunks, and other common objects that will not be moved.

[0116] In specific practice, the step S12 of "constructing a main object descriptor with the main object as the center" includes:

[0117] See also Figure 3 , for any main object ( Figure 3 The five-pointed star in the figure represents the main object), a polar coordinate system is established with the main object as the center and the line connecting the laser radar to the main object as the polar axis, and the polar axis is divided into N r Divide the polar angle into Ns Divide equally to divide the environment around the main object into N r ×N s grids, project the points in the submap downward to the corresponding grids, and calculate the average height of all points in each grid (ignore the points not projected to the grid); where N r ≥2, N s ≥2;

[0118] The average height values ​​corresponding to each grid are combined to form an N r ×N s The matrix of N r ×N s The matrix is ​​determined as the main object descriptor of the main object.

[0119] It is understandable that since the height information of the ground points is very similar, it may cause mismatching, so the ground points are not considered when establishing the main object descriptor, and the filtering method is to use a height threshold for filtering. Therefore, points in the grid whose height is lower than the height threshold can be deleted, and the height threshold is determined according to the average height of the ground points.

[0120] In specific practice, the method further includes:

[0121] The column vector obtained by averaging the main object descriptor by row is defined as the ring feature vector; the row vector obtained by averaging the main object descriptor by column is defined as the fan feature vector;

[0122] For any sub-map, save the main object position list, main object description sub-list, and fan feature vector list into the database according to the key frame sequence number;

[0123] The main object position list stores the X coordinate and Y coordinate of each main object in the sub-map;

[0124] The main object descriptor list stores the main object descriptor of each main object in the sub-map;

[0125] The feature vector list stores the fan feature vectors of each main object descriptor of the sub-map.

[0126] In specific practice, the step S13 of "matching the main object descriptors of the current key frame with the main object descriptors of the historical key frames in pairs" includes:

[0127] Construct a multi-dimensional tree structure based on the ring feature vectors of historical key frames;

[0128] Create a candidate keyframe set;

[0129] Traverse the ring feature vectors of all main object descriptors in the current key frame, search for a preset number of similar nodes in the multidimensional tree structure for each ring feature vector, and add the key frame numbers corresponding to the similar nodes to the candidate key frame set;

[0130] The historical key frames corresponding to the key frame serial numbers in the candidate key frame set are taken as candidate key frames, and the main object descriptors of the current key frame and the main object descriptors of the candidate key frames are matched in pairs.

[0131] It is understandable that since a sub-map may contain multiple main object descriptors, in order to improve the efficiency of the search and the accuracy of the search results, a candidate keyframe set can be created, and the ring feature vectors of all main object descriptors in the current keyframe can be traversed. For each ring feature vector, a preset number of nodes similar to it are searched in the multidimensional tree structure, and the keyframe numbers corresponding to the similar nodes are added to the candidate keyframe set. After the traversal is completed, all historical keyframes similar to the current keyframe are obtained. The candidate keyframe set is a non-repetitive set to prevent repeated operations on the same keyframe.

[0132] The preset number is set according to user needs or historical experience values ​​or experimental data, for example, set to 10.

[0133] In specific practice, the matching of the main object descriptor of the current key frame with the main object descriptor of the candidate key frame in pairs includes:

[0134] Get the fan feature vector list and main object descriptor list of the candidate keyframes;

[0135] Calculating a coarse offset shift required for aligning a main object descriptor of a current key frame with a main object descriptor of a candidate key frame according to the fan feature vectors in the fan feature vector list;

[0136] For any main object descriptor of the current key frame, traverse the main object descriptors of the candidate key frames and calculate the coarse offset shift required for alignment between the two (it can be stipulated that the left is positive). The calculation formula is as follows:

[0137]

[0138] In the formula, S q represents the fan feature vector of the candidate primary object descriptor, It indicates that the fan feature vector of the current main object descriptor is shifted to the left by shift units, and ||S|| indicates the binary norm of vector S.

[0139] The following optimization equation is constructed in the offset window centered on the coarse offset to calculate the fine offset n required for aligning the main object descriptor of the current key frame with the main object descriptor of the candidate key frame:

[0140]

[0141]

[0142] Wherein, shift is the coarse offset, [shift-k, shift+k] represents the offset window with shift as the center and k as the radius. In this embodiment, k=3, I q represents the candidate primary object descriptor, Indicates that the current main object descriptor is offset to the left by n units, and d represents the distance between the two main object seedlings. The calculation formula is as follows:

[0143]

[0144] In the formula, and They represent the j-th Sector from left to right after the main object descriptor is aligned, and ||c|| represents the binary norm of vector c.

[0145] Then, the similarity calculation formula is as follows. The similarity value range is 0 to 1. The closer to 1, the more similar. The calculation formula is as follows:

[0146]

[0147] The calculated similarity is stored in the matching result.

[0148] In specific practice, the matching of the main object descriptor of the current key frame with the main object descriptor of the candidate key frame in pairs also includes:

[0149] Get the list of main object positions of candidate keyframes;

[0150] According to the X coordinate and Y coordinate (x1, y1) of the main object in the sub-map in the current key frame, and the X coordinate and Y coordinate (x2, y2) of the main object in the sub-map in the candidate key frame.

[0151] It should be noted that the X coordinate and Y coordinate of the main object in the sub-map are the coordinates of the main object in the keyframe.

[0152] See also Figure 4 , the relative pose between the main object of the current key frame and the main object of the candidate key frame can be derived:

[0153] Δx=x1-x2cosΔθ o +y2sinΔθo (6)

[0154] Δy=y1-x2sinΔθ o +y2cosΔθ o (7)

[0155]

[0156] Where γ is the angle corresponding to the main object descriptor in the current key frame shifted to the left by n units, and n is the precise offset calculated in the above steps: Δx is the offset of the main object of the current key frame relative to the main object of the candidate key frame in the X direction; Δy is the offset of the main object of the current key frame relative to the main object of the candidate key frame in the Y direction; Δθ0 is the rotation angle of the main object of the current key frame relative to the main object of the candidate key frame, and counterclockwise is positive; where

[0157]

[0158] The calculated relative pose and the similarity calculated in the above step are stored in the matching result.

[0159] It can be understood that, assuming that the current key frame has M main object descriptors and the subsequent key frames have N main object descriptors, then M*N groups of matches generate a total of M*N similarities and relative poses; among them, M≥1, N≥1.

[0160] In specific practice, the step S14 of “determining whether the current key frame and the historical key frame are in the same scene according to the matching result” includes:

[0161] Eliminate the similarities and corresponding relative postures in the matching results that are lower than the similarity threshold;

[0162] In the remaining matching results, for each main object descriptor of the current key frame, find the main object descriptor with the highest similarity in the candidate key frames, and filter out the corresponding similarities.

[0163] Specifically, first select the match corresponding to the maximum similarity, assuming that the serial numbers of the main object descriptors of this match are p and q respectively, and then delete all matches containing the main object descriptors p and q in the matching results; then select the match corresponding to the maximum similarity in the remaining matching results, and repeat this process until for each main object descriptor of the current key frame, the main object descriptor with the highest similarity is found in the candidate key frames.

[0164] The maximum similarity value and the corresponding relative posture selected each time are the final matching results.

[0165] See also Figure 5As shown in the figure, assuming that there are 8 main object descriptors in the current key frame and 7 main object descriptors in the candidate key frame, the pairwise matching generates 56 matching similarities. According to the above screening strategy, first select the maximum value 0.864, then delete the 4th row and 8th column in the table (after deletion, the remaining matching results still form a matrix); then select the maximum value 0.862 in the remaining matching results, then delete the 3rd row and 1st column in the table (after deletion, the remaining matching results still form a matrix), and so on. The final similarities screened out are: 0.864, 0.862, 0.861, 0.821, 0756, 0.724, 0.684.

[0166] The relative poses (Δx, Δy, Δθ) corresponding to the selected similarities are o ) is regarded as a set of three-dimensional points, mapped into three-dimensional space, and clustered to obtain multiple classes (for example, Euclidean clustering is performed to obtain multiple classes {C i |i=1,2,3…});

[0167] If there is at least one class containing more points than the threshold (assuming that the kth class C k The class with the largest number of points contains the largest number of points. If the number of points is greater than the number threshold 2), the current keyframe and the historical keyframe are determined to be in the same scene, and the average pose of all points in the class with the largest number of points is taken as the relative pose between the current keyframe and the historical keyframe (calculation of C k The average pose of is taken as the final relative pose);

[0168] If there is no class whose number of points is greater than the quantity threshold, it is determined that the current key frame and the historical key frame are not in the same scene, and there is no loop edge between the current key frame and the historical key frame.

[0169] Furthermore, if it is determined that there is no loop edge between the current key frame and the historical key frames, all loop feature vectors of the current key frame are added to the multi-dimensional tree structure.

[0170] It is understandable that if it is determined that there is no loop edge between the current key frame and the historical key frame, all loop feature vectors of the current key frame are added to the multidimensional tree structure, which can ensure that the candidate key frame can be searched in the multidimensional tree structure when returning to the current scene next time. If a loop edge is detected and the loop feature vector of the current key frame is inserted into the multidimensional tree structure, there will be redundancy of loop edges when returning to the current scene next time, which is unnecessary.

[0171] It can be understood that the technical solution provided by this embodiment constructs a main object descriptor with the main object as the center. It only requires that the main object positions of the two frames are close and one or more main objects in the same scene can be observed. Compared with the method of establishing a Scan Context descriptor with a lidar as the center in the prior art, the descriptor provided by this embodiment that is independent of the observation position can solve the problem that the Scan Context descriptor cannot perform loop detection when the positions of the two frames do not overlap, thereby expanding the scope of application of loop detection and improving the robustness of loop detection.

[0172] It should be noted that the technical solution provided in this embodiment is independent of the observation position, which means that the descriptor is established with the main object as the center, and the content of the descriptor is roughly the same when the laser radar is at different positions around it (it only needs to be rotated to match), while the laser radar position of the Scan Context method in the prior art is different, so its descriptor is completely different.

[0173] Furthermore, the technical solution provided by this embodiment avoids the influence of mismatching by screening the matching results of the main object descriptor of the current key frame and the main object descriptor of the historical key frame. The relative pose between the current key frame and the historical key frame finally calculated includes the offset and rotation angle in the x and y directions. Compared with the prior art that cannot calculate the offset of the X and Y coordinates between the two frames, the technical solution provided by this embodiment can obtain better pose optimization results when the results obtained by loop detection are sent to the back end for global target map optimization, which can improve the robustness of the entire SLAM system and the accuracy of positioning and mapping.

[0174] In practice, the backend uses the pose of each keyframe as a pose node and the relative poses between keyframes as constraint edges to construct a pose graph. Whenever the pose data from the frontend is sent to the backend, the backend optimizes the pose of each node by minimizing the overall error of the pose graph and updates the keyframe with the optimized pose.

[0175] When the loop detection detects the loop edge, a constraint edge is added between the current key frame and the corresponding historical key frame node (that is, the relative posture calculated in this embodiment), and the posture graph forms a loop. After optimization, the posture of the historical key frame will change significantly, and the posture optimization process can be clearly seen in the loop scene.

[0176] Specifically, the backend performs global posture optimization based on the loop detection results provided in this embodiment, including:

[0177] The vertices represent the optimization variables and the constraint edges represent the error terms. The global pose of the vehicle is the vertex and the constraint edge is the relative pose estimate between two pose nodes. This estimate can come from the feature point method or the direct method, or from GPS or IMU integration. Regardless of the method, assume that the pose node T is estimated. i and pose node T j A movement ΔT between ij This movement can be expressed in several ways, choose the one that is more natural:

[0178]

[0179] It can also be expressed as:

[0180]

[0181] According to the idea of ​​graph optimization, the above formula will not be accurate in practice, so the least squares error is set up, and then the derivative of the error with respect to the optimization variable is discussed. ij Moving to the right side of the equation, construct the error e ij :

[0182]

[0183] There are two optimization variables in the above formula: i and j , so find e ij Regarding the derivatives of these two variables, according to the derivation method of Lie algebra, we can give ξ i and j Each with a left perturbation: δξ i and δξ j . The error then becomes:

[0184]

[0185] According to the adjoint property exp((Ad(T)ξ)^)=Texp(T^)T -1 We can get:

[0186] exp(T^)T=T exp((Ad(T)ξ)^) (14)

[0187] According to the above formula, the Jacobian matrix in right-hand multiplication form can be derived (left-hand multiplication is formed when moving to the left):

[0188]

[0189] So far, the Jacobian matrix of the error with respect to the two poses has been calculated.

[0190] About T i The Jacobian matrix of :

[0191]

[0192] About T j The Jacobian matrix of :

[0193]

[0194] After Jacobian derivation, the remaining part is a simple least squares problem. The optimization variables are the postures of each vertex, and the edges come from the posture observation constraints.

[0195] Let ε be the set of all edges, then the overall objective function is

[0196] This problem is solved using the Gauss-Newton method or the Levenberg-Marquardt method (LM method). In actual engineering, g2o or Ceres is used for solution.

[0197] Specifically, after the back-end global pose is optimized, the global target map must be constructed and updated (i.e., the point cloud map must be constructed and updated), including:

[0198] All sub-maps in keyframes are transformed three-dimensionally according to the poses in the keyframes, and the transformed results are fused together and filtered to obtain a global sparse point cloud map. The sparsity of the map can be changed by adjusting the filtering parameters. Whenever a new node is inserted into the pose graph, the pose graph is optimized, and the poses in all keyframes are updated, and the global target map is updated at the same time.

[0199] Embodiment 2

[0200] According to an exemplary embodiment, a method for constructing a point cloud map is shown, the method at least comprising:

[0201] Radar odometry based on main object features;

[0202] The loop detection method described in Example 1.

[0203] It can be understood that, from the description of the first embodiment, the construction of the point cloud map includes: front-end odometer, loop detection, back-end optimization and mapping. Since the implementation of loop detection, back-end optimization and mapping has been described in detail in the first embodiment, this embodiment will not be repeated. Now, taking the front-end odometer as a radar odometer as an example, the implementation of the radar odometer based on the main object feature is explained as follows:

[0204] 1. Data collection and preprocessing

[0205] Collect data from sensors such as lidar, camera, IMU, GNSS, and wheel speed encoder. Because the clock source has clock drift, and the clock drift of each sensor is different, it is necessary to align the timestamps of each sensor at the initial moment, use linear interpolation to synchronize time, and use the timestamp of the lidar as the standard to calculate the remaining sensor data at this moment.

[0206] Point cloud motion distortion processing:

[0207] The reason for motion distortion is that the points in a frame of point cloud are not collected at the same time. During the collection process, the LiDAR moves with the vehicle, but the LiDAR measures the distance between the observed object and the LiDAR, so the coordinate systems of different laser points are different. The point cloud motion distortion phenomenon will increase with the increase of vehicle speed and the decrease of LiDAR frequency. The point cloud motion distortion phenomenon will seriously affect the results of point cloud matching and produce inaccurate odometer information.

[0208] In order to eliminate point cloud motion distortion, the speed and angular velocity of the vehicle in the previous frame are used to predict the speed and angular velocity of the vehicle in the current frame, or sensors such as IMU are directly used to obtain the vehicle motion state in the current frame. Combined with the scanning method of the laser radar, the position of each point in the laser point cloud of the current frame at the same moment as the first point is calculated. This process is called point cloud motion distortion processing. Point cloud motion distortion processing can significantly improve the accuracy of mapping.

[0209] 2. 3D point cloud semantic segmentation and extraction of main object features

[0210] How to determine the main object from the key frame has been introduced in the first embodiment. The method of determining the main object from each frame is the same as determining the main object from the key frame, and will not be repeated in this embodiment.

[0211] After the main object is determined, the main object features are extracted including:

[0212] The centroid coordinates of each main object are calculated, and principal component analysis is performed on each main object, and the eigenvector corresponding to the largest eigenvalue is taken as the direction vector of the main object.

[0213] 3. Radar odometer based on main object features

[0214] Each frame of 3D point cloud is matched with the previous frame of 3D point cloud. The matching method is a matching method based on the main object, including:

[0215] For each main object in the current frame, search for the main object closest to it within a certain spatial range in the previous frame (if no main object with the same semantics can be found within a certain range, the error term corresponding to this main object is ignored) and the two main objects are matched. The matching error is:

[0216] error=e(c1,c2)+e(n1,n2) (19)

[0217] Where e(c1, c2) is the matching error of the center of mass coordinates of the main object, c1 and c2 are the center of mass coordinates of the main object, e(n1, n2) is the matching error of the direction vector of the main object, n1 and n2 are the direction vectors of the main object.

[0218]

[0219]

[0220] In the formula, R and T three-dimensional rotation transformation matrix and three-dimensional translation transformation matrix, combined together to represent the three-dimensional transformation of the point. Construct the following optimization equation:

[0221]

[0222] Where N represents the number of main objects in the point cloud of the current frame. The overall matching error is the sum of the matching errors of each main object. The matching error of each main object includes the main object centroid coordinate matching error and the main object direction vector matching error. Point cloud matching is essentially an optimization problem. After constructing the above optimization equation, the solution method can be selected from the Gauss-Newton method (Gauss-Newton method) or the Levenberg-Marquardt method (LM method) to iteratively calculate the accurate matching pose.

[0223] Calculate the relative pose of each frame relative to the previous frame, and multiply them in sequence to get the pose of each frame in the world coordinate system, which is the odometer information.

[0224] It can be understood that the technical solution provided by this embodiment includes the loop detection method described in Example 1. This loop detection method constructs a main object descriptor with the main object as the center. It only requires that the main object positions of the two frames are close and one or more main objects in the same scene can be observed. Compared with the method of establishing a ScanContext descriptor with a lidar as the center in the prior art, the descriptor provided by this embodiment that is independent of the observation position can solve the problem that the ScanContext descriptor cannot perform loop detection when the positions of the two frames do not overlap, thereby expanding the scope of application of loop detection and improving the robustness of loop detection.

[0225] Embodiment 3

[0226] According to an exemplary embodiment, an electronic device includes:

[0227] A communication module, a processor and a memory, wherein the memory stores program instructions;

[0228] The processor is used to execute program instructions stored in the memory to execute the loop detection method described in Example 1 and / or the point cloud map construction method described in Example 2.

[0229] It should be noted that the electronic equipment includes but is not limited to: vehicle-mounted computers and external computer equipment. Communication modules include but are not limited to: wired communication modules and wireless communication modules, such as: WCDMA, GSM, CDMA and / or LTE communication modules, ZigBee modules, Bluetooth modules, Wi-Fi modules, etc.

[0230] Processors include but are not limited to: CPU, single chip microcomputer, PLC controller, FPGA controller, etc.

[0231] The memory may include computer system readable media in the form of volatile memory, such as random access memory (RAM) and / or cache memory; it may also include other removable / non-removable, volatile / non-volatile computer system storage media. The memory may include at least one program product having a set (e.g., at least one) of program modules that are configured to perform the functions of various embodiments of the present invention.

[0232] It can be understood that the technical solution provided by this embodiment includes the loop detection method described in Example 1. This loop detection method constructs a main object descriptor with the main object as the center. It only requires that the main object positions of the two frames are close and one or more main objects in the same scene can be observed. Compared with the method of establishing a ScanContext descriptor with a lidar as the center in the prior art, the descriptor provided by this embodiment that is independent of the observation position can solve the problem that the ScanContext descriptor cannot perform loop detection when the positions of the two frames do not overlap, thereby expanding the scope of application of loop detection and improving the robustness of loop detection.

[0233] Embodiment 4

[0234] According to an exemplary embodiment, a computer-readable storage medium is shown, on which a rewritable computer program is stored;

[0235] When the computer program runs on a computer device, the computer device executes the loop detection method described in Example 1 and / or the point cloud map construction method described in Example 2.

[0236] The computer-readable storage medium disclosed in this embodiment includes, but is not limited to: electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or components, or any combination of the above. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present 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, device or device.

[0237] It can be understood that the technical solution provided by this embodiment includes the loop detection method described in Example 1. This loop detection method constructs a main object descriptor with the main object as the center. It only requires that the main object positions of the two frames are close and one or more main objects in the same scene can be observed. Compared with the method of establishing a ScanContext descriptor with a lidar as the center in the prior art, the descriptor provided by this embodiment that is independent of the observation position can solve the problem that the ScanContext descriptor cannot perform loop detection when the positions of the two frames do not overlap, thereby expanding the scope of application of loop detection and improving the robustness of loop detection.

[0238] It can be understood that the same or similar parts of the above embodiments can be referenced to each other, and the contents not described in detail in some embodiments can refer to the same or similar contents in other embodiments.

[0239] It should be noted that, in the description of the present invention, the terms "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or implying relative importance. In addition, in the description of the present invention, unless otherwise specified, the meaning of "plurality" refers to at least two.

[0240] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code that includes one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may not be performed in the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention belong.

[0241] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, a plurality of steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0242] A person skilled in the art may understand that all or part of the steps in the method for implementing the above-mentioned embodiment may be completed by instructing related hardware through a program, and the program may be stored in a computer-readable storage medium, which, when executed, includes one or a combination of the steps of the method embodiment.

[0243] In addition, each functional unit in each embodiment of the present invention may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0244] The storage medium mentioned above can be a read-only memory, a magnetic disk or an optical disk, etc.

[0245] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0246] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A loop detection method, characterized in that: include: Obtaining a key frame constructed by a front end, and a main object determined from the key frame; For any main object, a polar coordinate system is established with the main object as the center and the line connecting the laser radar to the main object as the polar axis to construct a main object descriptor; Match the main object descriptor of the current key frame with the main object descriptors of the historical key frames in pairs; According to the matching results, determine whether the current key frame and the historical key frame are in the same scene. If so, determine whether there is a loop edge between the current key frame and the historical key frame, and add the relative posture between the current key frame and the historical key frame as a constraint edge to the posture graph constructed by the backend, so that the backend performs global posture optimization on the posture graph according to the constraint edge; The front end constructs a key frame by fusing and filtering the 3D point cloud in the current frame and the 3D point clouds in the previous four frames according to the matching results of the front end, and defines the pose of the current frame as the pose of the key frame, and defines the point cloud in the key frame as a submap; The front end determines the main object from the key frame by the following method, including: For any sub-map, obtain the semantic label of each point in the sub-map; According to the semantic label, the main object point cloud is filtered out; Cluster the filtered main object point cloud to obtain all the main objects of the sub-map; For any main object, calculate the X and Y coordinates of the main object according to the coordinate values ​​of all points in the main object; Among them, judging whether the current key frame and the historical key frame are in the same scene according to the matching results includes: eliminating the similarities and corresponding relative poses below the similarity threshold in the matching results; in the remaining matching results, for each main object descriptor of the current key frame, finding the main object descriptor with the highest similarity in the candidate key frames determined according to the historical key frames, and screening out the corresponding similarities; regarding the relative pose corresponding to the screened similarities as a group of three-dimensional points, mapping them to the three-dimensional space, and clustering them to obtain multiple classes; if there is at least one class containing more than the number threshold of points, it is determined that the current key frame and the historical key frame are in the same scene, and the average pose of all points in the class containing the largest number of points is used as the relative pose between the current key frame and the historical key frame; if there is no class containing more than the number threshold of points, it is determined that the current key frame and the historical key frame are not in the same scene, and there is no loop edge between the current key frame and the historical key frame.

2. The method according to claim 1, characterized in that For any main object, a polar coordinate system is established with the main object as the center and a line connecting the laser radar to the main object as the polar axis to construct a main object descriptor, including: For any main object, a polar coordinate system is established with the main object as the center and the line connecting the laser radar to the main object as the polar axis. The polar axis is divided into Divide the polar angle into Divide equally to divide the environment around the main object into grids, project the points in the sub-map downward to the grids, and calculate the average height of all points in each grid; ≥2, ≥2; The average height values ​​corresponding to each grid are combined to form a The matrix of The matrix is ​​determined as the main object descriptor of the main object.

3. The method according to claim 2, characterized in that Also includes: Deleting points in the grid whose height is lower than a height threshold, where the height threshold is determined based on an average height of ground points; and / or ignoring points that are not projected into the grid.

4. The method according to claim 2, characterized in that: Also includes: The column vector obtained by averaging the main object descriptor row by row is defined as the ring feature vector; The row vector obtained by averaging the main object descriptor by column is defined as the fan feature vector; For any sub-map, save the main object position list, main object description sub-list, and fan feature vector list into the database according to the key frame sequence number; The main object position list stores the X coordinate and Y coordinate of each main object in the sub-map; The main object descriptor list stores the main object descriptor of each main object in the sub-map; The feature vector list stores the fan feature vectors of each main object descriptor of the sub-map.

5. The method according to claim 4, characterized in that The matching of the main object descriptor of the current key frame with the main object descriptors of the historical key frames in pairs includes: Construct a multi-dimensional tree structure based on the ring feature vectors of historical key frames; Create a candidate keyframe set; Traverse the ring feature vectors of all main object descriptors in the current key frame, search for a preset number of similar nodes in the multidimensional tree structure for each ring feature vector, and add the key frame numbers corresponding to the similar nodes to the candidate key frame set; The historical key frames corresponding to the key frame serial numbers in the candidate key frame set are taken as candidate key frames, and the main object descriptors of the current key frame and the main object descriptors of the candidate key frames are matched in pairs.

6. The method according to claim 5, characterized in that The matching of the main object descriptor of the current key frame with the main object descriptor of the candidate key frame in pairs includes: Get the fan feature vector list and main object descriptor list of the candidate keyframes; Calculating a coarse offset required for aligning a main object descriptor of a current key frame with a main object descriptor of a candidate key frame according to the fan feature vectors in the fan feature vector list; Constructing an optimization equation in an offset window centered on the coarse offset to calculate a fine offset required for aligning a main object descriptor of the current key frame with a main object descriptor of a candidate key frame; The similarity between the main object descriptor of the current key frame and the main object descriptor of the candidate key frame is calculated according to the precise offset, and the similarity is stored in the matching result.

7. The method according to claim 6, characterized in that The matching of the main object descriptor of the current key frame with the main object descriptor of the candidate key frame in pairs also includes: Get the list of main object positions of candidate keyframes; According to the X coordinate and Y coordinate of the main object in the current key frame in the submap, and the X coordinate and Y coordinate of the main object in the candidate key frame in the submap, calculate the relative pose between the main object of the current key frame and the main object of the candidate key frame, and store the relative pose and the similarity in the matching result in correspondence; The relative position and posture at least includes: an offset in the X direction, an offset in the Y direction, and a rotation angle of the main object of the current key frame relative to the main object of the candidate key frame.

8. The method according to claim 7, characterized in that Also includes: If it is determined that there is no loop edge between the current key frame and the historical key frame, all loop feature vectors of the current key frame are added to the multi-dimensional tree structure.

9. A method for constructing a point cloud map, characterized in that: At least: The radar odometry based on the main object feature is used to perform: determining the main object from each frame of the 3D point cloud; After the main object is determined, the main object features are extracted, and the main object features at least include: the centroid coordinates of each main object, and the direction vector of each main object; Each time a frame of 3D point cloud is obtained, the main object is matched with the previous frame of 3D point cloud, and the overall matching error is controlled to be minimum, and the relative posture of each frame relative to the previous frame is calculated; the overall matching error is related to all the main object features in each frame; the calculated relative postures are sequentially multiplied to obtain the posture of each frame in the world coordinate system; The loop detection method according to any one of claims 1 to 8.

10. An electronic device, characterized in that: include: A communication module, a processor and a memory, wherein the memory stores program instructions; The processor is used to execute program instructions stored in the memory, execute the loop detection method according to any one of claims 1 to 8, and / or the point cloud map construction method according to claim 9.

11. A computer-readable storage medium, characterized in that: A rewritable computer program is stored thereon; When the computer program runs on a computer device, the computer device executes the loop detection method according to any one of claims 1 to 8, and / or the point cloud map construction method according to claim 9.

Images