Map determination method and apparatus, electronic device, and storage medium

Abstract

A method, apparatus, electronic device, and storage medium for map determination are disclosed, relating to the field of point cloud map technology. The method includes: acquiring raw point cloud data and inertial measurement data, and determining multiple keyframe poses based on the raw point cloud data and inertial measurement data; determining a first pose map, and determining edge constraints of the first pose map based on the pose estimation of each keyframe pose and the global pose observations of each keyframe pose; performing global optimization on the first pose map based on the edge constraints to determine an initial trajectory; determining candidate keyframe pose pairs based on the initial trajectory, and determining loop closure constraints based on the candidate keyframe pose pairs; determining a second pose map, and performing global optimization on the second pose map based on the loop closure constraints and edge constraints to determine the target keyframe pose; and stitching and fusing the raw point cloud corresponding to the target keyframe pose to obtain the target map. This application can effectively eliminate accumulated errors and obtain a map with optimal global consistency and higher accuracy.

Description

Technical Field

[0001] This application relates to the field of point cloud map technology, and more specifically, to a map determination method, apparatus, electronic device, and storage medium. Background Technology

[0002] With the widespread application of autonomous driving in vehicles, achieving all-weather, highly reliable centimeter-level positioning has become a core prerequisite for the autonomous operation of autonomous driving systems. In environments with weak GPS signals (such as tunnels, underground parking garages, and densely populated urban areas), matching and positioning based on pre-built high-precision point cloud maps has become the mainstream technical approach. Currently, high-precision point cloud maps are mainly constructed through online and offline methods. However, both online and offline construction suffer from certain global distortions or misalignments, leading to low accuracy in the resulting maps. Therefore, improving the accuracy of the constructed maps has become an urgent problem to be solved. Summary of the Invention

[0003] In view of this, embodiments of this application propose a map determination method, apparatus, electronic device, and storage medium to improve the above-mentioned problems.

[0004] According to a first aspect of the embodiments of this application, a method for determining a map is provided. The method includes: acquiring raw point cloud data and inertial measurement data, and determining multiple keyframe poses based on the raw point cloud data and the inertial measurement data, wherein each keyframe pose includes a pose estimate and a global pose observation; determining a first pose graph based on the multiple keyframe poses, and determining edge constraints of the first pose graph based on the pose estimate of each keyframe pose and the global pose observation of each keyframe pose; and performing edge constraints on the first pose graph. A global optimization solution is performed to determine an initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the multiple keyframe poses; candidate keyframe pose pairs are determined based on the initial trajectory, and loop closure constraints are determined based on the candidate keyframe pose pairs; a second pose map is determined based on the original point cloud data, the inertial measurement data, and the initial trajectory, and the second pose map is globally optimized based on the loop closure constraints and the edge constraints to determine the target keyframe pose; the original point cloud data corresponding to the target keyframe pose is stitched and fused to obtain the target map.

[0005] In some embodiments, determining a first pose graph based on the plurality of keyframe poses and determining edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose includes: determining a first relative pose transformation relationship between adjacent keyframe poses in the plurality of keyframe poses based on the pose estimation of each keyframe pose, and determining a first edge constraint based on the first relative pose transformation relationship; and determining a second edge constraint based on the global pose observation of each keyframe pose.

[0006] In some embodiments, before determining candidate keyframe pose pairs based on the initial trajectory and determining loop closure constraints based on the candidate keyframe pose pairs, the method further includes: obtaining the optimized residual corresponding to the global pose observation value of each keyframe pose; if it is determined that there is a reference keyframe pose with the optimized residual greater than the residual threshold, then removing the global pose observation value corresponding to the reference keyframe pose from the initial trajectory and updating the second edge constraint.

[0007] In some embodiments, after updating the second side constraint, the method further includes: determining an optimized pose for each of the plurality of keyframe poses based on the initial trajectory; determining a rotation matrix based on the optimized pose and real-time dynamic positioning observation data of each of the plurality of keyframe poses; aligning the initial trajectory based on the rotation matrix and updating the initial trajectory.

[0008] In some embodiments, after stitching and fusing the original point cloud data corresponding to the target keyframe pose to obtain the target map, the method further includes: sampling the target map according to a preset voxel size to determine multiple sampled point clouds, and determining the grid index of each sampled point cloud in the multiple sampled points according to a preset map tile size; dividing the target map into blocks according to the grid index to obtain multiple map tiles; and in response to a positioning command, determining the target map tile in the multiple map tiles and displaying the target map tile.

[0009] In some embodiments, determining candidate keyframe pose pairs based on the initial trajectory includes: traversing the plurality of keyframe poses in the initial trajectory to determine the distance and time interval between the plurality of keyframe poses; and determining keyframe pose pairs whose distance is less than a distance threshold and whose time interval is greater than a duration threshold as candidate keyframe pose pairs among the plurality of keyframe poses.

[0010] In some embodiments, for each pair of candidate keyframe poses, determining the loop closure constraint based on the candidate keyframe poses includes: determining multiple adjacent keyframe poses of the first candidate keyframe pose in the candidate keyframe pose pair, and determining a local point cloud map based on the multiple adjacent keyframe poses; determining a single-frame scan point cloud of the second candidate keyframe pose in the candidate keyframe pose pair, and registering the single-frame scan point cloud with the local point cloud map to obtain a second relative pose transformation relationship and a registration score; if the registration score is determined to be greater than a score threshold, then the second relative pose transformation relationship is determined as the loop closure constraint of the candidate keyframe pose pair.

[0011] According to a second aspect of the embodiments of this application, a map determination apparatus is provided. The apparatus includes: an acquisition module, configured to acquire raw point cloud data and inertial measurement data, and determine a plurality of keyframe poses based on the raw point cloud data and the inertial measurement data, wherein each keyframe pose includes a pose estimate and a global pose observation; an edge constraint determination module, configured to determine a first pose graph based on the plurality of keyframe poses, and determine edge constraints of the first pose graph based on the pose estimate of each keyframe pose and the global pose observation of each keyframe pose; and a first optimization module, configured to perform global optimization on the first pose graph based on the edge constraints. The algorithm includes: an optimization solution to determine an initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the multiple keyframe poses; a loop closure constraint determination module, used to determine candidate keyframe pose pairs based on the initial trajectory, and determine loop closure constraints based on the candidate keyframe pose pairs; a second optimization module, used to determine a second pose map based on the original point cloud data, the inertial measurement data, and the initial trajectory, and to perform global optimization on the second pose map based on the loop closure constraints and the edge constraints to determine the target keyframe pose; and a target map determination module, used to stitch and fuse the original point cloud data corresponding to the target keyframe pose to obtain the target map.

[0012] According to a third aspect of the embodiments of this application, an electronic device is provided, comprising: a processor; and a memory storing computer-readable instructions, wherein when executed by the processor, the computer-readable instructions implement the map determination method as described above.

[0013] According to a fourth aspect of the embodiments of this application, a computer-readable storage medium is provided, on which computer-readable instructions are stored, which, when executed by a processor, implement the map determination method as described above.

[0014] In this application, the scheme first determines multiple keyframe poses based on the acquired raw point cloud data and inertial measurement data. Then, a first position map is determined based on these keyframe poses. This allows for the determination of edge constraints for the first pose map based on the pose estimation and global pose observations of each keyframe pose. The mechanical energy of these edge constraints is then used for the first round of optimization to determine the initial trajectory. Candidate keyframe pose pairs are then selected from the initial trajectory, and closure constraints are determined based on these pairs. This allows for a second round of optimization based on the closure and edge constraints to obtain the target keyframe pose. Finally, the target map is determined based on the raw point cloud data corresponding to the target keyframe pose. This application designs a tightly coupled global optimization process that includes a two-stage process: a first round of optimization using edge constraints and a second round of optimization combining edge constraints and closure constraints. This process deeply integrates local continuous observations, global absolute observations, and cross-time-period closed-loop observation information, fully utilizing offline computing resources for joint optimization. This effectively eliminates accumulated errors and yields a point cloud map with optimal global consistency and higher accuracy. This solves the problems of insufficient absolute accuracy and scene integrity of point cloud maps caused by shallow multi-sensor fusion and insufficient global optimization in existing methods, as well as the problem of difficulty in ensuring global consistency of point cloud maps due to the separation of system error processing in existing methods.

[0015] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit the embodiments of this application. Attached Figure Description

[0016] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application. It is obvious that the drawings described below are merely some embodiments of this application, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort.

[0017] Figure 1 This is a schematic diagram of a map determination system according to an embodiment of this application;

[0018] Figure 2 This is a schematic flowchart illustrating a map determination method according to an embodiment of this application; Figure 3 This is a schematic flowchart illustrating a map determination method according to another embodiment of this application; Figure 4 This is a schematic flowchart illustrating a map determination method according to another embodiment of this application; Figure 5 This is a schematic flowchart illustrating a map determination method according to another embodiment of this application; Figure 6 This is a schematic flowchart illustrating a map determination method according to yet another embodiment of this application; Figure 7 This is a schematic flowchart illustrating a map determination method according to an embodiment of this application; Figure 8 This is a block diagram of a map determining device according to an embodiment of this application; Figure 9 This is a hardware structure diagram of an electronic device according to an embodiment of this application.

[0019] The accompanying drawings have illustrated specific embodiments of the present application. More detailed descriptions will follow. These drawings and descriptions are not intended to limit the scope of the present application's embodiments in any way, but rather to illustrate the concepts of the present application's embodiments to those skilled in the art through specific embodiments. Detailed Implementation

[0020] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided to make this application more comprehensive and complete, and to fully convey the concept of the exemplary embodiments to those skilled in the art.

[0021] Furthermore, the described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. Numerous specific details are provided in the following description to give a thorough understanding of embodiments of this application. However, those skilled in the art will recognize that the technical solutions of this application can be practiced without one or more of the specific details, or other methods, apparatuses, steps, etc., can be employed. In other instances, well-known methods, apparatuses, implementations, or operations are not shown or described in detail to avoid obscuring various aspects of this application.

[0022] Currently, existing point cloud map construction technologies mainly include two paradigms: online construction and offline construction. Online construction technologies, such as real-time localization and mapping (RTD) based on LiDAR, can generate environmental maps in real time. Their core purpose is to support online positioning and navigation, but they struggle with time-consuming yet crucial global backend optimization, inevitably leading to accumulated errors and localized distortions in the map. Another common approach relies on professional-grade mobile mapping systems to acquire data and process it offline. While this method offers high accuracy, it is extremely expensive, and the data processing flow is closed, making it difficult to customize optimizations for specific positioning needs.

[0023] To strike a balance between cost and performance, and to improve the performance of point cloud map-based localization systems in complex scenarios, offline mapping using consumer-grade multi-sensors has become a research hotspot. Existing technologies typically employ loosely coupled front-end fusion and independent back-end optimization processes.

[0024] However, existing multi-sensor fusion methods often focus on the front-end data association or state estimation stage (loose coupling), or only optimize the pose map in the back-end. This shallow fusion fails to integrate and tightly couple the geometry, texture, scale, and motion constraints of different sensors during the global point cloud reconstruction stage. Offline processing has ample computational resources and should be able to solve more complex joint optimization problems to maximize map accuracy, but existing technologies have failed to effectively utilize this advantage, resulting in limited improvement in the accuracy and integrity of the generated map in areas with complementary sensor characteristics (such as pure white walls without lasers or dark corridors with visual impairment).

[0025] Meanwhile, during offline mapping, systematic errors such as spatiotemporal calibration errors between sensors and zero bias of inertial measurement units are coupled with motion estimation and map reconstruction errors, accumulating continuously. Existing technologies typically execute "calibration," "odometry," and "mapping" as separate modules sequentially, lacking a mechanism for online joint calibration and compensation of these systematic errors during global reconstruction. This approach leads to unavoidable global distortions or misalignments in long-sequence, large-loop scenarios, even after pose graph optimization, due to deeply coupled systematic errors, severely affecting the long-term consistency of the map as a positioning reference.

[0026] Furthermore, existing point cloud maps typically output a single, complete point cloud map file. When applied to real-time positioning systems such as autonomous driving, it is necessary to dynamically load map data of the surrounding environment based on the vehicle's location. A single large file cannot efficiently support this partial loading and unloading requirement, affecting the system's operational efficiency and flexibility.

[0027] To address the aforementioned issues, embodiments of this application provide a map determination method. By designing a two-stage optimization process that combines edge constraints and closure constraints in a tightly coupled global optimization flow, it deeply integrates local continuous observation, global absolute observation, and cross-time period closed-loop observation information. It fully utilizes offline computing resources for joint optimization, thereby effectively eliminating accumulated errors and obtaining a point cloud map with optimal global consistency and higher accuracy.

[0028] Please see Figure 1 , Figure 1 This application illustrates a map determination system according to an embodiment of the present application, such as... Figure 1 As shown below, the map determination system implements a map determination method by example.

[0029] In one alternative implementation, the map determination system includes an inertial measurement unit 110, a radar sensor 120, a global navigation satellite module 130, and a processing module 140, wherein the inertial measurement unit 110, the radar sensor 120, and the global navigation satellite module 130 refer to hardware devices, and the processing module 140 refers to a software unit or module.

[0030] For example, inertial measurement unit 110 can first acquire inertial measurement data and radar sensor 120 can acquire raw point cloud data, which are then sent to processing module 140. Processing module 140 can then determine multiple keyframe poses based on the raw point cloud data and inertial measurement data. Each keyframe pose includes a pose estimate and a global pose observation. Next, processing module 140 determines a first pose map based on the multiple keyframe poses, and determines the edge constraints of the first pose map based on the pose estimate and the global pose observation of each keyframe pose. The global pose observation is acquired by global navigation satellite module 130 and sent to processing module 140. Then, processing module 140 performs global optimization on the first pose map based on the edge constraints to determine an initial trajectory, which indicates the pose trajectory after connecting multiple keyframe poses. Subsequently, processing module 140 determines candidate keyframe pose pairs based on the initial trajectory and determines loop closure constraints based on the candidate keyframe pose pairs. Next, the processing module 140 determines the second pose map based on the original point cloud data, inertial measurement data, and initial trajectory. It then performs global optimization on the second pose map based on loop closure constraints and edge constraints to determine the target keyframe pose. Finally, the processing module 140 stitches and fuses the original point cloud data corresponding to the target keyframe pose to obtain the target map.

[0031] Figure 1 The map determination system in the text can be used to achieve the following Figure 2 For the method used to determine the described map, please refer to [link / reference needed]. Figure 2 , Figure 2 This application illustrates a map determination method according to an embodiment of the present application. In a specific embodiment, this map determination method can be applied to, for example... Figure 8 The map determining device 700 shown and the electronic device 800 equipped with the map determining device 700 ( Figure 9 The specific process of this embodiment will be described below. Of course, it is understood that this method can be executed by an electronic device with computing power, such as a vehicle-mounted server, a cloud server, or other processors. The following will focus on... Figure 2 The process shown is described in detail. The method for determining the map may specifically include the following steps 210-260.

[0032] Step 210: Obtain raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation.

[0033] As an alternative approach, various sensors can be installed on the vehicle or other map-building equipment to collect different types of environmental data about the vehicle's surroundings during operation. This allows for the construction of corresponding maps based on the different types of environmental data. Specifically, LiDAR sensors and inertial measurement units can be installed on the vehicle to collect data about its surroundings, thereby obtaining raw point cloud data and inertial measurement data of the environment in which the vehicle is located.

[0034] In some optional embodiments, after acquiring the raw point cloud data and inertial measurement data, the raw point cloud data and inertial measurement data can be input into a laser inertial odometry (LIO) system based on iterative error state Kalman filtering (IEKF). The LIO system then determines multiple keyframe poses based on the raw point cloud data and inertial measurement data. While the raw point cloud data acquired by the lidar is highly accurate, it undergoes motion distortion during vehicle movement and is prone to data loss in simple environments (such as corridors). In contrast, the IMU (Inertial Measurement Unit) has a high update frequency (typically between 100Hz and 1000Hz), providing accurate acceleration and angular velocity over a short time to calculate relative motion, but it is subject to drift. Therefore, the motion gaps in point cloud data acquisition by the lidar can be filled by the inertial measurement data collected by the IMU, thus correcting point cloud distortion. At the same time, the point cloud data collected by the lidar can be used to correct the cumulative drift of the inertial measurement data collected by the IMU, thereby obtaining a more accurate keyframe pose.

[0035] Optionally, after inputting the raw point cloud data and inertial measurement data into the LIO based on iterative error state Kalman filtering, the LIO first preprocesses the raw point cloud data and inertial measurement data to handle outlier data. Then, it performs state estimation on the data after outlier handling. State estimation can be performed through optimization methods or methods based on iterative error state Kalman filtering. That is, the state vector is first recursively derived from the inertial measurement data to obtain the initial estimated pose. Then, the initial estimated pose is corrected and updated based on the raw point cloud data using Kalman gain. In this way, the LIO outputs a continuous pose sequence. Then, multiple poses that meet preset conditions are selected from the output continuous pose sequence to obtain multiple keyframe poses.

[0036] Optionally, in the continuous pose sequence output by LIO, the keyframe position can be determined based on the distance and angle difference between different poses. If the distance or angle between any two different frame poses exceeds a certain threshold, the corresponding pose frame can be determined as the keyframe pose. The distance between any two frame poses can be the relative translation distance between them, and the angle between them can be the relative rotation angle. For example, any pose can be selected as the current keyframe pose, and then poses in the continuous pose sequence whose relative translation exceeds 1.5 meters or whose relative rotation exceeds 15 degrees are determined as the next keyframe pose, thus obtaining multiple keyframe poses.

[0037] Optionally, to ensure that the target map can be determined based on the keyframe poses, after determining multiple keyframe poses, the corresponding timestamp, ID information, pose estimate at the corresponding time determined by LIO, and global pose observation value can be recorded for each keyframe pose. The global pose observation value is RTK (Real-Time Kinematic) data observed by the Global Navigation Satellite System (GNSS) using real-time dynamic differential positioning technology.

[0038] Optionally, all RTK data during the vehicle's operation can be acquired through the GNSS system in the vehicle. The timestamps in the RTK data are used to sort all RTK data according to time sequence and store them in a queue. Then, a general pose interpolation function is called to interpolate and calculate the corresponding RTK observation position from the RTK queue based on the timestamp of each keyframe pose. The determined RTK observation position is then associated with the corresponding keyframe pose, ensuring that each keyframe pose includes the corresponding global pose observation value. Specifically, among all RTK data, the first valid RTK position can be selected as the origin of the map coordinate system based on the time sequence of timestamps, and all subsequent RTK observation positions are represented relative to this origin.

[0039] Optionally, to facilitate the determination of the target map based on multiple keyframe poses, the original point cloud data of each keyframe pose can be temporarily saved to the disk in PCD format, thereby releasing the memory occupied by the original point cloud data and managing memory consumption. Then, the information of multiple keyframe poses and their pairs can be saved to a text file.

[0040] Step 220: Determine the first pose graph based on the multiple keyframe poses, and determine the edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose.

[0041] As an alternative approach, after obtaining multiple keyframe poses, the g2o (General Graph Optimization) optimizer can be used to optimize the multiple keyframe poses. This allows each keyframe to be added to the graph as a pose vertex, with its initial value set as the pose estimate of each keyframe pose, thus obtaining the first pose graph.

[0042] In some alternative embodiments, the trajectory output by the LIO may exhibit cumulative drift due to inherent limitations of the radar sensor, leading to cumulative drift in the resulting map. Therefore, to ensure the accuracy of the determined map, the cumulative drift of the LIO trajectory can be corrected using global observations acquired by GNSS.

[0043] Optionally, the first pose graph can be optimized by adding edge constraints to correct the cumulative drift of the LIO trajectory. The edge constraints can include edge constraints for GNSS absolute position and edge constraints for LIO relative motion. The edge constraints for GNSS absolute position are determined by the global pose observations of each keyframe pose, and the edge constraints for LIO relative motion are determined based on the pose estimation of each keyframe pose.

[0044] Step 230: Perform global optimization on the first pose graph according to the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the poses of the multiple keyframes.

[0045] As an alternative approach, since the g2o optimizer models the optimization problem as a graph problem, the optimization objective is to adjust the values ​​of all vertices in the graph (each vertex corresponds to a keyframe pose) to minimize the sum of errors of all edges. This allows for a global optimization solution to the first position graph, yielding the initial trajectory. Alternatively, since the g2o optimizer outputs a continuous pose sequence, concatenating this sequence yields the initial trajectory.

[0046] Step 240: Determine candidate keyframe pose pairs based on the initial trajectory, and determine loop closure constraints based on the candidate keyframe pose pairs.

[0047] As one approach, due to factors such as sensor noise, registration error, and IMU integration drift, the estimated pose of each keyframe determined from the raw point data and inertial measurement data will gradually deviate from the true trajectory over time. Furthermore, this error is cumulative, leading to misalignment or ghosting issues in the determined map after prolonged operation. Therefore, to ensure the accuracy of the determined map, loop closure constraints can be established, and these constraints can be used to determine the target map.

[0048] In some alternative embodiments, since loop closure constraints are used to indicate strong correlations between non-adjacent frames—essentially adding a closed-loop constraint to the graph—the optimizer is forced to adjust the entire trajectory to make the pose of the current frame consistent with that of historical frames, thereby eliminating drift. Therefore, candidate key pose pairs can be determined in the initial trajectory first, and these candidate key pose pairs can then be used to determine the loop closure constraints.

[0049] Optionally, keyframe pose pairs can be obtained by determining non-continuous keyframe pairs describing the same location in the initial trajectory. Then, the corresponding loop closure constraints can be determined based on the keyframe pose pairs, and the target map can be determined through the loop closure constraints.

[0050] Step 250: Determine the second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and perform global optimization on the second pose map based on the loop closure constraint and the edge constraint to determine the target keyframe pose.

[0051] As an alternative approach, after determining the loop closure constraints, a second pose graph can be determined based on the original point cloud data, inertial measurement data, and initial trajectory. Then, the corresponding loop closure constraints and edge constraints are used to perform global optimization on the second pose graph to achieve a second optimization. Finally, the target keyframe pose with global consistency is obtained.

[0052] In some alternative embodiments, since the initial trajectory is obtained after optimization based on edge constraints, a second pose map can be obtained by inputting the original point cloud data, inertial measurement data and the initial trajectory into the g2o optimizer again.

[0053] Optionally, since loop closure constraints provide strong geometric consistency information across long time intervals, the g2o optimizer will adjust the second pose graph based on edge constraints and loop closure constraints, thereby realigning poses or regions that have been misaligned due to accumulated errors in the early stages, and thus outputting the target keyframe pose with the best global consistency.

[0054] As an alternative approach, MAST3R-Fusion can be used to determine the second pose map based on the original point cloud data, inertial measurement data, and initial trajectory. The second pose map can then be globally optimized based on loop closure constraints and edge constraints to determine the target keyframe pose.

[0055] Step 260: The original point cloud data corresponding to the target keyframe pose is stitched and fused to obtain the target map.

[0056] As an alternative approach, after obtaining the target keyframe pose, the original point cloud corresponding to each target keyframe pose can be transformed to obtain the world point cloud in the world coordinate system. Then, the world point clouds corresponding to all target keyframe poses can be stitched and fused to obtain a complete, high-precision, and globally consistent point cloud map.

[0057] Optionally, to avoid the long acquisition time and large memory consumption caused by loading the complete point cloud map every time it is used, after the complete point cloud map is generated, it can be divided into blocks according to preset rules and stored. Then, it can be quickly responded to the point cloud map acquisition command by acquiring different map blocks.

[0058] In the embodiments of this application, multiple keyframe poses are first determined based on the acquired raw point cloud data and inertial measurement data. Then, a first position map is determined based on the multiple keyframe poses. This allows for the determination of the edge constraints of the first pose map based on the pose estimation of each keyframe pose and the global pose observation value of each keyframe pose. This enables the first round of optimization based on the mechanical energy of the edge constraints, thereby determining the initial trajectory. Then, candidate keyframe pose pairs are selected from the initial trajectory, and loop closure constraints are determined based on the candidate keyframe pose pairs. This enables the second round of optimization based on the loop closure constraints and edge constraints to obtain the target keyframe pose. Finally, the target map is determined based on the raw point cloud data corresponding to the target keyframe pose. This application designs a tightly coupled global optimization process that includes two stages: the first round of optimization through edge constraints and the second round of optimization combining edge constraints and loop closure constraints. It deeply integrates local continuous observation, global absolute observation, and cross-time period closed-loop observation information, making full use of offline computing resources for joint optimization. This effectively eliminates accumulated errors and obtains a point cloud map with optimal global consistency and higher accuracy. This solves the problems of insufficient absolute accuracy and scene integrity of point cloud maps caused by shallow multi-sensor fusion and insufficient global optimization in existing methods, as well as the problem of difficulty in ensuring global consistency of point cloud maps due to the separation of system error processing in existing methods.

[0059] Please see Figure 3 , Figure 3 This application illustrates a method for determining a map according to an embodiment of the present application. The following will focus on... Figure 3 The process shown is described in detail, and the method for determining the map may specifically include the following steps 310-370.

[0060] Step 310: Obtain raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation.

[0061] Step 320: Determine the first relative pose transformation relationship between adjacent keyframe poses in multiple keyframe poses based on the pose estimation of each keyframe pose, and determine the first edge constraint based on the first relative pose transformation relationship.

[0062] As an alternative approach, during the map determination process using data fusion from LiDAR sensors and inertial measurement units, LIO estimates the relative motion between adjacent frames at a high frequency, but this can lead to cumulative drift over long periods. Therefore, to reduce and eliminate this cumulative drift, the relative pose between adjacent keyframe positions can be added as an edge to the optimization. This forces the optimized global pose sequence to conform locally to the high-precision relative motion of LIO, thus ensuring the smoothness and accuracy of the optimized initial trajectory at the local scale.

[0063] In some optional embodiments, the Iterative Closest Point (ICP) algorithm can be used to iteratively find the minimum point-to-point distance between adjacent keyframe poses, thereby obtaining the first relative pose transformation relationship between adjacent key poses. Alternatively, the Normal Distributions Transform (NDT) algorithm can be used to divide the reference point cloud into a fixed-size voxel grid. Then, within each voxel, assuming the point cloud follows a Gaussian distribution, the mean and covariance of all points within that voxel are calculated to obtain the NDT representation of the reference point cloud. That is, each voxel is described by a Gaussian probability density function. Then, during registration, the key poses are transformed to the reference coordinate system. For each key pose, its probability density value in the Gaussian distribution of its respective voxel is calculated. The optimal transformation is estimated by maximizing the sum of the probability densities of all points, thereby obtaining the first relative pose transformation relationship. The first relative pose transformation relationship can be a transformation matrix, including a translation matrix and a rotation matrix, which can determine the pose transformation of adjacent keyframes from frame i+1 to frame i, that is, the coordinate transformation from coordinate system i to coordinate system i+1.

[0064] Optionally, in the g2o optimizer, each keyframe pose is used as a vertex, and the constraints between vertices can be used as binary edges between different keyframes to connect different vertices, thereby constraining the relative poses between adjacent keyframes.

[0065] Optionally, to ensure the accuracy of the determined target map, during the g2o optimizer's optimization of multiple keyframe poses, an information matrix between adjacent keyframe poses can be determined. This information matrix is ​​used to determine the relative pose relationship between adjacent keyframe positions, thereby bringing the determined first relative pose relationship closer to the actual value and reducing errors. The information matrix can be determined based on the estimated covariance of LIO, reflecting the confidence level of the constraints.

[0066] Step 330: Determine the second side constraint based on the global pose observation value of each keyframe pose.

[0067] As an alternative approach, since inertial measurement data has cumulative drift errors, in order to reduce or even eliminate these errors, GNSS information can be used to constrain the poses of multiple keyframes, thereby ensuring the accuracy of the determined target map.

[0068] In some alternative embodiments, in order to eliminate the cumulative drift of the odometry or laser inertial system and achieve global consistency, the translation position of the keyframe pose can be constrained by adding GNSS absolute position edges.

[0069] Optionally, the translation portion of the keyframe pose can be limited by using the global pose observations of each keyframe pose as unary edge constraints to obtain second-ary edge constraints. Here, the global pose observations are unknown in the RTK. Considering the installation extrinsic parameters between the GNSS antenna and the lidar (i.e., the pose relationship between the vehicle body and the GNSS), a position transformation can be performed when determining the error of each keyframe pose, i.e., determining the pose relationship between the vehicle body coordinate system and the GNSS coordinate system.

[0070] Step 340: Perform global optimization on the first pose graph based on the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the poses of the multiple keyframes.

[0071] In some embodiments, before step 350, the method further includes: obtaining the optimized residual corresponding to the global pose observation value of each keyframe pose; if it is determined that there is a reference keyframe pose whose optimized residual is greater than the residual threshold, then removing the global pose observation value corresponding to the reference keyframe pose from the initial trajectory and updating the second side constraint.

[0072] As an alternative approach, since RTK observation locations may exhibit certain anomalies due to various factors, in order to avoid the adverse effects of these anomalies on optimization and thus ensure the accuracy of the determined target map, RTK observations can be optimized and outliers removed. This ensures the accuracy of the determined second constraint edge and, consequently, the accuracy of the determined target map.

[0073] Optionally, a robust kernel function (such as the Cauchy kernel) can be set for the second constraint edge to suppress the adverse effects of possible aberrant RTK observations on the optimization. This allows the residuals of each RTK edge to be checked after the g2o optimizer performs the first graph optimization, thus filtering and updating the second constraint edge. To ensure the accuracy of the determined second constraint edge, all global pose observations can be filtered before determining the second constraint edge to identify valid global pose observations. Specifically, in the g2o optimizer, a valid identifier is set for each global pose observation. The second constraint edge is determined based on the global pose observations with valid identifiers among all global pose observations.

[0074] Optionally, during the optimization process, the g2o optimizer outputs the optimization residual of the keyframe pose corresponding to each second constraint edge, thereby updating the second constraint edge based on the optimization residual. Specifically, when the optimization residual is determined to be greater than a residual threshold, it indicates a large error between the position information of the keyframe pose and the position information of its corresponding global pose observation. To avoid large errors in the target map caused by determining the second constraint edge using RTK observations with large errors, after identifying a reference keyframe pose with an optimization residual greater than the residual threshold, the global pose observation corresponding to this reference keyframe pose is removed from the initial trajectory and also removed from the second constraint edge, thus updating the second constraint edge. This ensures that the second constraint edge using the global pose observation corresponding to the reference keyframe pose is not used in the second optimization.

[0075] After updating the second side constraint, the method further includes: determining an optimized pose for each keyframe pose among the plurality of keyframe poses based on the initial trajectory; determining a rotation matrix based on the optimized pose and real-time dynamic positioning observation data of each keyframe pose among the plurality of keyframe poses; aligning the initial trajectory according to the rotation matrix and updating the initial trajectory.

[0076] As an alternative approach, since the optimized initial trajectory may differ from the RTK trajectory by a rotation, after updating the second constraint edge by removing outliers, ICP matching is performed on the optimized poses of all keyframes and the corresponding valid real-time dynamic positioning observation data to estimate a globally optimal rotation matrix. This rotation is then applied to the poses of all keyframes to achieve the final alignment of the trajectory with the global coordinate system, thereby updating the initial trajectory.

[0077] In some optional embodiments, since the real-time dynamic positioning observation data (i.e., RTK position) only provides position and not attitude, the overall degrees of freedom of the optimized initial trajectory in rotation is not constrained. In order to ensure the accuracy of the determined initial trajectory, the rotation matrix can be determined by performing ICP matching between the real-time dynamic positioning observation data of each keyframe pose in multiple keyframe poses and the optimized pose in the optimized initial trajectory. Finally, the initial trajectory is updated by aligning the initial trajectory with the mechanical energy of the world coordinate system through the rotation matrix.

[0078] Step 350: Determine candidate keyframe pose pairs based on the initial trajectory, and determine loop closure constraints based on the candidate keyframe pose pairs.

[0079] Step 360: Determine the second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and perform global optimization on the second pose map based on the loop closure constraint and the edge constraint to determine the target keyframe pose.

[0080] Step 370: The original point cloud data corresponding to the target keyframe pose is stitched and fused to obtain the target map.

[0081] The specific steps of steps 310 and 340-370 can be found in steps 210 and 230-260, and will not be repeated here.

[0082] In this embodiment, the first round of optimization is performed by combining the first side constraint of LIO relative motion and the second side constraint of GNSS absolute position to obtain the initial trajectory, thereby eliminating the cumulative drift of the odometry or laser inertial system, achieving global consistency, and further improving the accuracy of the determined target map.

[0083] Please see Figure 4 , Figure 4 This application illustrates a method for determining a map according to an embodiment of the present application. The following will focus on... Figure 4 The process shown is described in detail. The method for determining the map may specifically include the following steps 410-490.

[0084] Step 410: Obtain raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation.

[0085] Step 420: Determine the first pose graph based on the multiple keyframe poses, and determine the edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose.

[0086] Step 430: Perform global optimization on the first pose graph according to the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the poses of the multiple keyframes.

[0087] Step 440: Determine candidate keyframe pose pairs based on the initial trajectory, and determine loop closure constraints based on the candidate keyframe pose pairs.

[0088] Step 450: Determine the second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and perform global optimization on the second pose map based on the loop closure constraint and the edge constraint to determine the target keyframe pose.

[0089] Step 460: The original point cloud data corresponding to the target keyframe pose is stitched and fused to obtain the target map.

[0090] The specific steps of steps 410-460 can be found in steps 210-260, and will not be repeated here.

[0091] Step 470: Sample the target map according to the preset voxel size to determine multiple sampling point clouds, and determine the grid index of each sampling point cloud among the multiple sampling points according to the preset map tile size.

[0092] As an alternative approach, current methods typically output a single, complete point cloud map file. However, real-time positioning systems, such as those for autonomous driving, require dynamically loading map data of the surrounding environment based on the vehicle's location. A single, large file is insufficient to efficiently support this partial loading and unloading requirement, impacting system efficiency and flexibility. Therefore, by storing the target map in chunks, and loading the corresponding map chunks for each use, the time required to load the complete point cloud map is reduced, improving system efficiency and flexibility.

[0093] Optionally, since the point cloud density corresponding to the original keyframe pose is extremely high, directly storing and using the target point cloud map would result in huge computational and memory overhead. In order to process the target map into blocks, the voxel size for sampling the target map can be preset. By sampling the target map with the preset voxel size, voxel filtering can be used to significantly reduce the point cloud density while preserving the overall geometric structure, thus balancing map accuracy and resource consumption.

[0094] Optionally, the unit space corresponding to the target map can be divided into cubic voxels of fixed size according to a preset voxel size, and then the point cloud of the centroid or center point in the cubic voxel can be determined as the sampled point cloud.

[0095] Optionally, after determining the sampled point cloud, the grid index of each sampled point cloud can be calculated according to the preset map tile size. For example, when the preset map tile size is a square with a side length of L, the grid index (gx, gy) of each sampled point cloud P(x, y, z) can be calculated using the formulas gx=floor((x+L / 2) / L) and gy=floor((y+L / 2) / L).

[0096] Step 480: Divide the target map into blocks according to the grid index to obtain multiple map blocks.

[0097] As an optional approach, after determining the grid index, the point cloud corresponding to each keyframe pose in the target map can be traversed to identify point clouds belonging to the same grid index. These point clouds are then stored in the same PCD file, resulting in multiple map tiles, each containing multiple point clouds belonging to the same tile. The PCD file can be saved in a binary compression format to reduce file size and improve dynamic loading speed.

[0098] Step 490: In response to the positioning command, determine the target map tile among the multiple map tiles and display the target map tile.

[0099] As an alternative approach, after the cloud server receives a positioning command from the vehicle or an electronic device connected to the vehicle, it can first determine the first map block containing the current location information and multiple adjacent map blocks associated with the first map block based on the vehicle's current location information in the positioning command. This yields the target map block, which is then sent to the vehicle-side server or the electronic device connected to the vehicle. The target map block can then be displayed on the vehicle's large screen via the vehicle-side server or on the display device of the electronic device.

[0100] In some alternative embodiments, in order to quickly identify the target map tile, after obtaining multiple map tiles, a text index file can be created first, and then the information of each map tile can be recorded in the text index file, thereby providing a quick lookup table for indexing to the actual data file.

[0101] Optionally, when generating the text index file, the pose range information of each map tile can be recorded. This pose range information can be determined based on the point cloud in each map tile, thereby determining the range to which the current location information belongs to locate the target map tile.

[0102] In this embodiment, multiple sampling point clouds are determined by sampling the target map according to a preset voxel size. Then, the grid index of each sampling point cloud in the multiple sampling points is determined according to a preset map tile size. In this way, the target map can be divided and stored in blocks according to the grid index to obtain multiple map tiles. Then, in response to the positioning command, the corresponding target map tile can be quickly determined through the multiple map tiles stored in blocks for rapid response. This solves the dilemma that the existing generated map data has a single organization form and is difficult to support the dynamic loading requirements of real-time positioning systems.

[0103] Please see Figure 5 , Figure 5 This application illustrates a method for determining a map according to an embodiment of the present application. The following will focus on... Figure 5 The process shown is described in detail. The method for determining the map may specifically include the following steps 510-570.

[0104] Step 510: Obtain raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation.

[0105] Step 520: Determine the first pose graph based on the multiple keyframe poses, and determine the edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose.

[0106] Step 530: Perform global optimization on the first pose graph according to the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the multiple keyframe poses.

[0107] Step 540: Traverse the multiple keyframe poses in the initial trajectory to determine the distance and time interval between the multiple keyframe poses.

[0108] As an alternative approach, loop closure constraints are crucial for eliminating accumulated drift and ensuring global consistency. However, not any two frames can serve as loop closure candidates; rigorous screening is necessary to ensure that the added loop closure constraints are correct, effective, and beneficial to optimization. Therefore, the poses of multiple keyframes can be filtered based on the distance and time interval between different keyframes.

[0109] Optionally, the distance between different keyframe poses can be determined by determining the Euclidean distance between different keyframe poses in three-dimensional space, and the time interval between different keyframe poses can be determined by determining the timestamps corresponding to different keyframe poses.

[0110] Step 550: Among the multiple keyframe poses, the keyframe pose pairs with a distance less than a distance threshold and a time interval greater than a duration threshold are determined as candidate keyframe pose pairs, and loop closure constraints are determined based on the candidate keyframe pose pairs.

[0111] As an alternative approach, since loop closure constraints essentially correct poses that should overlap after a long period of drift, if two frames are spatially far apart, they shouldn't overlap in the first place, and forced constraints would introduce errors. Only when they are spatially close enough can true environmental overlap exist. Therefore, the distance between different keyframe poses can be determined to filter multiple keyframe poses and thus determine the loop closure constraints.

[0112] As an alternative approach, since the relative motion between adjacent keyframe poses is already covered by edge constraints, there is no need to repeatedly add loop closure edges. The purpose of loop closure constraints is to correct long-term drift; if the time interval is too short and the accumulated error is small, adding loop closure edges may introduce unnecessary constraints, leading to an ill-conditioned optimization problem. Therefore, the time interval between different keyframe poses can be determined to filter multiple keyframe poses, thereby determining the loop closure constraints.

[0113] In some optional embodiments, in order to ensure the accuracy of the determined loop closure constraints, a distance threshold and a duration threshold can be preset. By comparing the distance between different keyframe poses with the distance threshold and the time interval between different keyframe poses with the duration threshold, multiple keyframe poses can be selected to determine candidate keyframe poses, thereby ensuring the accuracy of the determined loop closure constraints.

[0114] Step 560: Determine the second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and perform global optimization on the second pose map based on the loop closure constraint and the edge constraint to determine the target keyframe pose.

[0115] Step 570: The original point cloud data corresponding to the target keyframe pose is stitched and fused to obtain the target map.

[0116] The specific steps of steps 510-530 and 560-570 can be found in steps 210-230 and 250-260, and will not be repeated here.

[0117] In this embodiment, multiple keyframe poses can be traversed in the initial trajectory to determine the distance and time interval between the multiple keyframe poses. In this way, among the multiple keyframe poses, keyframe pose pairs with a distance less than a distance threshold and a time interval greater than a duration threshold are identified as candidate keyframe pose pairs. This enables the determination of more accurate loop closure constraints and further improves the accuracy of the determined target map.

[0118] Please see Figure 6 , Figure 6 This application illustrates a method for determining a map according to an embodiment of the present application. The following will focus on... Figure 6 The process shown is described in detail. For each pair of candidate keyframe poses, the method for determining the map may specifically include the following steps 610-680.

[0119] Step 610: Obtain raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation.

[0120] Step 620: Determine the first pose graph based on the multiple keyframe poses, and determine the edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose.

[0121] Step 630: Perform global optimization on the first pose graph according to the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the poses of the multiple keyframes.

[0122] Step 640: Determine candidate keyframe pose pairs based on the initial trajectory, determine multiple adjacent keyframe poses of the first candidate keyframe pose in the candidate keyframe pose pairs, and determine a local point cloud map based on the multiple adjacent keyframe poses.

[0123] As an alternative approach, after determining multiple candidate keyframe pose pairs, since these pairs may contain false positives (caused by perceptual aliasing), more rigorous geometric registration is needed to confirm their physical consistency. Therefore, the pose of the first candidate keyframe in each pair can be used as the center to determine multiple adjacent keyframe poses, constructing a local point cloud map of a locally dense map. This provides richer geometric constraints, resulting in more accurate geometric registration. Since the registration of each candidate keyframe pose pair is independent, multi-threaded concurrent processing can significantly improve verification efficiency for multiple candidate keyframe pose pairs.

[0124] In some optional embodiments, the determined local point cloud map can be ensured to cover a sufficient spatial range by taking the candidate key frame poses of preset frames forward and backward, with the first candidate key frame pose as the center (for example, the first candidate key frame pose may be located at the center of the local point cloud map).

[0125] Step 650: Determine the single-frame scan point cloud of the second candidate keyframe pose in the candidate keyframe pose pair, and register the single-frame scan point cloud with the local point cloud map to obtain the second relative pose transformation relationship and registration score.

[0126] As an alternative approach, after determining the local point cloud map, since the local point cloud map represents the local environment near the first candidate keyframe pose, while the single-frame scan point cloud of the second candidate keyframe pose is the query frame to be registered, the information content is relatively sparse, but it retains the original observation from the current viewpoint. Therefore, by registering the second candidate keyframe pose with the local point cloud map near the first candidate keyframe pose, the precise relative pose of the second candidate keyframe pose with respect to the first candidate keyframe pose can be estimated.

[0127] In some optional embodiments, after determining the single-frame scan point cloud of the second candidate keyframe pose, the single-frame scan point cloud can be registered with the local point cloud map by using the NDT registration method, thereby obtaining a more accurate second relative pose transformation relationship and registration score of the second candidate keyframe pose relative to the first candidate keyframe pose.

[0128] Optionally, when registering a single-frame scanned point cloud with a local point cloud map, a coarse-to-fine strategy is adopted to improve the registration success rate and accuracy. This is achieved by setting the voxel resolution of the NDT from coarse to fine. For example, the grid resolution of the NDT is set sequentially to 10.0 meters, 5.0 meters, 4.0 meters, and 3.0 meters. This allows for rapid convergence to a rough transformation using a coarse resolution, avoiding getting trapped in local optima. Then, the resolution is gradually refined, using finer voxels to improve registration accuracy based on the coarse result. In the first registration process, the original pose of the single-frame scanned point cloud can be used for registration. In the coarse-to-fine registration process, each registration uses the output of the previous registration as the input for the next registration.

[0129] Optionally, the registration score can be the transformation probability of NDT or the likelihood value of the point cloud distribution after registration. The higher the score, the better the match. The score is output through the NDT algorithm during the registration process.

[0130] Step 660: If it is determined that the registration score is greater than the score threshold, then the second relative pose transformation relationship is determined as the loop constraint of the candidate keyframe pose pair.

[0131] As one approach, if a candidate keyframe pose pair has a registration score greater than a score threshold, it can be determined that the loop closure detection is successful. At this point, the second relative pose transformation relationship of the candidate keyframe pose pair with a registration score greater than the score threshold can be determined as the loop closure constraint of the pose pair. This allows for a second optimization based on the loop closure constraints of all candidate keyframe pose pairs to determine the target map, thus ensuring the accuracy of the determined target map.

[0132] Step 670: Determine the second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and perform global optimization on the second pose map based on the loop closure constraint and the edge constraint to determine the target keyframe pose.

[0133] Step 680: The original point cloud data corresponding to the target keyframe pose is stitched and fused to obtain the target map.

[0134] The specific steps of steps 610-630 and 670-680 can be found in steps 210-230 and 250-260, and will not be repeated here.

[0135] In this embodiment, firstly, multiple adjacent keyframe poses of the first candidate keyframe pose in the candidate keyframe pose pair are determined, and a local point cloud map is determined based on the multiple adjacent keyframe poses. Then, a single-frame scan point cloud of the second candidate keyframe pose in the candidate keyframe pose pair is determined, and the single-frame scan point cloud is registered with the local point cloud map to obtain a second relative pose transformation relationship and a registration score. Finally, if the registration score is determined to be greater than a score threshold, the second relative pose transformation relationship is determined as the loop closure constraint of the candidate keyframe pose pair, ensuring the effectiveness and accuracy of the determined loop closure constraint, and further improving the accuracy of the determined target map.

[0136] Figure 7 The method for determining a map is based on an embodiment of this application, such as... Figure 7 As shown, data acquisition and front-end odometry can be performed first to generate multiple keyframe poses and keyframe pose information. The keyframe pose information includes the timestamp, unique ID, pose estimation, and corresponding original LiDAR point cloud scan for each keyframe pose. First, all RTK observation data can be extracted and stored in a queue, sorted by timestamp. The first valid RTK position is used as the origin of the map coordinate system, and all subsequent RTK observation positions are represented relative to this origin. Then, LiDAR point cloud and IMU data are read synchronously. The LiDAR point cloud and IMU data are input into a LiDAR inertial odometry (LIO) system based on Iterative Error State Kalman Filter (IEKF). Next, from the continuous pose sequence output by the LIO, multiple keyframe poses are determined based on distance and angle thresholds. For example, the distance threshold can be set to 1.5 meters and the angle threshold to 15 degrees. When the relative translation between the current frame and the previous keyframe in the multiple point cloud data exceeds 1.5 meters or the relative rotation exceeds 15 degrees, the current frame point cloud is determined as the new keyframe pose. Then, for each keyframe pose, a timestamp, a unique ID, the pose estimate provided by LIO at that moment, and the corresponding original LiDAR point cloud scan are recorded. Simultaneously, a general pose interpolation function is invoked to interpolate and calculate the corresponding RTK observation position from the RTK queue based on the keyframe timestamp and associate it with the keyframe pose. Finally, the point cloud data for each keyframe is temporarily saved to disk in PCD format and released from memory to manage memory consumption. All keyframe pose information is saved to a text file.

[0137] Next, a first round of global optimization is performed based on the determined keyframe poses. The main purpose is to use GNSS global observations to correct the cumulative drift of the LIO trajectory. First, a g2o optimization is initialized, adding each keyframe pose as a pose vertex to the graph. The initial values ​​are set with the pose estimates from the keyframe pose information corresponding to multiple keyframe poses, thus constructing a pose graph. Then, constraint edges for LIO relative motion and GNSS absolute position are added. A robust kernel function is then used to optimize the constraint edges and remove outliers to obtain the initial trajectory. Finally, the initial trajectory is rotated and aligned to obtain the reference trajectory. The LIO relative motion edge can be calculated for all consecutive keyframe pairs (i, i+1), using the relative pose transformation given by LIO as the observation value, and adding an EdgeRelativeMotion binary edge between vertex i and vertex i+1. The information matrix of this EdgeRelativeMotion binary edge is set according to the estimated covariance of LIO to reflect the confidence of the constraint. For each keyframe pose j, if its associated RTK observation is valid, an EdgeGNSSTransOnly unary edge is added to vertex j using its RTK position P_rtk_j as the observation value. This edge only constrains the translation (x, y, z) of the pose vertex.

[0138] Optionally, during optimization and outlier removal, a robust kernel function (such as the Cauchy kernel) can be set for the RTK edges to suppress the adverse effects of possible outlier RTK observations on the optimization, and then the first graph optimization is performed. After optimization, the residuals of each RTK edge are checked. If the residual exceeds a preset threshold (e.g., 3 meters), the RTK observation of that keyframe is determined to be invalid in this round of optimization, and the RTK edge is removed or its weight is significantly reduced to obtain the initial trajectory; if the residual is less than or equal to the preset threshold, it is retained as a valid constraint.

[0139] Optionally, after obtaining the initial trajectory, since only positional RTK data is used, the optimized initial trajectory may differ from the overall RTK trajectory by one rotation. Therefore, after obtaining the initial trajectory, ICP matching is performed on the optimized poses of all keyframes in the initial trajectory with the corresponding valid RTK positions to estimate a globally optimal rotation matrix. This rotation is then applied to all keyframe poses to complete the final alignment of the trajectory with the global coordinate system, thus obtaining the reference trajectory.

[0140] Then, loop closure constraints are added between keyframe pairs that are not contiguous in time but describe the same location to eliminate accumulated errors. This can be done by first traversing all keyframes. For keyframe A, search for keyframe B whose spatial distance is less than a threshold but whose temporal ID interval is greater than a threshold, and mark the (A,B) pair as a candidate keyframe pair. To avoid excessive computation, an interval sampling strategy can be used, for example, checking every 5 keyframes.

[0141] Next, concurrent registration verification is performed. Since the registration calculations for each candidate pair are independent, this step uses concurrent programming and multi-threading to process all candidate pairs. Therefore, for each candidate pair (A, B), a local sub-map is first constructed, and multi-resolution NDT registration is performed on this local sub-map. The loop closure constraint is then determined based on the registration score. Specifically, the optimized pose of keyframe A is used as the center, and point clouds of N (e.g., N=40) adjacent keyframe poses are selected. These point clouds are transformed to the world coordinate system using their corresponding pose trajectories, and then stitched together to form a dense local point cloud sub-map Map_A. Then, the single-frame point cloud Scan_B corresponding to keyframe pose B is loaded, and finally, the Normal Distribution Transform (NDT) algorithm is used to register Scan_B to Map_A.

[0142] Optionally, to improve registration success rate and accuracy, a coarse-to-fine strategy is adopted, setting the grid resolution of NDT to 10.0 meters, 5.0 meters, 4.0 meters, and 3.0 meters sequentially. The initial pose is set to the pose estimate corresponding to the keyframe pose B. After registration is completed at each resolution level, the output transformation matrix T_A_B is used as the initial value for the next level of finer registration.

[0143] Finally, after registration at the highest resolution (3.0 meters), the final relative pose transformation and a matching score (such as the transformation probability of NDT) are obtained. If the matching score is less than a preset threshold, the transformation matrix is ​​discarded; if the matching score is greater than or equal to the preset threshold (such as 0.5), the loop closure detection is considered successful, and the transformation matrix T_A_B is recorded as a loop closure constraint between keyframes A and B. Finally, all successful loop closure constraints (including keyframe ID pairs and relative pose T_A_B) are saved to a file.

[0144] After determining the loop closure constraints, the loop closure constraints and edge constraints are fused for global consistency optimization and map generation, achieving the second round of optimization. In the second round of optimization, a new pose graph is first constructed based on multiple keyframe poses and their information. The vertex initialization values ​​are set to the pose estimates corresponding to the respective keyframe poses. Then, all edge constraints and all valid loop closure constraint edges are used as target constraints to perform the second round of global graph optimization. Since the loop closure constraints provide strong geometric consistency information across long time intervals, the optimizer adjusts the entire trajectory accordingly, realigning map regions that have been misaligned due to accumulated errors. After optimization, the globally consistent optimal keyframe pose sequence is obtained. Then, using the final keyframe pose sequence, the original point cloud corresponding to each keyframe (reloaded from disk) is transformed to the world coordinate system, and all point clouds are stitched and fused to generate a complete, high-precision, globally consistent point cloud map file.

[0145] Finally, to make the generated map suitable for real-time positioning systems, post-processing and tile export can be performed. First, the complete map point cloud is downsampled using a voxel grid filter, setting the voxel size (e.g., 0.1 meters). Then, the size of the map tiles is defined. For each downsampled point, its corresponding grid index is calculated, and a dedicated map data directory is created. All points are traversed, and point clouds belonging to the same index are stored in the same PCD file. Finally, a text index file is created to record information for each map tile, providing a fast lookup table from the grid index to the actual data file.

[0146] The above embodiments describe in detail the map determination method provided by the embodiments of this application. In other embodiments, this application also provides a map determination apparatus. Figure 8 This is a block diagram of a map determining device according to an embodiment of this application, such as... Figure 8 As shown, the map determination device 700 includes: an acquisition module 710, an edge constraint determination module 720, a first optimization module 730, a lap constraint determination module 740, a second optimization module 750, and a target map determination module 760.

[0147] The acquisition module 710 is used to acquire raw point cloud data and inertial measurement data, and determine multiple keyframe poses based on the raw point cloud data and the inertial measurement data, wherein each keyframe pose includes pose estimation and global pose observation; the edge constraint determination module 720 is used to determine a first pose graph based on the multiple keyframe poses, and determine the edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose; the first optimization module 730 is used to perform global optimization on the first pose graph based on the edge constraints to determine an initial trajectory, wherein the initial... The initial trajectory indicates the pose trajectory after connecting the multiple keyframe poses; the loop closure constraint determination module 740 is used to determine candidate keyframe pose pairs based on the initial trajectory, and determine loop closure constraints based on the candidate keyframe pose pairs; the second optimization module 750 is used to determine a second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and perform global optimization on the second pose map based on the loop closure constraints and the edge constraints to determine the target keyframe pose; the target map determination module 760 is used to stitch and fuse the original point cloud data corresponding to the target keyframe pose to obtain the target map.

[0148] In some embodiments, the edge constraint determination module 720 includes: a first edge constraint determination unit, configured to determine a first relative pose transformation relationship between adjacent keyframe poses in a plurality of keyframe poses based on the pose estimation of each keyframe pose, and to determine a first edge constraint based on the first relative pose transformation relationship; and a second edge constraint determination unit, configured to determine a second edge constraint based on the global pose observation value of each keyframe pose.

[0149] In some embodiments, the map determining device 700 further includes: an optimized residual acquisition module, configured to acquire the optimized residual corresponding to the global pose observation value of each keyframe pose; and a first update module, configured to remove the global pose observation value corresponding to the reference keyframe pose from the initial trajectory and update the second edge constraint if it is determined that there is a reference keyframe pose with the optimized residual greater than the residual threshold.

[0150] In other embodiments, the map determining device 700 further includes: an optimized pose determining module, configured to determine an optimized pose for each keyframe pose among the plurality of keyframe poses based on the initial trajectory; a rotation matrix determining module, configured to determine a rotation matrix based on the optimized pose and real-time dynamic positioning observation data of each keyframe pose among the plurality of keyframe poses; and a second updating module, configured to align the initial trajectory according to the rotation matrix and update the initial trajectory.

[0151] In some other embodiments, the map determining device 700 further includes: a grid index determining module, used to sample the target map according to a preset voxel size, determine multiple sampled point clouds, and determine the grid index of each sampled point cloud among the multiple sampled points according to a preset map tile size; a map tile determining module, used to divide the target map into blocks according to the grid index to obtain multiple map tiles; and a display module, used to respond to a positioning command, determine the target map tile among the multiple map tiles, and display the target map tile.

[0152] In some embodiments, the loop constraint determination module 740 includes: a first determination unit, configured to traverse the plurality of keyframe poses in the initial trajectory and determine the distance and time interval between the plurality of keyframe poses; and a candidate keyframe pose determination unit, configured to determine, among the plurality of keyframe poses, keyframe pose pairs whose distance is less than a distance threshold and whose time interval is greater than a duration threshold as candidate keyframe pose pairs.

[0153] In some embodiments, for each pair of candidate keyframe poses, the loop closure constraint determination module 740 further includes: a local point cloud map determination unit, configured to determine multiple adjacent keyframe poses of the first candidate keyframe pose in the candidate keyframe pose pair, and determine a local point cloud map based on the multiple adjacent keyframe poses; a registration unit, configured to determine a single-frame scan point cloud of the second candidate keyframe pose in the candidate keyframe pose pair, and register the single-frame scan point cloud with the local point cloud map to obtain a second relative pose transformation relationship and a registration score; and a loop closure constraint determination unit, configured to determine the second relative pose transformation relationship as the loop closure constraint of the candidate keyframe pose pair if the registration score is determined to be greater than a score threshold.

[0154] According to one aspect of the embodiments of this application, an electronic device is also provided, such as... Figure 9 As shown, the electronic device 800 also includes a processor 810 and one or more memories 820. The one or more memories 820 are used to store program instructions executed by the processor 810. When the processor 810 executes the program instructions, it implements the map determination method described above.

[0155] Furthermore, the processor 810 may include one or more processing cores. The processor 810 runs or executes instructions, programs, code sets, or instruction sets stored in the memory 820, and retrieves data stored in the memory 820. Optionally, the processor 810 may be implemented using at least one hardware form selected from Digital Signal Processing (DSP), Field-Programmable Gate Array (FPGA), and Programmable Logic Array (PLA). The processor 810 may integrate one or a combination of several of the following: Central Processing Unit (CPU), Graphics Processing Unit (GPU), and modem. The CPU primarily handles the operating system, user interface, and applications; the GPU is responsible for rendering and drawing the displayed content; and the modem handles wireless communication. It is understood that the modem may also not be integrated into the processor and may be implemented using a separate communication chip.

[0156] According to one aspect of this application, a computer-readable storage medium is also provided, which may be included in the cloud server described in the above embodiments; or it may exist independently and not assembled into the cloud server. The aforementioned computer-readable storage medium carries computer-readable instructions that, when executed by a processor, implement the methods in any of the above embodiments.

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

[0158] The units described in the embodiments of this application can be implemented in software or hardware, and the described units can also be located in a processor. The names of these units do not necessarily limit the specific unit itself.

[0159] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the embodiments disclosed herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

[0160] It should be understood that this application is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of this application is limited only by the appended claims.

Claims

1. A method of determining a map, characterized by, The method includes: Acquire raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation. The first pose graph is determined based on the multiple keyframe poses, and the edge constraints of the first pose graph are determined based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose. The first pose graph is globally optimized based on the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the multiple keyframe poses. Candidate keyframe pose pairs are determined based on the initial trajectory, and loop closure constraints are determined based on the candidate keyframe pose pairs. The second pose map is determined based on the original point cloud data, the inertial measurement data and the initial trajectory, and the second pose map is globally optimized based on the loop closure constraint and the edge constraint to determine the target keyframe pose; The target map is obtained by stitching and fusing the original point cloud data corresponding to the pose of the target keyframe.

2. The method of claim 1, wherein, The step of determining the first pose graph based on the multiple keyframe poses, and determining the edge constraints of the first pose graph based on the pose estimation of each keyframe pose and the global pose observation of each keyframe pose, includes: Based on the pose estimation of each keyframe pose, a first relative pose transformation relationship between adjacent keyframe poses in multiple keyframe poses is determined, and a first edge constraint is determined based on the first relative pose transformation relationship. The second side constraint is determined based on the global pose observations of each keyframe pose.

3. The method of claim 2, wherein, Before determining candidate keyframe pose pairs based on the initial trajectory and determining loop closure constraints based on the candidate keyframe pose pairs, the method further includes: Obtain the optimized residual corresponding to the global pose observation value of each keyframe pose; If it is determined that there exists a reference keyframe pose with an optimized residual greater than the residual threshold, then the global pose observation corresponding to the reference keyframe pose is removed from the initial trajectory, and the second side constraint is updated.

4. The method of claim 3, wherein, After updating the second edge constraint, the method further includes: The optimized pose of each keyframe pose in the plurality of keyframe poses is determined based on the initial trajectory. The rotation matrix is ​​determined based on the optimized pose and the real-time dynamic positioning observation data of each keyframe pose in the plurality of keyframe poses; The initial trajectory is aligned according to the rotation matrix, and the initial trajectory is updated.

5. The method according to any one of claims 1 to 4, characterized in that, After stitching and fusing the original point cloud data corresponding to the target keyframe pose to obtain the target map, the method further includes: The target map is sampled according to a preset voxel size to determine multiple sampled point clouds, and the grid index of each sampled point cloud in the multiple sampled points is determined according to a preset map tile size. The target map is divided into blocks and stored according to the grid index, resulting in multiple map blocks; In response to a positioning command, a target map tile is identified among the plurality of map tiles, and the target map tile is displayed.

6. The method according to any one of claims 1-4, characterized in that, The step of determining candidate keyframe pose pairs based on the initial trajectory includes: Traverse the multiple keyframe poses in the initial trajectory to determine the distance and time interval between the multiple keyframe poses; Among the multiple keyframe poses, the keyframe pose pairs whose distance is less than a distance threshold and whose time interval is greater than a duration threshold are determined as the candidate keyframe pose pairs.

7. The method according to any one of claims 1-4, characterized in that, For each pair of candidate keyframe poses, determining the loop closure constraint based on the candidate keyframe poses includes: Determine multiple adjacent keyframe poses of the first candidate keyframe pose in the candidate keyframe pose pair, and determine a local point cloud map based on the multiple adjacent keyframe poses. Determine the single-frame scan point cloud of the second candidate keyframe pose in the candidate keyframe pose pair, and register the single-frame scan point cloud with the local point cloud map to obtain the second relative pose transformation relationship and registration score. If the registration score is determined to be greater than the score threshold, then the second relative pose transformation relationship is determined as the loop constraint of the candidate keyframe pose pair.

8. A map determining device, characterized in that, The device includes: The acquisition module is used to acquire raw point cloud data and inertial measurement data, and determine multiple key frame poses based on the raw point cloud data and the inertial measurement data, wherein each key frame pose includes pose estimation and global pose observation. The edge constraint determination module is used to determine the first pose graph based on the multiple key frame poses, and to determine the edge constraints of the first pose graph based on the pose estimation of each key frame pose and the global pose observation of each key frame pose. The first optimization module is used to perform global optimization on the first pose graph according to the edge constraints to determine the initial trajectory, wherein the initial trajectory indicates the pose trajectory after connecting the multiple keyframe poses. The loop closure constraint determination module is used to determine candidate keyframe pose pairs based on the initial trajectory, and to determine loop closure constraints based on the candidate keyframe pose pairs. The second optimization module is used to determine the second pose map based on the original point cloud data, the inertial measurement data and the initial trajectory, and to perform global optimization on the second pose map based on the loop closure constraint and the edge constraint to determine the target keyframe pose; The target map determination module is used to stitch and fuse the original point cloud data corresponding to the pose of the target keyframe to obtain the target map.

9. An electronic device, characterized in that, The electronic device includes: processor; A memory, wherein computer-readable instructions are stored thereon, which, when executed by the processor, implement the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium contains program code that can be invoked by a processor to execute the method as described in any one of claims 1 to 7.

Images