CN116737851A - Storage and updating method of node type point cloud map - Google Patents
Storage and updating method of node type point cloud map Download PDFInfo
- Publication number
- CN116737851A CN116737851A CN202210207911.6A CN202210207911A CN116737851A CN 116737851 A CN116737851 A CN 116737851A CN 202210207911 A CN202210207911 A CN 202210207911A CN 116737851 A CN116737851 A CN 116737851A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- key frame
- map
- nodes
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000005457 optimization Methods 0.000 claims description 20
- 238000005516 engineering process Methods 0.000 description 5
- 238000013507 mapping Methods 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000003780 insertion Methods 0.000 description 1
- 230000037431 insertion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/22—Indexing; Data structures therefor; Storage structures
- G06F16/2228—Indexing structures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/23—Updating
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/24—Querying
- G06F16/245—Query processing
- G06F16/2453—Query optimisation
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Data Mining & Analysis (AREA)
- Physics & Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Software Systems (AREA)
- Computational Linguistics (AREA)
- Processing Or Creating Images (AREA)
Abstract
The invention provides a storage and update method of a node type point cloud map. The storage method of the node type point cloud map comprises the following steps: acquiring pose information and point cloud data of each key frame in a key frame sequence; acquiring tile numbers corresponding to pose information of each key frame in the key frame sequence under an earth projection plane coordinate system; and forming a node by pose information, point cloud data and corresponding tile numbers of each key frame in the key frame sequence, and storing all nodes formed by the key frame sequence by taking the tile numbers as indexes to form a point cloud map. The method and the system can meet the requirements of rapid indexing of the scene point cloud map and the integrity of the point cloud data.
Description
Technical Field
The invention relates to the technical field of digital application, in particular to a method for storing and updating a node type point cloud map.
Background
The vehicle self-positioning technology is an important part of the technical field of automatic driving, reliable and accurate vehicle self-positioning in a large-scale scene generally needs to rely on multi-source information such as a laser radar, an IMU, a GNSS, a scene map and the like, wherein the laser radar-based instant positioning and mapping technology (simultaneous localization and mapping, SLAM) is an effective method for constructing the scene map, and can finish scene point cloud map construction of a single acquisition area.
However, the scene point cloud map area constructed by the instant positioning and mapping technology is limited, and the problems of distortion, distortion and the like exist in the process of constructing a large-area map once, so that the accuracy of the constructed scene map is affected, and the scene map expansion is often required to be realized by adopting a splicing updating mode; in practical applications, the scene environment is changed due to engineering such as road finishing, and the scene map also needs to be updated in a local area.
Because the data volume of map sequence point cloud data generated by single application of the laser radar-based instant positioning and mapping technology is huge, the data volume is stored in a time sequence, and the loading speed is limited during application, and the requirement of quick index of a scene map can not be met, the loading, splicing and updating of a large-scale point cloud map are still a difficult problem in practical map application.
In the prior art, the point cloud map is compressed or the storage scale of the point cloud map is reduced by projecting the point cloud map into a planar grid map, so that the problem that the loading speed and the operation speed in the actual map application are not satisfied is solved, and the original point cloud information is lost in the mode. For example, a real-time grid map generation method for a large scene is disclosed in chinese patent application publication No. CN113034687a, with application date 2021, 3, 29. The method comprises the steps of generating and splicing a large scene map, projecting a three-dimensional point cloud into a two-dimensional grid map through a likelihood field model, matching in the grid map, determining the accumulated deviation between a newly built sub-map and an existing map, correcting pose information of each key frame, and realizing the splicing of the newly built sub-map and a historical map. However, the map stored in the method is a grid map, only the height and reflectivity parameter information in the likelihood field model is stored, and the three-dimensional space structure information of the original point cloud is lost, so that the storage mode of the scene map can realize positioning only by adapting to a specific grid map-based matching algorithm.
Disclosure of Invention
The technical problem to be solved by the embodiment of the invention is to provide a method for storing and updating a node type point cloud map, which can meet the requirements of rapid indexing of the scene point cloud map and the integrity of point cloud data.
In order to solve the technical problems, the invention provides a method for storing a node type point cloud map, which comprises the following steps:
acquiring pose information and point cloud data of each key frame in a key frame sequence;
acquiring corresponding tile numbers of pose information of each key frame in the key frame sequence under an earth projection plane coordinate system;
and forming a node by pose information, point cloud data and corresponding tile numbers of each key frame in the key frame sequence, and storing all nodes formed by the key frame sequence by taking the tile numbers as indexes to form the point cloud map.
Further, the point cloud data of each key frame comprises the complete point cloud data in the key frame or comprises the point cloud data corresponding to the selected feature point in the key frame.
Further, the point cloud data of each key frame comprises point cloud data corresponding to selected feature points in the key frames, wherein the selected feature points correspond to feature points required by a map matching algorithm adopted by vehicle self-positioning.
Further, the earth projection plane coordinate system is a UTM coordinate system.
The embodiment of the invention also provides a method for updating the node type point cloud map, wherein the initial point cloud map of the point cloud map is stored according to the storage method, and the method comprises the following steps:
acquiring pose information and point cloud data of each key frame in the new key frame sequence;
acquiring corresponding tile numbers of pose information of each key frame in the new key frame sequence under an earth projection plane coordinate system;
traversing all key frames in the new key frame sequence, searching adjacent nodes in the point cloud map based on tile numbers corresponding to the key frames for each key frame, determining loop-back nodes matched with the key frames in the point cloud map, determining relative pose between the key frames and the loop-back nodes matched with the key frames to establish pose constraint, and determining pose information of the key frames in the point cloud map according to pose information of the loop-back nodes;
determining associated map nodes of all key frames in the new key frame sequence in the point cloud map, and determining relative pose between the associated map nodes and the loop link nodes to establish pose constraint;
according to pose constraints among key frames in the new key frame sequence, pose constraints among key frames in the new key frame sequence and the matched loop link nodes, and pose constraints among the associated map nodes and the corresponding loop link nodes, carrying out pose map optimization, and determining pose change amounts of the associated map nodes and pose change amounts of key frames in the new key frame sequence;
determining pose information of the associated map nodes after optimization according to the pose change quantity of the associated map nodes, and obtaining new tile numbers corresponding to the pose information of the associated map nodes after optimization under an earth projection plane coordinate system; updating pose information of each key frame in the point cloud map to be optimized pose information according to pose change quantity of each key frame in the new key frame sequence, and obtaining new tile numbers corresponding to the pose information of each key frame after optimization under an earth projection plane coordinate system;
updating the point cloud map according to the pose information after the associated map node optimization and the corresponding new tile number;
deleting nodes in which node areas in the updated point cloud map overlap with areas determined by the pose information after optimizing the key frames;
and forming a new node by the pose information, the point cloud data and the corresponding new tile number after optimizing each key frame in the new key frame sequence, and storing all new nodes formed by the new key frame sequence into the updated point cloud map by taking the new tile number as an index.
Further, the determining a loop node in the point cloud map, which is matched with the key frame, specifically includes:
converting the searched point cloud data of the adjacent nodes into the same space coordinate system through corresponding pose information to form a sub-map;
and determining loop-back nodes matched with the key frames in the sub-map through point cloud registration based on the point cloud data of the key frames and the point cloud data of the adjacent nodes.
Further, determining pose information of the key frame in the point cloud map according to pose information of the loop-back node specifically includes:
and determining pose information of the key frame in the sub map according to the pose information of the loop-back node.
Further, determining associated map nodes of all key frames in the new key frame sequence in the point cloud map, and determining relative pose between the associated map nodes and the loop-back link nodes to establish pose constraint, specifically including:
searching nodes in all loop-back link node adjacent areas corresponding to all key frames in the new key frame sequence in the point cloud map as associated map nodes;
and determining the relative pose between the associated map node and the corresponding loop link node to establish pose constraint.
Further, the deleting the node in the updated point cloud map, where the node area overlaps with the area determined by the pose information after optimizing the key frames, specifically includes:
searching nodes with similar distances to each key frame in the updated point cloud map according to new tile numbers corresponding to the pose information after the key frames are optimized in the new key frame sequence;
judging whether a node area determined by the nodes with similar distances is overlapped with an area determined by pose information after optimizing the corresponding key frames;
and deleting the nodes with overlapping node areas in the nodes with similar distances and the areas determined by the pose information after the optimization of the corresponding key frames.
The embodiment of the invention has the following beneficial effects: the embodiment of the invention solves the problems of generating, loading and updating a large-scale point cloud map by a block index storage and local splicing updating mode, firstly, the embodiment of the invention stores the point cloud data and pose information of a key frame by a point cloud map mode, combines the advantages of a multi-resolution hierarchical structure of a tile map, correspondingly codes the pose information of the key frame into a tile number based on the coding mode of the tile map, takes the tile number as the node index of the point cloud map, and can realize quick index of adjacent map nodes by the block index storage, dynamically and quickly search and load the point cloud map in a nearby area through the tile number in practical application, thereby improving the loading speed; meanwhile, in the embodiment of the invention, the point cloud data are stored in the point cloud map in a node mode, so that the integrity of the point cloud data is ensured, and the point cloud data corresponding to the point cloud data required by the point cloud matching algorithm used by different SLAM algorithms can be stored in a matching mode; further, when the point cloud map is updated, the embodiment of the invention realizes the splicing of the map co-view area by optimizing the newly added key frame sequence and the pose of the associated map node in the existing map, and realizes the updating and correction of the map partial area by deleting the adjacent old node and reinserting the new node, so that the map area can be conveniently and accurately expanded, and the method is suitable for large-scale map expansion and partial area updating.
Drawings
In order to more clearly illustrate the embodiments of the invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, it being obvious that the drawings in the following description are only some embodiments of the invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a flowchart of a method for storing a node point cloud map according to an embodiment of the present invention.
Fig. 2 and 3 are schematic diagrams of the layer 1 and layer 2 tile numbering of the tile map, respectively, in an embodiment of the present invention.
Fig. 4 is a flowchart of a method for updating a node point cloud map according to an embodiment of the present invention.
Detailed Description
The following description of embodiments refers to the accompanying drawings, which illustrate specific embodiments in which the invention may be practiced.
As shown in fig. 1, the method for storing a node type point cloud map according to the embodiment of the invention includes the following steps:
step S11, pose information and point cloud data of each key frame in the key frame sequence are obtained.
Specifically, the embodiment of the invention can obtain the key frame sequence for establishing the point cloud map by acquiring the sensor information and utilizing the SLAM method, wherein the key frame sequence comprises pose information of key frames in the key frame sequence, pose constraint among the key frames and point cloud data. Wherein the sensor information may be obtained by a laser radar, an inertial sensor (IMU), a Global Positioning System (GPS), etc., and is not limited thereto; the specific SLAM to be used is not limited.
Further, in step S11, the point cloud data of each key frame is not limited to the complete point cloud data in the key frame, but may also be part of the point cloud data in the key frame, for example, the point cloud data may only include the point cloud data corresponding to the selected feature point in the key frame, and the feature point may depend on the specific application of the node type point cloud map according to the embodiment of the present invention, for example, when applied to the vehicle self-positioning, the selected feature point may be a feature point for positioning, which corresponds to the feature point required by the map matching algorithm used for the vehicle self-positioning.
The coordinate system of the point cloud data of each key frame can be a local coordinate system with the pose of the key frame as an origin, or can be projected to a world coordinate system through the pose of the key frame.
Step S12, obtaining corresponding tile numbers of pose information of each key frame in the key frame sequence under an earth projection plane coordinate system.
Specifically, the tile map is a multi-resolution hierarchical model, the tile numbers of which are numbered according to hierarchical division, each sub-hierarchy is obtained by dividing the previous hierarchical region into four equal parts horizontally and vertically, the hierarchical division of the tile map can be exemplified by referring to fig. 2 and 3, and the tile numbers are formed according to the hierarchy in sequence. The techniques for tile map and tile numbering are well known to those of ordinary skill in the art and will not be described in detail herein. As the tile number length increases, the map tile may represent a smaller and more accurate location area, but the actual area range corresponding to the tile level varies in size from geographic location on earth, and the area range below ten square centimeters may be corresponding to the 29-level tile number. Therefore, in the embodiment of the present invention, the number of the adopted tile numbers may be set according to the calculation accuracy and the actual use requirement, for example, the tile numbers are set to 30 levels or less.
In step S12, pose information of each key frame in the key frame sequence is encoded into a corresponding tile number under the map projection plane coordinate system. In one embodiment, the map projection plane coordinate system may be a UTM (Universal Transverse Mercator Grid System, universal cross ink card grid system) coordinate system, but is not limited thereto.
In particular, the pose information of each key frame can be encoded into a corresponding tile number by associating the plane coordinates of the pose information of the key frame in the map projection plane coordinate system with the tile map coordinates.
Step S13, forming a node by pose information, point cloud data and corresponding tile numbers of each key frame in the key frame sequence, and storing all nodes formed by the key frame sequence by taking the tile numbers as indexes to form a point cloud map. The point cloud map can be used as an initial point cloud map, namely a first edition point cloud map.
Therefore, in the embodiment of the invention, the association between the pose information and the geographic coordinates is established by acquiring the tile number corresponding to the pose information of each key frame in the key frame sequence, and then the nodes in the point cloud map are stored by taking the tile number as the index of the nodes, so that the point cloud data can be efficiently and reasonably stored, and the scene point cloud map can be quickly indexed when being used; and the point cloud data is stored in a node mode, namely, the point cloud data can be complete point cloud data in a key frame, and the point cloud data corresponding to the selected characteristic points in the key frame can be three-dimensional mapping of a real scene, so that the integrity of the required point cloud data is ensured.
On the basis of forming the point cloud map as the initial point cloud map according to the storage method, the embodiment of the invention further provides an updating method for the node point cloud map. It should be noted that, the update method of the node type point cloud map provided by the embodiment of the invention is to update the point cloud map based on the existing point cloud map, where the existing point cloud map is the point cloud map stored last time, for example, if the point cloud map is updated based on the initial point cloud map, the existing point cloud map is the initial point cloud map; if the point cloud map is updated for a plurality of times, the existing point cloud map is the point cloud map updated and stored last time.
Based on this, as shown in fig. 4, the method for updating the node point cloud map according to the embodiment of the invention includes the following steps:
step S21, pose information and point cloud data of each key frame in the new key frame sequence are obtained.
Step S22, the corresponding tile numbers of the pose information of each key frame in the new key frame sequence under the earth projection plane coordinate system are obtained.
For the specific implementation of step S21 and step S22, reference may be made to the above description of step S11 and step S12, which are not repeated here.
Step S23, determining pose information of each key frame in the new key frame sequence in the point cloud map (existing point cloud map).
Specifically, in step S23, all key frames in the new key frame sequence are traversed, for each key frame, based on the tile number corresponding to the key frame, neighboring nodes are searched in the point cloud map, loop-back nodes matched with the key frame in the point cloud map are determined, the relative pose between the key frame and the loop-back nodes matched with the key frame is further determined to establish pose constraint, and pose information of the key frame in the point cloud map is determined according to the pose information of the loop-back nodes.
Further, for each key frame, a loop point in the point cloud map that matches the key frame may be determined by:
searching adjacent nodes in the point cloud map based on the tile numbers corresponding to the key frames, and converting the point cloud data of the searched adjacent nodes into the same space coordinate system through corresponding pose information to form a sub-map, wherein the same space coordinate system can be a world coordinate system;
and determining the loop-back node matched with the key frame in the sub map through point cloud registration in loop-back detection based on the point cloud data of the key frame and the point cloud data of the searched adjacent nodes.
After determining the loop point in the sub-map that matches the key frame, the relative pose between the key frame and the matched loop point can be determined to establish a pose constraint. And further, according to the pose information of the matched loop nodes, determining the pose information of the key frame in the sub map as the pose information of the key frame in the point cloud map.
And step S24, determining associated map nodes of all key frames in the new key frame sequence in the point cloud map, and determining relative pose between the associated map nodes and the loop link nodes to establish pose constraint.
In particular, the associated map node of a key frame may select map nodes within a certain range threshold that is determined by the application requirements, e.g., selecting all map nodes within 50m as associated map nodes.
In an embodiment, nodes in the neighboring areas of all loop-back nodes corresponding to all key frames in the new key frame sequence are selected as associated map nodes to establish pose constraints. Thus, step S24 may specifically include: firstly, searching nodes in all loop-back link node adjacent areas corresponding to all key frames in a new key frame sequence in a point cloud map as associated map nodes; next, relative pose between the associated map node and the corresponding loop-back node is determined to establish pose constraints.
Step S25, according to pose constraints among the key frames in the new key frame sequence, pose constraints among the key frames in the new key frame sequence and the matched loop nodes, and pose constraints among the associated map nodes and the corresponding loop nodes, pose map optimization is carried out, and pose change amounts of the associated map nodes and pose change amounts of the key frames in the new key frame sequence are determined.
It is noted here that the specific pose chart optimization is not limited.
Step S26, according to the pose change quantity of the associated map nodes, pose information of the associated map nodes after optimization is determined, and new tile numbers corresponding to the pose information of the associated map nodes after optimization under an earth projection plane coordinate system are obtained; and updating pose information of each key frame in the point cloud map to obtain new tile numbers corresponding to the optimized pose information of each key frame under the earth projection plane coordinate system according to the pose change amount of each key frame in the new key frame sequence.
And step S27, updating the point cloud map according to the pose information after the associated map node optimization and the corresponding new tile number.
And S28, deleting nodes in which the node areas in the updated point cloud map overlap with the areas determined by the pose information after the key frame optimization.
Specifically, according to the embodiment of the invention, according to the new tile number corresponding to the pose information optimized by each key frame in the new key frame sequence, the nodes with similar distances to each key frame in the updated point cloud map are searched, whether the node areas determined by the nodes with similar distances overlap with the areas determined by the pose information optimized by the corresponding key frame or not is judged, and the nodes with overlapping node areas in the nodes with similar distances and the areas determined by the pose information optimized by the corresponding key frame are deleted. Therefore, the embodiment of the invention can delete the node which is overlapped with the newly added node area in the existing point cloud map.
In this embodiment, the neighboring node, neighboring area, or nodes that are close in distance may be nodes or areas that are within a threshold of a certain range, and the threshold may be determined according to the application requirement.
Step S29, the pose information, the point cloud data and the corresponding new tile numbers of each key frame in the new key frame sequence are formed into a new node, and all the new nodes formed by the new key frame sequence are stored into the updated point cloud map by taking the new tile numbers as indexes.
Therefore, the embodiment of the invention can finish updating the existing point cloud map based on the new key frame sequence, namely, the splicing of the new map and the existing map is realized.
As can be seen from the above description, the embodiment of the present invention is based on the key frame sequence generated by SLAM, and uses tile map coding to correspondingly code pose information of each key frame into tile numbers by associating plane coordinates of pose information of each key frame with tile map coordinates, and uses the tile numbers as indexes to construct a point cloud map, and stores pose information and point cloud data of each key frame in the key frame sequence. In the point cloud map, point cloud data is stored in a node mode and is a three-dimensional map of a real scene.
Further, when the point cloud map is spliced and updated based on the node point cloud map built in the storage mode in the embodiment of the invention, the embodiment of the invention constructs pose constraint by matching link points in the existing point cloud map and determining associated map nodes; and then, based on pose map optimization, obtaining pose variation amounts of each key frame in the associated map node (existing map node) and the new key frame sequence so as to adjust pose information of each key frame in the associated map node and the new key frame sequence. And determining a new tile number corresponding to the associated map node and a tile number corresponding to each key frame in the new key frame sequence according to the adjusted pose information, and using the new tile number and the tile number corresponding to each key frame in the new key frame sequence as the corresponding tile number in the spliced point cloud map. When in splicing, the existing point cloud map is updated firstly, nodes which are overlapped with each key frame in the new key frame sequence in the existing point cloud map are deleted, pose information, point cloud data and corresponding new tile numbers after the key frames in the new key frame sequence are optimized are stored as new nodes, and the insertion of the point cloud map of the new key frame sequence and the updating of the point cloud map are completed, so that the splicing of the newly added map and the existing map can be realized, and the map region expansion is performed.
Compared with the prior art, the embodiment of the invention solves the problems of generating, loading and updating a large-scale point cloud map by means of block index storage and local splicing updating, firstly, the embodiment of the invention stores the point cloud data and pose information of key frames by means of the point cloud map, combines the advantages of a multi-resolution hierarchical structure of a tile map, correspondingly codes the pose information of the key frames into tile numbers by means of the coding of the tile map, takes the tile numbers as node indexes of the point cloud map, and stores the block indexes in such a way that adjacent map nodes can be rapidly indexed, and in practical application, the point cloud map in a nearby area can be dynamically and rapidly searched and loaded by the tile numbers, thereby improving the loading speed; in addition, in the embodiment of the invention, the point cloud data are stored in the point cloud map in a node mode, so that the integrity of the point cloud data can be ensured, and the point cloud data corresponding to the point cloud data required by the point cloud matching algorithm used by different SLAM algorithms can be stored; further, when the point cloud map is updated, the embodiment of the invention realizes the splicing of the map common view area by optimizing the newly added key frame sequence and the pose of the associated map node in the existing point cloud map, and can realize the update and correction of the map partial area by deleting the adjacent old node and reinserting the new node, so that the map area can be conveniently and accurately expanded, and the method is suitable for large-scale map expansion and partial area update.
The storage and update mode of the node type point cloud map can be adapted to key frame sequence point cloud maps generated by SLAM algorithm of point cloud matching in various different modes, when the actual map matching and positioning application is performed, map nodes adjacent to the current pose in the existing map can be quickly indexed through multi-level tile numbers to be spliced into sub-maps for matching, and the problem of loading the point cloud map can be further solved through a mode of carrying out block loading by taking the nodes as basic units, so that the loaded data volume is reduced, and the loading speed and the operation speed are improved. The method can be applied to scene map storage and splicing updating in the vehicle self-positioning technology in the technical field of automatic driving.
The foregoing disclosure is illustrative of the present invention and is not to be construed as limiting the scope of the invention, which is defined by the appended claims.
Claims (9)
1. The method for storing the node type point cloud map is characterized by comprising the following steps of:
acquiring pose information and point cloud data of each key frame in a key frame sequence;
acquiring corresponding tile numbers of pose information of each key frame in the key frame sequence under an earth projection plane coordinate system;
and forming a node by pose information, point cloud data and corresponding tile numbers of each key frame in the key frame sequence, and storing all nodes formed by the key frame sequence by taking the tile numbers as indexes to form the point cloud map.
2. The storage method according to claim 1, wherein the point cloud data of each key frame includes complete point cloud data in the key frame or includes point cloud data corresponding to a selected feature point in the key frame.
3. The storage method according to claim 1, wherein the point cloud data of each key frame includes point cloud data corresponding to a selected feature point in the key frame, the selected feature point corresponding to a feature point required by a map matching algorithm employed for vehicle self-positioning.
4. The storage method of claim 1, wherein the earth projection plane coordinate system is a UTM coordinate system.
5. A method of updating a node point cloud map, the initial point cloud map of the point cloud map being stored according to the method of any one of claims 1-4, the method comprising:
acquiring pose information and point cloud data of each key frame in the new key frame sequence;
acquiring corresponding tile numbers of pose information of each key frame in the new key frame sequence under an earth projection plane coordinate system;
traversing all key frames in the new key frame sequence, searching adjacent nodes in the point cloud map based on tile numbers corresponding to the key frames for each key frame, determining loop-back nodes matched with the key frames in the point cloud map, determining relative pose between the key frames and the loop-back nodes matched with the key frames to establish pose constraint, and determining pose information of the key frames in the point cloud map according to pose information of the loop-back nodes;
determining associated map nodes of all key frames in the new key frame sequence in the point cloud map, and determining relative pose between the associated map nodes and the loop link nodes to establish pose constraint;
according to pose constraints among key frames in the new key frame sequence, pose constraints among key frames in the new key frame sequence and the matched loop link nodes, and pose constraints among the associated map nodes and the corresponding loop link nodes, carrying out pose map optimization, and determining pose change amounts of the associated map nodes and pose change amounts of key frames in the new key frame sequence;
determining pose information of the associated map nodes after optimization according to the pose change quantity of the associated map nodes, and obtaining new tile numbers corresponding to the pose information of the associated map nodes after optimization under an earth projection plane coordinate system; updating pose information of each key frame in the point cloud map to be optimized pose information according to pose change quantity of each key frame in the new key frame sequence, and obtaining new tile numbers corresponding to the pose information of each key frame after optimization under an earth projection plane coordinate system;
updating the point cloud map according to the pose information after the associated map node optimization and the corresponding new tile number;
deleting nodes in which node areas in the updated point cloud map overlap with areas determined by the pose information after optimizing the key frames;
and forming a new node by the pose information, the point cloud data and the corresponding new tile number after optimizing each key frame in the new key frame sequence, and storing all new nodes formed by the new key frame sequence into the updated point cloud map by taking the new tile number as an index.
6. The updating method according to claim 5, wherein the determining a loop node in the point cloud map that matches the key frame specifically comprises:
converting the searched point cloud data of the adjacent nodes into the same space coordinate system through corresponding pose information to form a sub-map;
and determining loop-back nodes matched with the key frames in the sub-map through point cloud registration based on the point cloud data of the key frames and the point cloud data of the adjacent nodes.
7. The updating method according to claim 6, wherein determining pose information of the key frame in the point cloud map according to pose information of the loop-back node specifically comprises:
and determining pose information of the key frame in the sub map according to the pose information of the loop-back node.
8. The updating method according to claim 5, wherein determining associated map nodes of all key frames in the new key frame sequence in the point cloud map, determining a relative pose between the associated map nodes and the loop-back node to establish a pose constraint, specifically comprises:
searching nodes in all loop-back link node adjacent areas corresponding to all key frames in the new key frame sequence in the point cloud map as associated map nodes;
and determining the relative pose between the associated map node and the corresponding loop link node to establish pose constraint.
9. The updating method according to claim 5, wherein the deleting the node in the updated point cloud map, where the node area overlaps with the area determined by the pose information after the optimization of each key frame, specifically includes:
searching nodes with similar distances to each key frame in the updated point cloud map according to new tile numbers corresponding to the pose information after the key frames are optimized in the new key frame sequence;
judging whether a node area determined by the nodes with similar distances is overlapped with an area determined by pose information after optimizing the corresponding key frames;
and deleting the nodes with overlapping node areas in the nodes with similar distances and the areas determined by the pose information after the optimization of the corresponding key frames.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210207911.6A CN116737851A (en) | 2022-03-04 | 2022-03-04 | Storage and updating method of node type point cloud map |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210207911.6A CN116737851A (en) | 2022-03-04 | 2022-03-04 | Storage and updating method of node type point cloud map |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116737851A true CN116737851A (en) | 2023-09-12 |
Family
ID=87901689
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210207911.6A Pending CN116737851A (en) | 2022-03-04 | 2022-03-04 | Storage and updating method of node type point cloud map |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116737851A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117132728A (en) * | 2023-10-26 | 2023-11-28 | 毫末智行科技有限公司 | Method and device for constructing map, electronic equipment and storage medium |
-
2022
- 2022-03-04 CN CN202210207911.6A patent/CN116737851A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117132728A (en) * | 2023-10-26 | 2023-11-28 | 毫末智行科技有限公司 | Method and device for constructing map, electronic equipment and storage medium |
| CN117132728B (en) * | 2023-10-26 | 2024-02-23 | 毫末智行科技有限公司 | Method and device for constructing map, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111862162B (en) | Loop detection method and system, readable storage medium and electronic device | |
| CN111343585B (en) | Mobile user track map matching method based on hidden Markov model | |
| CN110645998B (en) | Dynamic object-free map segmentation establishing method based on laser point cloud | |
| CN101498785B (en) | GNSS network differential positioning reference station network construction method and dynamic updating method | |
| CN110427360A (en) | Processing method, processing unit, processing system and the computer program product of track data | |
| CN111427904B (en) | High-precision map data updating method and device and electronic equipment | |
| CN110187348A (en) | A kind of method of laser radar positioning | |
| CN109086278B (en) | Map construction method, system, mobile terminal and storage medium for eliminating errors | |
| CN103562681A (en) | Method of generating a database for a navigation device, method of outputting a three-dimensional map, and navigation device | |
| CN110549339A (en) | navigation method, navigation device, navigation robot and storage medium | |
| CN113989451B (en) | High-precision map construction method and device and electronic equipment | |
| US20120323486A1 (en) | Method for simplifying a description of a route of travel | |
| CN109558643B (en) | Traffic scene modeling and model unitization method | |
| CN116737851A (en) | Storage and updating method of node type point cloud map | |
| CN105869512A (en) | Multi-information mixed measurement mapping method and device | |
| CN113514843A (en) | Multi-subgraph laser radar positioning method and system and terminal | |
| CN112132951B (en) | Construction method of grid semantic map based on vision | |
| Zhang et al. | A framework for road change detection and map updating | |
| CN114780758A (en) | Construction method of image control point library based on stereo mapping satellite image | |
| CN109947884B (en) | Efficient organization query method for global ICESat/GLAS point cloud | |
| CN110686693A (en) | Method for constructing road network information in closed scene | |
| CN114281915B (en) | Method, device and equipment for generating geometric road network and storage medium | |
| CN117870646A (en) | Laser SLAM mapping method suitable for underground mine environment | |
| CN112883290B (en) | Automatic graph cutting method based on branch delimitation method | |
| CN117128974B (en) | Navigation route searching method and device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |