CN114322994A - Multipoint cloud map fusion method and device based on offline global optimization - Google Patents

Multipoint cloud map fusion method and device based on offline global optimization Download PDF

Info

Publication number
CN114322994A
CN114322994A CN202210228899.7A CN202210228899A CN114322994A CN 114322994 A CN114322994 A CN 114322994A CN 202210228899 A CN202210228899 A CN 202210228899A CN 114322994 A CN114322994 A CN 114322994A
Authority
CN
China
Prior art keywords
point cloud
factor
information
cloud map
gps
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.)
Granted
Application number
CN202210228899.7A
Other languages
Chinese (zh)
Other versions
CN114322994B (en
Inventor
华炜
高海明
张顺
胡艳明
马也驰
邱奇波
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Lab
Original Assignee
Zhejiang Lab
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Lab filed Critical Zhejiang Lab
Priority to CN202210228899.7A priority Critical patent/CN114322994B/en
Publication of CN114322994A publication Critical patent/CN114322994A/en
Application granted granted Critical
Publication of CN114322994B publication Critical patent/CN114322994B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

The invention discloses a multipoint cloud map fusion method and device based on offline global optimization, wherein the method comprises the following steps: step 1, adapting a laser SLAM method based on GPS information initialization; step 2, sequentially obtaining a plurality of point cloud maps from a plurality of data sets required to be used for point cloud fusion according to the online laser SLAM method of S1; step 3, off-line loading the information of each point cloud map, and sequentially constructing the odometer factor, the loopback factor and the GPS factor of each point cloud map; step 4, constructing mutual constraint factors among different point cloud maps; and 5, setting optimization parameters, carrying out global optimization, and storing the final fusion point cloud and all key frame information. Compared with an online multipoint cloud map fusion method, the method provided by the invention has the advantages that the stability and reliability of multipoint cloud map fusion are improved, the difficulty of algorithm implementation is reduced, and the balance relation between the map building performance and the operation real-time performance is not considered.

Description

Multipoint cloud map fusion method and device based on offline global optimization
Technical Field
The invention relates to the field of positioning and mapping based on a laser radar sensor, in particular to a multipoint cloud map fusion method and device based on offline global optimization.
Background
Since the Simultaneous Localization and Mapping (SLAM) problem was first proposed in 1985, SLAM technology has been developed for over 30 years, and its theoretical system has been substantially perfected; at present, the method enters a robust sensing era facing specific applications, especially plays an important role in the fields of mobile robots, unmanned driving and the like, and a large number of SLAM methods with excellent performance are continuously emerged.
In the process of carrying out specific tasks by the unmanned vehicle, a prior point cloud map for matching and positioning is an indispensable component of the whole algorithm module, wherein the point cloud map can be obtained by the existing mature laser SLAM method. However, for a large-scale environment, it is difficult to obtain a complete prior point cloud map through one-time data acquisition (single-session); meanwhile, the map needs to be partially adjusted, and the scene changes locally. Therefore, the fusion of the multi-point cloud map is crucial to the acquisition of the complete point cloud map. However, at present, related research on multipoint cloud map fusion is few, and mainly focuses on an online method, which has high requirements on algorithm implementation, is not easy to develop, and needs to balance mapping performance and operation real-time performance.
Disclosure of Invention
In order to solve the technical problems in the prior art, the invention provides a multipoint cloud map fusion method and device based on offline global optimization, and the specific technical scheme is as follows:
a multipoint cloud map fusion method based on offline global optimization comprises the following steps:
step 1, based on the flow of an open source laser SLAM method-LIO-SAM, carrying out GPS information initialization adaptation, simultaneously constructing self-constraint during and after the operation process of using the open source laser SLAM method, and reserving keyframe position and attitude information, point cloud information and constraint information, wherein the constraint information comprises: an odometry factor, a GPS factor and a loopback factor;
step 2, sequentially obtaining a plurality of point cloud maps from a plurality of data sets for point cloud fusion according to the laser SLAM method in the step 1;
step 3, off-line loading pose information of each point cloud map, and constructing a mileometer factor, a GPS factor and a loopback factor among poses in each point cloud map;
step 4, constructing mutual constraint factors among different point cloud maps;
and 5, constructing a point cloud map fusion cost function according to the loaded self-constraint factor and the constructed mutual constraint factor, and performing global optimization to obtain fusion point cloud and all key frame information.
Further, the step 1 specifically includes the following substeps:
step 1.1, using GPS information as an initial pose of a first frame key frame, and adding a GPS factor to restrain the first frame key frame; first frame key frame is recorded as
Figure DEST_PATH_IMAGE002
Using GPS information
Figure DEST_PATH_IMAGE004
Initializing, and adding initial time GPS factor
Figure DEST_PATH_IMAGE006
Wherein
Figure DEST_PATH_IMAGE008
Representing a mapping function between the keyframe pose information and the GPS information,
Figure DEST_PATH_IMAGE010
representing a variance matrix;
step 1.2, selecting key frames according to distance threshold and angle threshold in the running process of laser SLAM
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE014
Adding and recording odometry factors between adjacent key frames
Figure DEST_PATH_IMAGE016
Wherein
Figure DEST_PATH_IMAGE018
And
Figure DEST_PATH_IMAGE020
respectively representing pose transformation functions and odometer observation information between adjacent key frames,
Figure DEST_PATH_IMAGE022
representing a variance matrix; running loop detection on-line, adding loop factor
Figure DEST_PATH_IMAGE024
Wherein
Figure DEST_PATH_IMAGE026
And
Figure DEST_PATH_IMAGE028
respectively representing pose transformation function and loop observation information between loop key frames,
Figure DEST_PATH_IMAGE030
representing a variance matrix; simultaneously adding and recording GPS factors according to the distance threshold value
Figure DEST_PATH_IMAGE032
；
Step 1.3, when the laser SLAM operation is finished, constructing a cost function for generating a single point cloud map, and recording all key frame pose information and corresponding point cloud information, wherein the formula is as follows:
Figure DEST_PATH_IMAGE034
wherein the content of the first and second substances,
Figure DEST_PATH_IMAGE036
and
Figure DEST_PATH_IMAGE038
respectively indicating whether a GPS factor and a loop factor exist, and obtaining a key frame sequence by optimizing a cost function
Figure DEST_PATH_IMAGE040
So as to obtain the point cloud map,
Figure DEST_PATH_IMAGE042
a cost function of a single point cloud map, wherein
Figure DEST_PATH_IMAGE044
Is shown as
Figure DEST_PATH_IMAGE046
A sequence of key frames.
Further, the step 2 specifically includes: using multiple off-line datasets needed for point cloud map fusion
Figure DEST_PATH_IMAGE048
Obtaining a plurality of corresponding point cloud maps by adopting the open source laser SLAM method in the step 1 and recording the point cloud maps as
Figure DEST_PATH_IMAGE050
The information of the corresponding point cloud maps comprises key frame pose information and corresponding key frame point cloud information, and simultaneously comprises a milemeter factor, a GPS factor and a loopback factor.
Further, the step 3 specifically includes: offline loading of multiple point cloud maps
Figure 187725DEST_PATH_IMAGE050
Each point cloud map includes keyframe pose information, point cloud information, and constraint information, rootAnd sequentially constructing a mileometer factor, a GPS factor and a loopback factor among all positions in the cloud map of each point according to the constraint information.
Further, the step 4 specifically includes the following substeps;
step 4.1, two point cloud maps with overlapped areas are obtained through traversal
Figure DEST_PATH_IMAGE052
And
Figure DEST_PATH_IMAGE054
，
Figure DEST_PATH_IMAGE056
for point cloud maps
Figure 760658DEST_PATH_IMAGE052
The set of poses it contains is noted
Figure DEST_PATH_IMAGE058
，
Figure DEST_PATH_IMAGE060
Based on a set of poses
Figure 868291DEST_PATH_IMAGE058
Constructing a point cloud of locations
Figure DEST_PATH_IMAGE062
And corresponding KD tree
Figure DEST_PATH_IMAGE064
；
Step 4.2, according to the point cloud map
Figure 794659DEST_PATH_IMAGE054
Traversing to obtain a pose set
Figure DEST_PATH_IMAGE066
The current frame is recorded as
Figure DEST_PATH_IMAGE068
By using
Figure 671348DEST_PATH_IMAGE064
Searching nearest neighbor key frames to obtain target key frames meeting the distance threshold condition, and recording the target key frames as the target key frames
Figure DEST_PATH_IMAGE070
；
Step 4.3, based on the target key frame
Figure 556127DEST_PATH_IMAGE070
To construct local point cloud map
Figure DEST_PATH_IMAGE072
Constructing a mutual constraint factor according to the point cloud matching result
Figure DEST_PATH_IMAGE074
Wherein
Figure DEST_PATH_IMAGE076
And
Figure DEST_PATH_IMAGE078
respectively representing pose transformation function and mutual constraint observation information between key frames of different point cloud maps,
Figure DEST_PATH_IMAGE080
a variance matrix is represented.
Further, in step 5, all the key frame information is obtained, and a specific expression is as follows:
Figure DEST_PATH_IMAGE082
wherein the content of the first and second substances,
Figure DEST_PATH_IMAGE084
the cost function comprises an internal mileometer factor, a loopback factor and a GPS factor of each point cloud map, the second item comprises the mutual constraint factor constructed in the step 4, and then the process is carried outLine global optimization to get all key frame information
Figure DEST_PATH_IMAGE086
。
A multipoint cloud map fusion device based on offline global optimization comprises one or more processors and is used for achieving the multipoint cloud map fusion method based on offline global optimization.
A computer-readable storage medium, on which a program is stored, which, when executed by a processor, implements the offline global optimization-based multipoint cloud map fusion method.
The invention has the advantages and beneficial effects that:
the multipoint cloud map fusion method provided by the invention has the advantages that the algorithm logic is simple and clear, the implementation scheme is flexible and changeable, the phenomena of double images and the like in the multipoint cloud map fusion process can be effectively avoided, meanwhile, the stability and the reliability of the multipoint cloud map fusion are improved based on the implementation means of the decoupling of the fusion process compared with the online multipoint cloud map fusion method, the difficulty of the algorithm implementation is reduced, and the balance relation between the map building performance and the operation real-time performance is not considered.
Drawings
FIG. 1 is a flowchart of a multipoint cloud map fusion method based on offline global optimization according to the present invention;
FIG. 2 is a flow chart of a multipoint cloud map fusion mutual constraint construction method in the present invention;
FIG. 3 is a schematic diagram showing a point cloud stitching effect before offline global optimization in the present invention;
FIG. 4 is a schematic diagram showing a point cloud fusion effect after offline global optimization in the present invention;
fig. 5 is a schematic structural diagram of the multipoint cloud map fusion device based on offline global optimization.
Detailed Description
In order to make the objects, technical solutions and technical effects of the present invention more clearly apparent, the present invention is further described in detail below with reference to the accompanying drawings and examples.
As shown in fig. 1, the multipoint cloud map fusion method based on offline global optimization of the present invention includes the following steps:
step 1, adapting a laser SLAM method based on GPS information initialization, and constructing self-restraint by the laser SLAM method: based on the open source laser SLAM method-LIO-SAM process, carrying out GPS information initialization adaptation, and simultaneously reserving key frame position and attitude information, point cloud information and constraint information during and after the operation process of using the open source laser SLAM method, wherein the constraint information comprises the following steps: an odometry factor, a GPS factor and a loopback factor.
Specifically, the method comprises the following substeps:
step 1.1, using GPS information as an initial pose of a first frame key frame, and adding a GPS factor to restrain the first frame key frame; first frame key frame is recorded as
Figure 72428DEST_PATH_IMAGE002
Using GPS information
Figure 864804DEST_PATH_IMAGE004
Initializing, and adding initial time GPS factor
Figure 2524DEST_PATH_IMAGE006
Wherein
Figure 589363DEST_PATH_IMAGE008
Representing a mapping function between the keyframe pose information and the GPS information,
Figure 546955DEST_PATH_IMAGE010
representing a variance matrix;
step 1.2, selecting key frames according to distance threshold and angle threshold in the running process of laser SLAM
Figure 877442DEST_PATH_IMAGE012
，
Figure 869669DEST_PATH_IMAGE014
Adding and recording odometers between adjacent keyframesFactor(s)
Figure 565092DEST_PATH_IMAGE016
Wherein
Figure 400193DEST_PATH_IMAGE018
And
Figure 878579DEST_PATH_IMAGE020
respectively representing pose transformation functions and odometer observation information between adjacent key frames,
Figure 115525DEST_PATH_IMAGE022
representing a variance matrix; running loop detection on-line, adding loop factor
Figure 919533DEST_PATH_IMAGE024
Wherein
Figure 241930DEST_PATH_IMAGE026
And
Figure 586324DEST_PATH_IMAGE028
respectively representing pose transformation function and loop observation information between loop key frames,
Figure 553143DEST_PATH_IMAGE030
representing a variance matrix; simultaneously adding and recording GPS factors according to the distance threshold value
Figure 918265DEST_PATH_IMAGE032
；
Step 1.3, recording all key frame position and posture information and corresponding point cloud information when the laser SLAM finishes running, constructing a cost function for generating a single point cloud map,
Figure DEST_PATH_IMAGE034A
wherein the content of the first and second substances,
Figure 400062DEST_PATH_IMAGE036
and
Figure 876043DEST_PATH_IMAGE038
respectively indicating whether a GPS factor and a loop factor exist, and obtaining a key frame sequence by optimizing a cost function
Figure 431789DEST_PATH_IMAGE040
So as to obtain the point cloud map,
Figure 967813DEST_PATH_IMAGE042
a cost function of a single point cloud map, wherein
Figure 874589DEST_PATH_IMAGE044
Is shown as
Figure 560785DEST_PATH_IMAGE046
A sequence of key frames.
Step 2, sequentially obtaining a plurality of point cloud maps from a plurality of data sets required to be used for point cloud fusion according to the online laser SLAM method in the step 1: using multiple off-line datasets needed for point cloud map fusion
Figure 626830DEST_PATH_IMAGE048
Obtaining a plurality of corresponding point cloud maps by adopting the open source laser SLAM method in the step 1 and recording the point cloud maps as
Figure 209121DEST_PATH_IMAGE050
The information of the corresponding point cloud maps comprises key frame pose information and corresponding key frame point cloud information, and simultaneously comprises a milemeter factor, a GPS factor and a loopback factor.
Step 3, off-line loading of each point cloud map information, loading self-constraint: offline loading of multiple point cloud maps
Figure 462248DEST_PATH_IMAGE050
Each point cloud map comprises key frame pose information, point cloud information and constraint information, and each pose in each point cloud map is sequentially constructed according to the constraint informationAn odometry factor, a GPS factor, and a loopback factor in between.
Step 4, as shown in fig. 2, constructing mutual constraint factors between different point cloud maps: traversing to obtain two point cloud maps with overlapped areas
Figure 155397DEST_PATH_IMAGE052
And
Figure 810370DEST_PATH_IMAGE054
，
Figure 563562DEST_PATH_IMAGE056
for point cloud maps
Figure 241668DEST_PATH_IMAGE052
The set of poses it contains is noted
Figure 863142DEST_PATH_IMAGE058
，
Figure 247987DEST_PATH_IMAGE060
And constructing a mutual constraint factor according to the point cloud matching.
The construction of the mutual constraint factor and the constraint factor between the cloud maps of each point are realized through target key frame search and point cloud matching, and the construction method specifically comprises the following substeps:
step 4.1, based on pose set
Figure 31135DEST_PATH_IMAGE058
Constructing a point cloud of locations
Figure 399800DEST_PATH_IMAGE062
And corresponding KD tree
Figure 559386DEST_PATH_IMAGE064
；
Step 4.2, according to the point cloud map
Figure 126633DEST_PATH_IMAGE054
Traversing to obtain a pose set
Figure 956049DEST_PATH_IMAGE066
The current frame is recorded as
Figure 936643DEST_PATH_IMAGE068
By using
Figure 509707DEST_PATH_IMAGE064
Searching nearest neighbor key frames to obtain target key frames meeting the distance threshold condition, and recording the target key frames as the target key frames
Figure 993778DEST_PATH_IMAGE070
；
Step 4.3, based on the target key frame
Figure 259674DEST_PATH_IMAGE070
To construct local point cloud map
Figure 134090DEST_PATH_IMAGE072
Constructing a mutual constraint factor according to the point cloud matching result
Figure 901057DEST_PATH_IMAGE074
Wherein
Figure 380580DEST_PATH_IMAGE076
And
Figure 4328DEST_PATH_IMAGE078
respectively representing pose transformation function and mutual constraint observation information between key frames of different point cloud maps,
Figure 303723DEST_PATH_IMAGE080
a variance matrix is represented.
Step 5, constructing a point cloud map fusion cost function according to the loaded self-constraint and the constructed mutual constraint, and carrying out global optimization to obtain a fusion point cloud and all key frame information, wherein the specific expression is as follows:
Figure DEST_PATH_IMAGE082A
wherein the content of the first and second substances,
Figure 671119DEST_PATH_IMAGE084
the cost function for constructing a single point cloud in the step 1 comprises an internal odometry factor, a loopback factor and a GPS factor of each point cloud map, the second item comprises the mutual constraint factor constructed in the step 4, and then global optimization is carried out to obtain all key frame information
Figure 5148DEST_PATH_IMAGE086
。
The method has the advantages that the construction process of the internal constraint factor and the mutual constraint factor in the multi-map fusion process is decoupled, and the internal constraint information of a single point cloud map is constructed by an online laser SLAM method, wherein the internal constraint information comprises an odometer factor, a loopback factor and a GPS factor; further, a mutual constraint factor between any two point cloud maps is constructed through target key frame search and point cloud matching results, and then a fused point cloud map and all key frame posture information are obtained through global optimization. As shown in fig. 3 and 4, the point cloud fusion effect before and after the offline global optimization embodiment is shown, and it can be seen that after the offline global optimization, the ghost phenomenon between the maps is eliminated. The invention provides a simple, clear and easily-developed solution for the construction of a complete prior point cloud map in the field of unmanned vehicles. Particularly, for the construction of a large-scale environment and the adjustment of a local scene, the flexibility of the fusion process is ensured, and the effective fusion of the multipoint cloud map can also be ensured.
Corresponding to the embodiment of the multipoint cloud map fusion method based on the offline global optimization, the invention also provides an embodiment of a multipoint cloud map fusion device based on the offline global optimization.
Referring to fig. 5, the multipoint cloud map fusion apparatus based on offline global optimization provided in the embodiment of the present invention includes one or more processors, and is configured to implement the multipoint cloud map fusion method based on offline global optimization in the embodiment.
The embodiment of the multipoint cloud map fusion device based on offline global optimization can be applied to any equipment with data processing capability, such as computers and other equipment or devices. The device embodiments may be implemented by software, or by hardware, or by a combination of hardware and software. The software implementation is taken as an example, and as a logical device, the device is formed by reading corresponding computer program instructions in the nonvolatile memory into the memory for running through the processor of any device with data processing capability. From a hardware aspect, as shown in fig. 5, the present invention is a hardware structure diagram of any device with data processing capability in which the multipoint cloud map fusion apparatus based on offline global optimization is located, except for the processor, the memory, the network interface, and the nonvolatile memory shown in fig. 5, any device with data processing capability in which the apparatus is located in the embodiment may also include other hardware according to the actual function of the any device with data processing capability, which is not described again.
The implementation process of the functions and actions of each unit in the above device is specifically described in the implementation process of the corresponding step in the above method, and is not described herein again.
For the device embodiments, since they substantially correspond to the method embodiments, reference may be made to the partial description of the method embodiments for relevant points. The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of the invention. One of ordinary skill in the art can understand and implement it without inventive effort.
The embodiment of the invention also provides a computer-readable storage medium, wherein a program is stored on the computer-readable storage medium, and when the program is executed by a processor, the multipoint cloud map fusion method based on the offline global optimization in the embodiment is realized.
The computer readable storage medium may be an internal storage unit, such as a hard disk or a memory, of any data processing capability device described in any of the foregoing embodiments. The computer readable storage medium may also be an external storage device of the wind turbine, such as a plug-in hard disk, a Smart Media Card (SMC), an SD Card, a Flash memory Card (Flash Card), and the like, provided on the device. Further, the computer readable storage medium may include both an internal storage unit and an external storage device of any data processing capable device. The computer-readable storage medium is used for storing the computer program and other programs and data required by the arbitrary data processing-capable device, and may also be used for temporarily storing data that has been output or is to be output.
The above description is only a preferred embodiment of the present invention, and is not intended to limit the present invention in any way. Although the foregoing has described the practice of the present invention in detail, it will be apparent to those skilled in the art that modifications may be made to the practice of the invention as described in the foregoing examples, or that certain features may be substituted in the practice of the invention. All changes, equivalents and modifications which come within the spirit and scope of the invention are desired to be protected.

Claims (8)

1. A multipoint cloud map fusion method based on offline global optimization is characterized by comprising the following steps:
step 1, based on the flow of an open source laser SLAM method-LIO-SAM, carrying out GPS information initialization adaptation, simultaneously constructing self-constraint during and after the operation process of using the open source laser SLAM method, and reserving keyframe position and attitude information, point cloud information and constraint information, wherein the constraint information comprises: an odometry factor, a GPS factor and a loopback factor;
step 2, sequentially obtaining a plurality of point cloud maps from a plurality of data sets for point cloud fusion according to the laser SLAM method in the step 1;
step 3, off-line loading pose information of each point cloud map, and constructing a mileometer factor, a GPS factor and a loopback factor among poses in each point cloud map;
step 4, constructing mutual constraint factors among different point cloud maps;
and 5, constructing a point cloud map fusion cost function according to the loaded self-constraint factor and the constructed mutual constraint factor, and performing global optimization to obtain fusion point cloud and all key frame information.
2. The multipoint cloud map fusion method based on the offline global optimization as claimed in claim 1, wherein the step 1 specifically comprises the following substeps:
step 1.1, using GPS information as an initial pose of a first frame key frame, and adding a GPS factor to restrain the first frame key frame; first frame key frame is recorded as
Figure DEST_PATH_IMAGE001
Using GPS information
Figure 590069DEST_PATH_IMAGE002
Initializing, and adding initial time GPS factor
Figure DEST_PATH_IMAGE003
Wherein
Figure 492166DEST_PATH_IMAGE004
Representing a mapping function between the keyframe pose information and the GPS information,
Figure DEST_PATH_IMAGE005
representing a variance matrix;
step 1.2, selecting key frames according to distance threshold and angle threshold in the running process of laser SLAM
Figure 300722DEST_PATH_IMAGE006
，
Figure DEST_PATH_IMAGE007
Adding and recording odometry factors between adjacent key frames
Figure 796425DEST_PATH_IMAGE008
Wherein
Figure DEST_PATH_IMAGE009
And
Figure 715839DEST_PATH_IMAGE010
respectively representing pose transformation functions and odometer observation information between adjacent key frames,
Figure DEST_PATH_IMAGE011
representing a variance matrix; running loop detection on-line, adding loop factor
Figure 738022DEST_PATH_IMAGE012
Wherein
Figure DEST_PATH_IMAGE013
And
Figure 717479DEST_PATH_IMAGE014
respectively representing pose transformation function and loop observation information between loop key frames,
Figure DEST_PATH_IMAGE015
representing a variance matrix; simultaneously adding and recording GPS factors according to the distance threshold value
Figure 825113DEST_PATH_IMAGE016
；
Step 1.3, when the laser SLAM operation is finished, constructing a cost function for generating a single point cloud map, and recording all key frame pose information and corresponding point cloud information, wherein the formula is as follows:
Figure 954743DEST_PATH_IMAGE018
wherein the content of the first and second substances,
Figure DEST_PATH_IMAGE019
and
Figure 831432DEST_PATH_IMAGE020
respectively indicating whether a GPS factor and a loop factor exist, and obtaining a key frame sequence by optimizing a cost function
Figure DEST_PATH_IMAGE021
So as to obtain the point cloud map,
Figure 981790DEST_PATH_IMAGE022
a cost function of a single point cloud map, wherein
Figure DEST_PATH_IMAGE023
Is shown as
Figure 311141DEST_PATH_IMAGE024
A sequence of key frames.
3. The multipoint cloud map fusion method based on the offline global optimization according to claim 2, wherein the step 2 specifically comprises: using multiple off-line datasets needed for point cloud map fusion
Figure DEST_PATH_IMAGE025
Obtaining a plurality of corresponding point cloud maps by adopting the open source laser SLAM method in the step 1 and recording the point cloud maps as
Figure 837937DEST_PATH_IMAGE026
The information of the corresponding point cloud maps comprises key frame pose information and corresponding key frame point cloud information, and simultaneously comprises a mileometer factor and a GPS factorAnd a loop back factor.
4. The multipoint cloud map fusion method based on the offline global optimization according to claim 3, wherein the step 3 specifically comprises: offline loading of multiple point cloud maps
Figure 710078DEST_PATH_IMAGE026
Each point cloud map comprises key frame position and attitude information, point cloud information and constraint information, and a odometer factor, a GPS factor and a loopback factor among positions in each point cloud map are sequentially constructed according to the constraint information.
5. The multipoint cloud map fusion method based on the offline global optimization as claimed in claim 4, wherein the step 4 specifically comprises the following substeps;
step 4.1, two point cloud maps with overlapped areas are obtained through traversal
Figure DEST_PATH_IMAGE027
And
Figure 320355DEST_PATH_IMAGE028
，
Figure DEST_PATH_IMAGE029
for point cloud maps
Figure 402580DEST_PATH_IMAGE027
The set of poses it contains is noted
Figure 608433DEST_PATH_IMAGE030
，
Figure DEST_PATH_IMAGE031
Based on a set of poses
Figure 459715DEST_PATH_IMAGE030
Structural location pointsCloud
Figure 358401DEST_PATH_IMAGE032
And corresponding KD tree
Figure DEST_PATH_IMAGE033
；
Step 4.2, according to the point cloud map
Figure 662343DEST_PATH_IMAGE028
Traversing to obtain a pose set
Figure 796521DEST_PATH_IMAGE034
The current frame is recorded as
Figure DEST_PATH_IMAGE035
By using
Figure 502309DEST_PATH_IMAGE033
Searching nearest neighbor key frames to obtain target key frames meeting the distance threshold condition, and recording the target key frames as the target key frames
Figure 306317DEST_PATH_IMAGE036
；
Step 4.3, based on the target key frame
Figure 769659DEST_PATH_IMAGE036
To construct local point cloud map
Figure DEST_PATH_IMAGE037
Constructing a mutual constraint factor according to the point cloud matching result
Figure 176370DEST_PATH_IMAGE038
Wherein
Figure DEST_PATH_IMAGE039
And
Figure 736664DEST_PATH_IMAGE040
respectively representing pose transformation function and mutual constraint observation information between key frames of different point cloud maps,
Figure DEST_PATH_IMAGE041
a variance matrix is represented.
6. The multipoint cloud map fusion method based on the offline global optimization according to claim 5, wherein in the step 5, all the key frame information is obtained, and a specific expression is as follows:
Figure DEST_PATH_IMAGE043
wherein the content of the first and second substances,
Figure 39470DEST_PATH_IMAGE044
the cost function comprises an internal mileometer factor, a loopback factor and a GPS factor of each point cloud map, the second item comprises the mutual constraint factor constructed in the step 4, and then global optimization is carried out to obtain all key frame information
Figure DEST_PATH_IMAGE045
。
7. A multipoint cloud map fusion device based on offline global optimization is characterized by comprising one or more processors and being used for realizing the multipoint cloud map fusion method based on offline global optimization according to any one of claims 1 to 6.
8. A computer-readable storage medium, on which a program is stored, which, when executed by a processor, implements the offline global optimization-based multipoint cloud map fusion method according to any one of claims 1 to 6.
CN202210228899.7A 2022-03-10 2022-03-10 Multipoint cloud map fusion method and device based on offline global optimization Active CN114322994B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210228899.7A CN114322994B (en) 2022-03-10 2022-03-10 Multipoint cloud map fusion method and device based on offline global optimization

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210228899.7A CN114322994B (en) 2022-03-10 2022-03-10 Multipoint cloud map fusion method and device based on offline global optimization

Publications (2)

Publication Number Publication Date
CN114322994A true CN114322994A (en) 2022-04-12
CN114322994B CN114322994B (en) 2022-07-01

Family

ID=81034106

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210228899.7A Active CN114322994B (en) 2022-03-10 2022-03-10 Multipoint cloud map fusion method and device based on offline global optimization

Country Status (1)

Country Link
CN (1) CN114322994B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116147484A (en) * 2023-02-27 2023-05-23 东南大学 Bridge service line shape identification method based on multipoint cloud fusion

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150323672A1 (en) * 2012-11-22 2015-11-12 Geosim Systems Ltd. Point-cloud fusion
CN110490809A (en) * 2019-08-28 2019-11-22 清华大学 Multiple agent co-located and build drawing method and device
CN110706279A (en) * 2019-09-27 2020-01-17 清华大学 Global position and pose estimation method based on information fusion of global map and multiple sensors
CN112132745A (en) * 2019-06-25 2020-12-25 南京航空航天大学 Multi-sub-map splicing feature fusion method based on geographic information
CN112268559A (en) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 Mobile measurement method for fusing SLAM technology in complex environment
CN112862894A (en) * 2021-04-12 2021-05-28 中国科学技术大学 Robot three-dimensional point cloud map construction and expansion method
CN112965063A (en) * 2021-02-11 2021-06-15 深圳市安泽智能机器人有限公司 Robot mapping and positioning method
CN113269094A (en) * 2021-05-26 2021-08-17 中国科学院自动化研究所 Laser SLAM system and method based on feature extraction algorithm and key frame
CN113658337A (en) * 2021-08-24 2021-11-16 哈尔滨工业大学 Multi-mode odometer method based on rut lines
CN113819914A (en) * 2020-06-19 2021-12-21 北京图森未来科技有限公司 Map construction method and device
CN113870318A (en) * 2021-12-02 2021-12-31 之江实验室 Moving target detection system and method based on multi-frame point cloud
CN114018248A (en) * 2021-10-29 2022-02-08 同济大学 Odometer method and map building method integrating coded disc and laser radar
CN114088081A (en) * 2021-10-10 2022-02-25 北京工业大学 Map construction method for accurate positioning based on multi-segment joint optimization
CN114283250A (en) * 2021-12-23 2022-04-05 武汉理工大学 High-precision automatic splicing and optimizing method and system for three-dimensional point cloud map

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150323672A1 (en) * 2012-11-22 2015-11-12 Geosim Systems Ltd. Point-cloud fusion
CN112132745A (en) * 2019-06-25 2020-12-25 南京航空航天大学 Multi-sub-map splicing feature fusion method based on geographic information
CN110490809A (en) * 2019-08-28 2019-11-22 清华大学 Multiple agent co-located and build drawing method and device
CN110706279A (en) * 2019-09-27 2020-01-17 清华大学 Global position and pose estimation method based on information fusion of global map and multiple sensors
CN113819914A (en) * 2020-06-19 2021-12-21 北京图森未来科技有限公司 Map construction method and device
CN112268559A (en) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 Mobile measurement method for fusing SLAM technology in complex environment
CN112965063A (en) * 2021-02-11 2021-06-15 深圳市安泽智能机器人有限公司 Robot mapping and positioning method
CN112862894A (en) * 2021-04-12 2021-05-28 中国科学技术大学 Robot three-dimensional point cloud map construction and expansion method
CN113269094A (en) * 2021-05-26 2021-08-17 中国科学院自动化研究所 Laser SLAM system and method based on feature extraction algorithm and key frame
CN113658337A (en) * 2021-08-24 2021-11-16 哈尔滨工业大学 Multi-mode odometer method based on rut lines
CN114088081A (en) * 2021-10-10 2022-02-25 北京工业大学 Map construction method for accurate positioning based on multi-segment joint optimization
CN114018248A (en) * 2021-10-29 2022-02-08 同济大学 Odometer method and map building method integrating coded disc and laser radar
CN113870318A (en) * 2021-12-02 2021-12-31 之江实验室 Moving target detection system and method based on multi-frame point cloud
CN114283250A (en) * 2021-12-23 2022-04-05 武汉理工大学 High-precision automatic splicing and optimizing method and system for three-dimensional point cloud map

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
TIXIAO SHAN ET AL.: "LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping", 《2020 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116147484A (en) * 2023-02-27 2023-05-23 东南大学 Bridge service line shape identification method based on multipoint cloud fusion

Also Published As

Publication number Publication date
CN114322994B (en) 2022-07-01

Similar Documents

Publication Publication Date Title
US11313684B2 (en) Collaborative navigation and mapping
Zhang et al. Reference pose generation for long-term visual localization via learned features and view synthesis
Chen et al. A survey on deep learning for localization and mapping: Towards the age of spatial machine intelligence
Cai et al. Practical optimal registration of terrestrial LiDAR scan pairs
Jones et al. Visual-inertial navigation, mapping and localization: A scalable real-time causal approach
WO2019169540A1 (en) Method for tightly-coupling visual slam, terminal and computer readable storage medium
CN109461208B (en) Three-dimensional map processing method, device, medium and computing equipment
CN111445526B (en) Method, device and storage medium for estimating pose of image frame
JP2020067439A (en) System and method for estimating position of moving body
Churchill et al. Experience based navigation: Theory, practice and implementation
CN114001733A (en) Map-based consistency efficient visual inertial positioning algorithm
CN114322994B (en) Multipoint cloud map fusion method and device based on offline global optimization
CN108520543B (en) Method, equipment and storage medium for optimizing relative precision map
Li-Chee-Ming et al. UAV navigation system using line-based sensor pose estimation
TW202238449A (en) Indoor positioning system and indoor positioning method
Tseng et al. A new architecture for simultaneous localization and mapping: An application of a planetary rover
Alam et al. A review of recurrent neural network based camera localization for indoor environments
Hinzmann et al. Deep uav localization with reference view rendering
Chen et al. Robust relocalization based on active loop closure for real-time monocular SLAM
Dai et al. A Review of Common Techniques for Visual Simultaneous Localization and Mapping
Hu et al. Accuracy enhancement for the front-end tracking algorithm of RGB-D SLAM
Lim et al. Online 3D reconstruction and 6-DoF pose estimation for RGB-D sensors
Aladem Robust real-time visual odometry for autonomous ground vehicles
Jiang et al. 3D reconstruction of spherical images: a review of techniques, applications, and prospects
Djordjevic et al. An accurate method for 3D object reconstruction from unordered sparse views

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
GR01 Patent grant
GR01 Patent grant