Loop detection method and device
By using geometric constraints and pose calculations in loopback detection, combined with odometer and GICP algorithm, the problem of loopback matching errors in complex scenarios is solved, and high-accurate loopback detection is achieved, reducing the recall rate of wrong loopbacks.
Patent Information
- Application Number
- CN202411247588.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-05
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2044-09-05
AI Technical Summary
Existing loopback detection methods are difficult to accurately filter out the correct loopback matching pairs in complex scenarios, resulting in error matching and trajectory optimization errors, especially in scenarios with relatively single features, which are difficult to correct error loopbacks.
By determining the geometric constraints between the reference frame and the detected frame, the relative pose is calculated using the odometer and GICP matching algorithm, the position and rotation angle thresholds are set, and the loopback matching error is determined when the relative pose difference is greater than the threshold.
The accuracy of loopback detection is significantly improved, and the error loopback recall rate is reduced to one thousandth, improving the accuracy of trajectory mapping of the SLAM system.
Smart Images

Figure CN118758343B_ABST
Abstract
Description
Technical Field
[0001] Embodiments of the present disclosure relate to the field of computer technology, and more particularly to a loop detection method and device. Background Art
[0002] Loop detection technology is an indispensable part of the SLAM (Simultaneous Localization and Mapping) system. Whether it is the latest open source LiDAR odometer or the traditional inertial wheel speed odometer method, if loop detection is not added to correct the odometer to a certain extent, cumulative errors will appear after a period of time and mileage, which will cause a vehicle to pass through the same scene twice. The sub-map built first and the sub-map built for the first time have a ghosting phenomenon. In the process of autonomous driving mapping, the vehicle often goes around and returns to the starting point to build a global map within a certain range. In order to obtain a globally consistent point cloud map, it is necessary to match the current scanned or illuminated local map with the historical record map, and then adjust the positional relationship between the current frame and the historical frame. Finally, the accumulated error of the odometer can be eliminated. This process is loop detection and matching.
[0003] Due to different scenes, different feature matching effects are also different. In actual scenes, in scenes with few features, mismatching may occur. How to efficiently screen out complete and correct loop matching pairs is a key step in the loop detection and matching module. At present, the method used in engineering to screen correct loops is relatively simple. Basically, the effective matching points or proportion of loop matching pairs are used to evaluate whether the loop of this pair is correct. Although this method is simple and easy to use, it is not suitable for complex actual scenes, especially scenes with relatively simple features, such as tunnels and roads with vegetation on both sides. It is not possible to screen out wrongly matched loop pairs, and it is very easy to have an incorrect loop that causes the overall trajectory to be optimized incorrectly. For example, there is no correctly matched pair of loops, but the proportion of effective matching points is large. In this case, this wrong loop match will be used as a constraint between the correct two frames to adjust the overall object trajectory, so the resulting trajectory is wrong and not easy to correct. It is impossible to find and correct the wrong loop matching pairs immediately.
[0004] Currently, the more common method for evaluating whether a loop is effective is based on the proportion of effective feature matches. For example, after iterative matching using ICP (Iterative Closest Point) or GICP (Generalized Iterative Closest Point), the proportion of matching points is used as the matching score to evaluate the quality of the match. This method is useful when the features are significant, but in complex and changing scenarios, the high or low matching score cannot fully indicate whether the loop is correct. Summary of the invention
[0005] The embodiments of the present disclosure provide a loop detection method and device.
[0006] In a first aspect, an embodiment of the present disclosure provides a loop detection method, comprising: determining a first relative pose between a reference frame and a detection frame in a matched loop pair; determining an adjacent frame with a predetermined cumulative mileage from the detection frame; determining a second relative pose between the adjacent frame and the detection frame, and determining a third relative pose between the adjacent frame and the reference frame; calculating a fourth relative pose between the reference frame and the detection frame based on the second relative pose and the third relative pose; in response to detecting that the difference between the first relative pose and the fourth relative pose is greater than a predetermined threshold, determining that the loop pair is mismatched.
[0007] In some embodiments, determining the first relative pose between the reference frame and the detection frame in the matched loop pair includes: splicing a predetermined first number of frames before and after the reference frame to obtain a spliced reference frame; and determining the first relative pose between the spliced reference frame and the detection frame.
[0008] In some embodiments, determining the adjacent frames with a predetermined cumulative distance from the detection frame includes: splicing a predetermined second number of frames before and after the adjacent frame with a predetermined cumulative distance from the detection frame to determine them as adjacent frames.
[0009] In some embodiments, determining the second relative pose between the adjacent frame and the detection frame includes: obtaining the second relative pose between the adjacent frame and the detection frame through an odometer.
[0010] In some embodiments, determining the third relative pose between the adjacent frame and the reference frame includes: obtaining the third relative pose between the adjacent frame and the reference frame by means of GICP generalized iterative closest point matching.
[0011] In some embodiments, the difference includes a position error and a rotation angle error, and the predetermined threshold includes a position error threshold and a rotation angle error threshold.
[0012] In some embodiments, the adjacent frames include a left adjacent frame and / or a right adjacent frame, and the fourth relative posture includes a fourth relative posture of the left adjacent frame and / or a fourth relative posture of the right adjacent frame; and in response to detecting that the difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold, comprising: in response to detecting that the difference between the first relative posture and the fourth relative posture of the left adjacent frame is greater than a predetermined threshold or the difference between the first relative posture and the fourth relative posture of the right adjacent frame is greater than a predetermined threshold.
[0013] In a second aspect, an embodiment of the present disclosure provides a loop detection device, comprising: a first determination unit, configured to determine a first relative pose between a reference frame and a detection frame in a matched loop pair; an adjacent frame determination unit, configured to determine an adjacent frame with a predetermined cumulative mileage from the detection frame; a second determination unit, configured to determine a second relative pose between the adjacent frame and the detection frame, and to determine a third relative pose between the adjacent frame and the reference frame; a pose calculation unit, configured to calculate a fourth relative pose between the reference frame and the detection frame based on the second relative pose and the third relative pose; and an error detection unit, configured to determine that the loop pair is mismatched in response to detecting that the difference between the first relative pose and the fourth relative pose is greater than a predetermined threshold.
[0014] In some embodiments, the first determination unit is further configured to: splice a predetermined first number of frames before and after the reference frame to obtain a spliced reference frame; and determine a first relative pose between the spliced reference frame and the detection frame.
[0015] In some embodiments, the adjacent frame determination unit is further configured to: splice a predetermined second number of frames before and after the adjacent frame with a predetermined cumulative distance from the detection frame to determine them as adjacent frames.
[0016] In some embodiments, the second determination unit is further configured to: obtain a second relative pose between the adjacent frame and the detection frame through an odometer.
[0017] In some embodiments, the second determination unit is further configured to obtain a third relative pose between the adjacent frame and the reference frame by means of GICP generalized iterative closest point matching.
[0018] In some embodiments, the difference includes a position error and a rotation angle error, and the predetermined threshold includes a position error threshold and a rotation angle error threshold.
[0019] In some embodiments, the adjacent frames include a left adjacent frame and / or a right adjacent frame, the fourth relative posture includes a fourth relative posture of the left adjacent frame and / or a fourth relative posture of the right adjacent frame; and the error detection unit is further configured to: in response to detecting that the difference between the first relative posture and the fourth relative posture of the left adjacent frame is greater than a predetermined threshold or the difference between the first relative posture and the fourth relative posture of the right adjacent frame is greater than a predetermined threshold.
[0020] In a third aspect, an embodiment of the present disclosure provides an electronic device, comprising: one or more processors; a storage device on which one or more computer programs are stored, and when the one or more computer programs are executed by the one or more processors, the one or more processors implement a method as described in any one of the first aspect or the second aspect.
[0021] In a fourth aspect, an embodiment of the present disclosure provides a computer-readable medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the method as described in any one of the first aspect or the second aspect is implemented.
[0022] In a fifth aspect, an embodiment of the present disclosure provides a computer program product, including a computer program, which implements the method as described in any one of the first aspect or the second aspect when executed by a processor.
[0023] The loop detection method and device provided by the embodiments of the present disclosure checks each pair of loops through the geometric constraints of the loop matching pairs. Taking a pair of loop detections found as an example, one frame of this pair of loops is used as a reference frame, and one frame of the other frame of this pair of loops (recorded as a detection frame) is selected at a distance of 3m on the left and right as the left and right neighboring frames of the detection frame. There is a constraint relationship between the odometer between the left and right neighboring frames and the detection frame, and there is a matching relationship with the reference frame. Through the geometric constraint relationship between the loop pairs, each pair of loops is detected. According to the scheme of the present application, the recall rate of the wrong loop after quality inspection is one thousandth (that is, the error rate of treating the wrong loop as a correct loop is as low as one thousandth). Compared with the current widely used quality inspection in the industry that only uses the ratio of effective matching points of the loop matching pair for quality inspection, the accuracy of the quality inspection loop is greatly improved.
[0024] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present disclosure, nor is it intended to limit the scope of the present disclosure. Other features of the present disclosure will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS
[0025] Other features, objects and advantages of the present disclosure will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:
[0026] Figure 1 is an exemplary system architecture diagram in which an embodiment of the present disclosure may be applied;
[0027] Figure 2 is a flow chart of an embodiment of a loop closure detection method according to the present disclosure;
[0028] Figure 3a-3c is a schematic diagram of an application scenario of the loop detection method according to the present disclosure;
[0029] Figure 4 is a schematic structural diagram of an embodiment of a loop detection device according to the present disclosure;
[0030] Figure 5 It is a structural diagram of a computer system of an electronic device suitable for implementing an embodiment of the present disclosure. DETAILED DESCRIPTION
[0031] The present disclosure is further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the relevant invention, rather than to limit the invention. It is also necessary to explain that, for ease of description, only the parts related to the relevant invention are shown in the accompanying drawings.
[0032] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present disclosure may be combined with each other. The present disclosure will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0033] Figure 1 An exemplary system architecture to which an embodiment of a loop closure detection method or a loop closure detection device of the present application can be applied is shown.
[0034] like Figure 1 As shown, the system architecture may include an unmanned vehicle 101. The unmanned vehicle 101 may be equipped with a driving control device 1011, a network 1012, and a laser radar 1013. The network 1012 is used to provide a medium for a communication link between the driving control device 1011 and the laser radar 1013. The network 1012 may include various connection types, such as wired, wireless communication links, or optical fiber cables, etc.
[0035] The driving control device (also called the on-board brain) 1011 is responsible for the intelligent control of the unmanned vehicle 101. The driving control device 1011 can be a separately set controller, such as a programmable logic controller (PLC), a single-chip microcomputer, an industrial control machine, etc.; it can also be a device composed of other electronic devices with input / output ports and operation control functions; it can also be a computer device installed with a vehicle driving control application.
[0036] It should be noted that in practice, the unmanned vehicle 101 may also be equipped with at least one sensor, such as a camera, a gravity sensor, a wheel speed sensor, etc. In some cases, the unmanned vehicle 101 may also be equipped with a GNSS (Global Navigation Satellite System) device and a SINS (Strap-down Inertial Navigation System), etc.
[0037] It should be noted that the loop detection method provided in the embodiment of the present application is generally executed by the driving control device 1011 , and accordingly, the loop detection device is generally disposed in the driving control device 1011 .
[0038] It should be understood that Figure 1 The number of driving control devices, networks, and laser radars in the embodiment is only for illustration. Any number of driving control devices, networks, and laser radars may be provided as required.
[0039] Continue to refer Figure 2 , shows a process 200 of an embodiment of a loop detection method according to the present disclosure. The loop detection method comprises the following steps:
[0040] Step 201, determining a first relative pose between a reference frame and a detection frame in a matched loop closure pair.
[0041] In this embodiment, the execution subject of the loop detection method (for example Figure 1 The driving control device shown in the figure processes the collected point cloud data and detects the matched loop pairs. The point cloud data collected at the starting point can be used as the reference frame, and the point cloud data collected at the end point can be used as the detection frame. The two frames of the loop pair are named loop_scan_left and loop_scan_right, and the first relative pose between them is recorded as
[0042] , abbreviated as T1.
[0043] Step 202, determining adjacent frames of the distance detection frame with a predetermined accumulated mileage.
[0044] In this embodiment, two frames with predetermined cumulative mileages before and after the loop_scan_right detection frame can be selected, such as Figure 3aAs shown, the adjacent frames loop_scan_right_neighbor (which may include the left neighbor frame and the right neighbor frame) of the loop matching pair loop_scan_right are recorded as the left neighbor frame loop_scan_right_neighbor_left and the right neighbor frame loop_scan_right_neighbor_right. The predetermined cumulative mileage can be 3 meters. The predetermined cumulative mileage can be set based on experience. It should not be too far to avoid excessive cumulative mileage error, nor too close, otherwise the two frames of point cloud data are too similar and indistinguishable, and geometric constraints cannot be formed.
[0045] Step 203, determining a second relative pose between the adjacent frame and the detection frame, and determining a third relative pose between the adjacent frame and the reference frame.
[0046] In this embodiment, the second relative position between the adjacent frame and the detection frame is denoted as , abbreviated as T2, the third relative pose between the adjacent frame and the reference frame is recorded as , abbreviated as T3.
[0047] Step 204: Calculate a fourth relative pose between the reference frame and the detection frame according to the second relative pose and the third relative pose.
[0048] In this embodiment, the second relative posture and the third relative posture are known, and the two loop_scan_left and loop_scan_right relative postures can be calculated through the conversion relationship of the relative postures, which are recorded as , abbreviated as T4, the specific calculation process is:
[0049] Right now
[0050]
[0051] Step 205: In response to detecting that the difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold, determining that a loop pair matching error occurs.
[0052] In this embodiment, the original loop matching pair matching results are calculated respectively.
[0053] (ie T1) and (i.e. T4) error :
[0054]
[0055] If the error is too large, it means the loopback matching is wrong, otherwise the loopback matching is correct.
[0056] The method provided by the above-mentioned embodiments of the present disclosure uses geometric constraints to perform quality detection on each pair of loop closures in order to prevent the found erroneous loop closures from producing erroneous loop optimization results. Compared with the traditional method of detecting the quality of loop closures based on the ratio of effective feature matching, the present disclosure takes a different approach and uses the geometric constraint relationship between loop closure pairs for detection. It is not only used in a variety of complex and diverse scenarios, but also has greatly improved accuracy compared to traditional quality inspection loop closures. The present disclosure has been verified by a large amount of data, and the recall rate of erroneous loops of the present disclosure is one thousandth (i.e., the error rate of treating erroneous loops as correct loops is as low as one thousandth), which provides accurate loop closure guarantees in the links of SLAM mapping and odometer optimization. Figure 3b-3c In the figure, red and green represent two sub-frames of a pair of loop matching. It can be seen that the features in the red frames of the two figures are not matched correctly. If the validity of the loop pair is judged based on the traditional ratio of valid feature matching points, the loops in the following two scenarios will be mistaken for correct ones, which will greatly mislead the SLAM system. However, by using the loop detection method disclosed in the present invention, similar erroneous loops will be screened out, thereby improving the accuracy of the SLAM system.
[0057] In some optional implementations of this embodiment, determining the first relative pose between the reference frame and the detection frame in the matched loop pair includes: splicing a predetermined first number of frames before and after the reference frame to obtain a spliced reference frame; determining the first relative pose between the spliced reference frame and the detection frame. Selecting 5 frames before and after the loop_scan_left frame and splicing them with loop_scan_left to obtain a sub-image is to increase the accuracy of subsequent matching. After the sub-image is spliced, the sub-image is still recorded as loop_scan_left. This can make the point cloud denser and improve the speed and accuracy of point cloud matching.
[0058] In some optional implementations of this embodiment, the determining of the adjacent frames with a predetermined cumulative distance from the detection frame includes: splicing a predetermined second number of frames before and after the adjacent frames with a predetermined cumulative distance from the detection frame to determine them as adjacent frames. The adjacent frames may include left adjacent frames and / or right adjacent frames. The selected left adjacent frame loop_scan_right_neighbor_left and right adjacent frame loop_scan_right_neighbor_right are respectively selected with 3 frames before and after to obtain submaps, which are recorded as submap_neightbor_left and submap_neightbor_right for better distinction.
[0059] In some optional implementations of this embodiment, the determining the second relative pose between the adjacent frame and the detection frame includes: obtaining the second relative pose between the adjacent frame and the detection frame by an odometer. The relative pose between submap_neightbor_left and loop_scan_right can be obtained by an odometer. , that is, T2. Similarly, the relative pose between submap_neightbor_right and loop_scan_right can be obtained through the odometer , that is, T2'. The odometer can quickly and accurately calculate the relative pose and obtain the local optimal result.
[0060] In some optional implementations of this embodiment, the determining the third relative pose between the adjacent frame and the reference frame includes: obtaining the third relative pose between the adjacent frame and the reference frame by GICP generalized iterative closest point matching. , that is, T3. Similarly, submap_neightbor_right and loop_scan_left are matched by GICP to obtain , that is, T3'. Since the reference frame is too far away from the adjacent frame, the accumulated error of the odometer is too high. If the odometer is used, the error will be too large. Therefore, the GICP matching method is used to calculate the relative pose.
[0061] In some optional implementations of this embodiment, the difference includes a position error and a rotation angle error, and the predetermined threshold includes a position error threshold and a rotation angle error threshold. By quantifying the error into a position error and a rotation angle error, it is easier to set the threshold, and it is recommended that the position threshold be set to 0.2m and the angle threshold be 1°.
[0062] In some optional implementations of this embodiment, the adjacent frames include a left adjacent frame and / or a right adjacent frame, and the fourth relative posture includes a fourth relative posture of the left adjacent frame and / or a fourth relative posture of the right adjacent frame; and in response to detecting that the difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold, including: in response to detecting that the difference between the first relative posture and the fourth relative posture of the left adjacent frame is greater than a predetermined threshold or the difference between the first relative posture and the fourth relative posture of the right adjacent frame is greater than a predetermined threshold.
[0063] Calculate the original loop matching pair matching results separately (ie T1) and Error And the original loop matching pair matching results (ie T1) and Error The reason why the error calculation is performed twice here is to more strictly screen out the wrong loops. Only when the geometric constraint errors on the left and right sides are less than a certain threshold can the pair of loops be considered correct.
[0064] and The calculation process is as follows:
[0065]
[0066]
[0067] and It represents the error of relative posture, so the two errors can be converted into position and posture errors, that is , after quantifying the error into position error and rotation angle error, it is easier to set the threshold. The recommended position threshold is set to 0.2m and the angle threshold is 1°. Therefore, the loop can only be considered correct when the following inequality holds, otherwise it will be eliminated:
[0068]
[0069] Continue to see Figure 3a-3c , Figure 3a-3c FIG. 1 is a schematic diagram of an application scenario of the loop detection method according to this embodiment. Figure 3aIn the application scenario, the red dot on the right represents the reference frame, which is the point cloud data collected by the unmanned vehicle at the starting position. The red dot on the left represents the detection frame, which is the point cloud data collected by the unmanned vehicle at the end position. The green dot on the left represents the left neighbor frame, which is the point cloud data collected 3 meters to the left of the collection position of the detection frame of the unmanned vehicle. The green dot on the right represents the right neighbor frame, which is the point cloud data collected 3 meters to the right of the collection position of the detection frame of the unmanned vehicle. The relative pose directly calculated between the detection frame and the reference frame is T1, the relative pose obtained between the detection frame and the left neighbor frame through the odometer is T2, and the relative pose obtained between the left neighbor frame and the reference frame through the GICP matching algorithm is T3. It can be seen that the positions of the left neighbor frame, reference frame, and detection frame form a triangle with geometric constraints. Therefore, according to the parameters T2 and T3 of the two sides of the triangle, the positions of the reference frame and the detection frame can be calculated to form the third side T4. Compare T4 with T1 to get the first error. Similarly, the relative pose between the detection frame and the right adjacent frame obtained by the odometer is T2', and the relative pose between the right adjacent frame and the reference frame obtained by the GICP matching algorithm is T3'. It can be seen that the positions of the right adjacent frame, reference frame, and detection frame form a triangle with geometric constraints. Therefore, according to the parameters T2' and T3' of the two sides of the triangle, the positions of the reference frame and the detection frame can be calculated to form the third side T4'. Compare T4' with T1 to get the second error.
[0070] This application uses geometric constraints to perform quality checks on each pair of loop closures in order to prevent the found erroneous loop closures from producing erroneous loop optimization results. Compared to the traditional method of detecting the quality of loop closures based on the ratio of effective feature matching, this application takes a different approach and uses the geometric constraint relationship between loop closure pairs for detection. It is not only used in a variety of complex and diverse scenarios, but also has greatly improved accuracy over traditional quality inspection loop closures. This application has been verified by a large amount of data, and the recall rate of erroneous loops in this application is one thousandth (that is, the error rate of treating erroneous loops as correct loops is as low as one thousandth), which provides accurate loop guarantee in the links of SLAM mapping and odometer optimization. Figure 3b-3c In the figure, red and green represent two sub-frames of a pair of loop matching. It can be seen that the features in the red frames of the two figures are not matched correctly. If the validity of the loop pair is judged based on the traditional ratio of valid feature matching points, the loops of the two scenes will be mistaken for correct ones, which will greatly mislead the SLAM system. However, by using the loop detection method of the present application, similar erroneous loops will be screened out, thereby improving the accuracy of the SLAM system.
[0071] Further references Figure 4 As an implementation of the methods shown in the above figures, the present disclosure provides an embodiment of a loop detection device. Figure 2 Corresponding to the method embodiment shown, the device can be specifically applied to various electronic devices.
[0072] like Figure 4 As shown, the loop detection device 400 of this embodiment includes: a first determination unit 401, an adjacent frame determination unit 402, a second determination unit 403, a posture calculation unit 404, and an error detection unit 405. Among them, the first determination unit 401 is configured to determine the first relative posture between the reference frame and the detection frame in the matched loop pair; the adjacent frame determination unit 402 is configured to determine the adjacent frame with a predetermined cumulative distance from the detection frame; the second determination unit 403 is configured to determine the second relative posture between the adjacent frame and the detection frame, and determine the third relative posture between the adjacent frame and the reference frame; the posture calculation unit 404 is configured to calculate the fourth relative posture between the reference frame and the detection frame according to the second relative posture and the third relative posture; the error detection unit 405 is configured to determine that the loop pair matching is wrong in response to detecting that the difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold.
[0073] In this embodiment, the specific processing of the first determination unit 401, the adjacent frame determination unit 402, the second determination unit 403, the posture calculation unit 404, and the error detection unit 405 of the loop detection device 400 can refer to Figure 2 Corresponding to step 201, step 202, step 203, step 204 and step 205 in the embodiment.
[0074] In some optional implementations of this embodiment, the first determination unit 401 is further configured to: splice a predetermined first number of frames before and after the reference frame to obtain a spliced reference frame; and determine a first relative pose between the spliced reference frame and the detection frame.
[0075] In some optional implementations of this embodiment, the adjacent frame determining unit 402 is further configured to: splice a predetermined second number of frames before and after the adjacent frame with a predetermined cumulative distance from the detection frame to determine them as adjacent frames.
[0076] In some optional implementations of this embodiment, the second determination unit 403 is further configured to: obtain a second relative posture between the adjacent frame and the detection frame through an odometer.
[0077] In some optional implementations of this embodiment, the second determination unit 403 is further configured to: obtain a third relative pose between the adjacent frame and the reference frame by means of GICP generalized iterative closest point matching.
[0078] In some optional implementations of this embodiment, the difference includes a position error and a rotation angle error, and the predetermined threshold includes a position error threshold and a rotation angle error threshold.
[0079] In some optional implementations of this embodiment, the adjacent frames include a left adjacent frame and / or a right adjacent frame, the fourth relative posture includes the fourth relative posture of the left adjacent frame and / or the fourth relative posture of the right adjacent frame; and the error detection unit 405 is further configured to: in response to detecting that the difference between the first relative posture and the fourth relative posture of the left adjacent frame is greater than a predetermined threshold or the difference between the first relative posture and the fourth relative posture of the right adjacent frame is greater than a predetermined threshold.
[0080] It should be noted that the collection, collection, updating, analysis, processing, use, transmission, storage and other aspects of user personal information involved in the technical solution of this disclosure are in compliance with the provisions of relevant laws and regulations, are used for legitimate purposes, and do not violate public order and good morals. Necessary measures are taken for user personal information to prevent illegal access to user personal information data and maintain the security of user personal information, network security and national security.
[0081] According to an embodiment of the present disclosure, the present disclosure also provides an electronic device and a readable storage medium.
[0082] An electronic device comprises: one or more processors; a storage device on which one or more computer programs are stored, and when the one or more computer programs are executed by the one or more processors, the one or more processors implement the method described in process 200.
[0083] A computer-readable medium stores a computer program, wherein the computer program implements the method described in process 200 when executed by a processor.
[0084] Figure 5 A schematic block diagram of an example electronic device 500 that can be used to implement an embodiment of the present disclosure is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present disclosure described and / or required herein.
[0085] like Figure 5As shown, the device 500 includes a computing unit 501, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 502 or a computer program loaded from a storage unit 508 into a random access memory (RAM) 503. In the RAM 503, various programs and data required for the operation of the device 500 can also be stored. The computing unit 501, the ROM 502, and the RAM 503 are connected to each other via a bus 504. An input / output (I / O) interface 505 is also connected to the bus 504.
[0086] A number of components in the device 500 are connected to the I / O interface 505, including: an input unit 506, such as a keyboard, a mouse, etc.; an output unit 507, such as various types of displays, speakers, etc.; a storage unit 508, such as a disk, an optical disk, etc.; and a communication unit 509, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 509 allows the device 500 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.
[0087] The computing unit 501 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 501 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 501 performs the various methods and processes described above, such as the road area planning method. For example, in some embodiments, the road area planning method may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit 508. In some embodiments, part or all of the computer program may be loaded and / or installed on the device 500 via the ROM 502 and / or the communication unit 509. When the computer program is loaded into the RAM 503 and executed by the computing unit 501, one or more steps of the road area planning method described above may be performed. Alternatively, in other embodiments, the computing unit 501 may be configured to perform the road area planning method in any other appropriate manner (e.g., by means of firmware).
[0088] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.
[0089] The program code for implementing the method of the present disclosure may be written in any combination of one or more programming languages. These program codes may be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that the program code, when executed by the processor or controller, enables the functions / operations specified in the flow chart and / or block diagram to be implemented. The program code may be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.
[0090] In the context of the present disclosure, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or device, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, 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 foregoing.
[0091] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).
[0092] The systems and techniques described herein may be implemented in a computing system that includes back-end components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes front-end components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such back-end components, middleware components, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), and the Internet.
[0093] A computer system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The relationship between the client and the server is generated by computer programs running on the respective computers and having a client-server relationship with each other. The server may be a server of a distributed system, or a server combined with a blockchain. The server may also be a cloud server, or an intelligent cloud computing server or intelligent cloud host with artificial intelligence technology. The server may be a server of a distributed system, or a server combined with a blockchain. The server may also be a cloud server, or an intelligent cloud computing server or intelligent cloud host with artificial intelligence technology.
[0094] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this disclosure can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solutions disclosed in this disclosure can be achieved, and this document does not limit this.
[0095] The above specific implementations do not constitute a limitation on the protection scope of the present disclosure. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present disclosure shall be included in the protection scope of the present disclosure.
Claims
1. A loop detection method, comprising: Determine a first relative pose between the reference frame and the detection frame in the matched loop closure pair; Determining an adjacent frame with a predetermined cumulative mileage from the detection frame; Determining a second relative pose between the adjacent frame and the detection frame, and determining a third relative pose between the adjacent frame and the reference frame; The fourth relative pose between the reference frame and the detection frame is calculated according to the second relative pose and the third relative pose. The specific calculation process is shown in the following formula: Among them, T4 represents the fourth relative posture, T2 represents the second relative posture, and T3 represents the third relative posture; In response to detecting that the difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold, determining that the loop pair matching is wrong.
2. The method according to claim 1, wherein: The determining of a first relative pose between a reference frame and a detection frame in a matched loop closure pair includes: splicing a predetermined first number of frames before and after the reference frame to obtain a spliced reference frame; Determine a first relative pose between the spliced reference frame and the detection frame.
3. The method according to claim 1, wherein: The step of determining an adjacent frame with a predetermined cumulative mileage from the detection frame comprises: A predetermined second number of frames before and after the adjacent frame with a predetermined cumulative distance from the detection frame are spliced to determine as adjacent frames.
4. The method according to claim 1, wherein: The determining a second relative position between the adjacent frame and the detection frame comprises: A second relative position and posture between the adjacent frame and the detection frame is obtained through the odometer.
5. The method according to claim 1, wherein: The determining a third relative pose between the adjacent frame and the reference frame comprises: The third relative pose between the adjacent frame and the reference frame is obtained by means of GICP generalized iterative closest point matching.
6. The method according to claim 1, wherein: The difference includes a position error and a rotation angle error, and the predetermined threshold includes a position error threshold and a rotation angle error threshold.
7. The method according to claim 1, wherein: The adjacent frames include a left adjacent frame and / or a right adjacent frame, and the fourth relative posture includes a fourth relative posture of the left adjacent frame and / or a fourth relative posture of the right adjacent frame; as well as In response to detecting that a difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold, the method comprises: In response to detecting that a difference between the first relative posture and the fourth relative posture of the left adjacent frame is greater than a predetermined threshold or a difference between the first relative posture and the fourth relative posture of the right adjacent frame is greater than a predetermined threshold.
8. A loop detection device, comprising: A first determination unit is configured to determine a first relative pose between a reference frame and a detection frame in a matched loop closure pair; an adjacent frame determining unit, configured to determine an adjacent frame with a predetermined cumulative mileage from the detection frame; a second determination unit configured to determine a second relative pose between the adjacent frame and the detection frame, and to determine a third relative pose between the adjacent frame and the reference frame; The posture calculation unit is configured to calculate a fourth relative posture between the reference frame and the detection frame according to the second relative posture and the third relative posture. The specific calculation process is shown in the following formula: Among them, T4 represents the fourth relative posture, T2 represents the second relative posture, and T3 represents the third relative posture; The error detection unit is configured to determine that the loop pair matching is wrong in response to detecting that the difference between the first relative posture and the fourth relative posture is greater than a predetermined threshold.
9. An electronic device, comprising: one or more processors; a storage device having one or more computer programs stored thereon, When the one or more computer programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 7.
10. A computer readable medium having a computer program stored thereon, wherein: When the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.
Citation Information
Patent Citations
Visual navigation method and system based on semi-consistent topology measurement map
CN117571002A
External parameter calibration method and device in autonomous navigation system, equipment and storage medium
CN118032011A
