CN115018999B - Multi-robot collaboration dense point cloud map construction method and device - Google Patents
Multi-robot collaboration dense point cloud map construction method and device Download PDFInfo
- Publication number
- CN115018999B CN115018999B CN202210611705.1A CN202210611705A CN115018999B CN 115018999 B CN115018999 B CN 115018999B CN 202210611705 A CN202210611705 A CN 202210611705A CN 115018999 B CN115018999 B CN 115018999B
- Authority
- CN
- China
- Prior art keywords
- map
- point cloud
- dense point
- key frame
- filtering
- 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.)
- Active
Links
- 238000010276 construction Methods 0.000 title claims abstract description 61
- 238000001914 filtration Methods 0.000 claims abstract description 83
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 47
- 238000000034 method Methods 0.000 claims abstract description 39
- 239000013598 vector Substances 0.000 claims abstract description 31
- 230000009466 transformation Effects 0.000 claims abstract description 25
- 238000012545 processing Methods 0.000 claims abstract description 22
- 238000004590 computer program Methods 0.000 claims description 21
- 230000006870 function Effects 0.000 claims description 14
- 238000003860 storage Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 7
- 230000001131 transforming effect Effects 0.000 claims description 7
- 238000003064 k means clustering Methods 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 13
- 239000011159 matrix material Substances 0.000 description 7
- 230000008569 process Effects 0.000 description 6
- 238000000605 extraction Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000013077 scoring method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Geometry (AREA)
- Software Systems (AREA)
- Computer Graphics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
The embodiment of the invention provides a method and a device for constructing a dense point cloud map by cooperation of multiple robots, wherein the method comprises the following steps: acquiring RGB-D images of key frames respectively acquired by two robots, extracting ORB characteristic data according to the RGB-D images, and calculating to obtain pose relations among the key frames; the extracted RGB-D image is subjected to downsampling filtering to screen map points, a pinhole camera model is utilized to carry out key frame dense point cloud map construction, and a key frame map is added to a local map through a key frame pose relation; processing the local map through voxel filtering to respectively finish the construction of dense point cloud maps of the two robots; calculating and matching the distance between two dense point cloud map key frame image word vectors by using a word bag model constructed by ORB characteristics, estimating the transformation relation of the two maps by using an ICP algorithm, fusing the two maps, performing primary voxel filtering and statistical filtering processing, and constructing a multi-robot dense point cloud map; the multi-robot SLAM map construction is realized under different scenes, and the method has better real-time performance and precision and better application prospect.
Description
Technical Field
The present invention relates to the field of computer technologies, and in particular, to a method for constructing a dense point cloud map by multi-robot cooperation, a device for constructing a dense point cloud map by multi-robot cooperation, a computer device, and a storage medium.
Background
Visual SLAM (Visual Simultaneous Localization AND MAPPING, V-SLAM) is an important research direction in the field of computer vision, and this technology has been widely applied to three-dimensional reconstruction, autopilot, augmented reality, etc. At present, V-SLAM has developed a plurality of relatively mature open source algorithms, such as MonoSLAM, PTAM, ORB-SLAM, LSD-SLAM, SVO, DTAM, DVO, DSO, VINS, RGBD-SLAM-V2, DSM and the like, which have better efficiency and precision for single-robot SLAM, but are not used for multi-robot SLAM. Among a plurality of SLAM algorithms, ORB-SLAM3 is one of the algorithms with the best comprehensive performance, and has the advantages of high feature extraction efficiency, high positioning accuracy, support of various sensors, support of loop detection, repositioning, multi-map management and the like. However, the method has the defects of less information quantity of the constructed sparse point cloud map, lower map construction efficiency of the single robot and the like.
Although the sparse point cloud can describe the surrounding environment to a certain extent, the positioning requirement of the robot in certain scenes is met, the requirements of robot navigation obstacle avoidance and the like cannot be met due to the fact that the distance between the points is large. The difference between the dense point cloud and the sparse point cloud is that the number of the map points of the dense point cloud is more dense, the environment can be described in more detail, the navigation obstacle avoidance requirement of the robot can be met after further processing, but the traditional algorithm for constructing the dense point cloud has the defects of higher calculation force and memory requirement of a computing platform and the like.
Research on single-robot SLAM algorithms has been relatively mature, but multi-robot SLAM has higher efficiency and robustness, so research on multi-robot SLAM is becoming a hotspot. Early multi-robot SLAM was mainly based on filtering algorithms, such as using constrained local sub-map filters (Constrained Local Submap Filter, CLSF), but the algorithms had the disadvantages of lower accuracy, limitation to indoor environments, etc. The distributed multi-robot SLAM algorithm based on Kimera algorithm has higher precision and robustness, but has higher calculation force requirement, and the real-time performance is poorer under a non-GPU platform, and the method for solving the problems is to adopt cloud computing, but also has the problem that continuous operation cannot be carried out when a communication signal is unstable.
Disclosure of Invention
In view of the above problems, embodiments of the present invention have been made to provide a multi-robot-cooperative dense point cloud map construction method, a multi-robot-cooperative dense point cloud map construction apparatus, a computer device, and a storage medium that overcome or at least partially solve the above problems.
In order to solve the above problems, the embodiment of the invention discloses a method for constructing a dense point cloud map by multi-robot cooperation, which comprises the following steps:
Acquiring RGB-D images of key frames respectively acquired by two robots, extracting ORB characteristic data according to the RGB-D images, and calculating to obtain pose relations among the key frames;
The extracted RGB-D image is subjected to downsampling filtering to screen map points, a pinhole camera model is utilized to carry out key frame dense point cloud map construction, and a key frame map is added to a local map through a key frame pose relation;
Processing the local map through voxel filtering to respectively finish the construction of dense point cloud maps of the two robots;
And calculating and matching the distance between the two dense point cloud map key frame image word vectors by using the word bag model constructed by the ORB characteristics, estimating the transformation relation of the two maps by using an ICP algorithm, fusing the two maps, performing voxel filtering and statistical filtering processing once, and constructing the multi-robot dense point cloud map.
Preferably, the step of downsampling filtering the extracted RGB-D image to screen map points, and then constructing a dense point cloud map of the key frame by using a pinhole camera model, and adding the key frame map to the local map through the pose relationship of the key frame comprises the following steps:
the RGB-D image is subjected to downsampling filtering in an interlaced and spaced-column sampling mode;
converting the downsampled and filtered data into a dense point cloud map of the key frame, and transforming the map into a local map by using the pose of the key frame;
Voxel filtering is performed for the local map.
Preferably, the computing and matching are performed on the distance between two dense point cloud map key frame image word vectors by using the word bag model constructed by using the ORB feature, the transformation relationship between the two maps is estimated by using the ICP algorithm, the two maps are fused and subjected to voxel filtering and statistical filtering processing once, and a multi-robot dense point cloud map is constructed, which comprises:
Constructing a K-ary tree dictionary model;
word vector matching is carried out aiming at the image characteristics;
and estimating the transformation relation of the two maps through an ICP algorithm, and registering.
Preferably, the constructing the K-tree dictionary model includes:
Extracting ORB characteristics;
clustering the features by using a K-means clustering method to generate words;
k-ary dictionary models are constructed for storing words.
Preferably, the performing word vector matching on the image features includes:
Extracting image features, judging the belonged words, and counting the category and frequency of the word;
Calculating weights by a TF-IDF method;
Constructing a word vector;
And performing scoring calculation on the similarity of the word vectors, performing Gaussian filtering on the scores, and identifying image pairs with similarity scores higher than a preset threshold.
Preferably, the estimating the transformation relation of the two maps by ICP algorithm, registering, includes:
Extracting and matching ORB characteristics;
Constructing an error function
And solving the constructed error function through an ICP algorithm, and registering.
The embodiment of the invention discloses a dense point cloud map construction device with multi-robot cooperation, which comprises the following components:
the acquisition module is used for acquiring RGB-D images of the key frames acquired by the two robots respectively, extracting ORB characteristic data according to the RGB-D images and calculating to obtain the pose relation between the key frames;
the local map adding module is used for carrying out downsampling filtering on the extracted RGB-D image to screen map points, carrying out key frame dense point cloud map construction by utilizing a pinhole camera model, and adding a key frame map to the local map through a key frame pose relation;
The dense point cloud map construction module is used for processing the local map through voxel filtering and respectively completing the construction of dense point cloud maps of the two robots;
The multi-robot dense point cloud map construction module is used for calculating and matching the distance between two dense point cloud map key frame image word vectors by utilizing a word bag model constructed by ORB characteristics, estimating the transformation relation of the two maps by utilizing an ICP algorithm, fusing the two maps, performing voxel filtering and statistical filtering processing once, and constructing the multi-robot dense point cloud map.
Preferably, the local map adding module includes:
The downsampling and filtering sub-module is used for downsampling and filtering the RGB-D image in an interlaced and spaced-column sampling mode;
the local map transformation sub-module is used for converting the downsampled and filtered data into a dense point cloud map of the key frame, and transforming the map into a local map by utilizing the pose of the key frame;
and the voxel filtering sub-module is used for carrying out voxel filtering on the local map.
The embodiment of the invention discloses a computer device, which comprises a memory and a processor, wherein the memory stores a computer program, and the processor realizes the steps of the multi-robot collaborative dense point cloud map construction method when executing the computer program.
The embodiment of the invention discloses a computer readable storage medium, on which a computer program is stored, wherein the computer program realizes the steps of the multi-robot collaborative dense point cloud map construction method when being executed by a processor.
The embodiment of the invention has the following advantages:
In the embodiment of the invention, a composition scheme based on an ORB-SLAM3 algorithm provides a two-layer filtering algorithm to screen map points, so that the construction of a dense point cloud map of a single robot is efficiently realized, and the problem that the traditional dense point cloud map construction algorithm has high calculation force and memory requirements is solved; and combining the word bag model, providing a scheme for indirectly registering the dense point cloud map by matching the map key frame image, and finally, filtering and denoising the map by utilizing voxel filtering and statistical filtering. Compared with a comparison algorithm, the experimental result shows that the algorithm can reliably realize multi-robot SLAM map construction under different scenes, has better instantaneity and accuracy, and has better application prospect.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed in the description of the embodiments will be briefly described below, it will be apparent that the drawings in the following description are only some embodiments of the present invention, and other drawings can be obtained according to these drawings without inventive effort for a person skilled in the art
FIG. 1 is a flow chart of steps of an embodiment of a multi-robot collaborative dense point cloud map construction method in accordance with an embodiment of the present invention;
FIG. 2 is a frame diagram of an embodiment of a multi-robot collaborative dense point cloud map construction method in accordance with an embodiment of the present invention;
FIG. 3 is a flowchart of the steps of dense point cloud map construction according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a dictionary construction process according to an embodiment of the present invention;
FIG. 5 is a schematic representation of a three-dimensional coordinate transformation according to an embodiment of the present invention;
FIG. 6 is a block diagram of an embodiment of a multi-robot collaborative dense point cloud map construction apparatus in accordance with an embodiment of the present invention;
FIG. 7 is an internal block diagram of a computer device of one embodiment.
Detailed Description
In order to make the technical problems, technical schemes and beneficial effects solved by the embodiments of the present invention more clear, the embodiments of the present invention are further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
Referring to fig. 1, a step flow chart of an embodiment of a multi-robot collaborative dense point cloud map construction method according to the embodiment of the present invention is shown, which may specifically include the following steps:
Step 101, acquiring RGB-D images of key frames respectively acquired by two robots, extracting ORB characteristic data according to the RGB-D images, and calculating to obtain pose relations among the key frames;
The framework diagram of the multi-robot collaborative dense point cloud map construction method is shown in fig. 2. Firstly, two robots carrying RGB-D cameras respectively run ORB-SLAM3 algorithm, RGB-D images, pose and ORB characteristic data of all key frames are respectively extracted from a key frame database, then downsampling filtering is carried out on the extracted RGB-D images to screen map points, a pinhole camera model is utilized to carry out key frame dense point cloud map construction, a key frame map is added to a local map through a key frame pose relation, finally the local map is processed through voxel filtering, and the construction of two single robot dense point cloud maps is completed. And calculating and matching the distance between the word vectors of the key frame images of the two maps by using the word bag model constructed by the ORB characteristics, estimating the transformation relation of the two maps by using an ICP algorithm, fusing the two maps, performing voxel filtering and statistical filtering processing once, and finally constructing the dense point cloud map of the multiple robots.
102, Carrying out downsampling filtering on the extracted RGB-D image to screen map points, carrying out key frame dense point cloud map construction by utilizing a pinhole camera model, and adding a key frame map to a local map through a key frame pose relation;
in the embodiment of the invention, a pinhole camera model can be abstracted into a mathematical model type (1);
The formula (1) can be transformed to obtain the formula (2);
PC＝ZK-1Puv (2)
Wherein P uv is the homogeneous coordinates of any pixel in the RGB map; k -1 is the inverse matrix of the camera internal parameters; z is the depth of the pixel point, and P C is the coordinates of the point in the camera coordinate system.
The two-layer filtering composition algorithm provided by the method can ensure that the composition speed is improved to a certain extent and the memory consumption is reduced on the premise that the map is not severely distorted.
The flow of the dense point cloud map construction of the invention is shown in fig. 3, and comprises the following steps:
Step 11: and (5) downsampling and filtering. Downsampling filtering may substantially reduce the amount of data. In traversing the RGB image and the depth image, an interlaced-to-interlaced sampling scheme is employed. The method can reduce the number of map points by more than 75%, and has no serious influence on the composition effect; the RGB-D image is subjected to downsampling filtering in an interlaced and spaced-column sampling mode;
Step 12: and (5) constructing a local dense point cloud map. Converting the downsampled filtered data into a dense point cloud map of the key frame using equation (2), transforming the map into a local map using the pose of the key frame; converting the downsampled and filtered data into a dense point cloud map of the key frame, and transforming the map into a local map by using the pose of the key frame;
Step 13: voxel filtering is performed for the local map. Voxel filtering can achieve the effect of downsampling without destroying the geometry of the point cloud itself. When more key frame maps are added to the local map, the point cloud is rasterized, and the center of gravity of all points in the voxel grid is used for representing all points in the grid. The method requires a large amount of calculation, but the voxel filtering can also ensure a certain degree of real-time performance after the step 1 downsampling and filtering.
Step 103, processing the local map through voxel filtering to respectively complete the construction of dense point cloud maps of the two robots;
and 104, calculating and matching the distance between the two dense point cloud map key frame image word vectors by using the word bag model constructed by the ORB characteristics, estimating the transformation relation of the two maps by using an ICP algorithm, fusing the two maps, performing voxel filtering and statistical filtering processing once, and constructing the multi-robot dense point cloud map.
The embodiment of the invention comprises the following steps: constructing a K-ary tree dictionary model; word vector matching is carried out aiming at the image characteristics; and estimating the transformation relation of the two maps through an ICP algorithm, and registering.
Further, the bag of words model (Bag of Words model, boW) is a method of describing complex information. The visual word bag model classifies the image feature descriptors into words through a clustering algorithm, and a K-ary dictionary is constructed for convenient retrieval. And counting the category and frequency of the word appearing in the image to be matched, calculating the weight of the word, and converting the weight of all the words into a word vector to describe the image. The similarity of the images can be judged by calculating the word vector distance of the two images.
The K-ary tree dictionary model construction method comprises the following steps of:
Step 21: and (5) extracting characteristics. ORB features are adopted in the embodiment of the invention, and common image features include SITF, SURF and the like;
Step 22: and generating a word. Clustering the features by a K-means clustering method to generate words, as shown in part (a) of FIG. 4;
Step 23: dictionary construction. Constructing KD-tree (K-tree dictionary model) to store words, which is convenient for word retrieval, as shown in part (b) in FIG. 4.
The word vector matching for the image features comprises the following steps:
step 31: word statistics. Extracting image features, judging the belonged words, and counting the category and frequency of the word;
step 32: and (5) calculating the weight of the word. Calculating weights by using a TF-IDF method;
TF i refers to the frequency of occurrence of the i-th word in the image, and defines the number of occurrences of the i-th word in the image, N i, over the total number of words in the image, N, as in equation (3);
IDF i refers to the negative logarithm of the ratio of the number of features n i of the leaf node where the i-th word is located to the number of all features n, as in equation (4);
The i-th word weight w i is the product of TF i and IDF i, as in formula (5);
wi＝TFi*IDFi (5)
Step 33: and constructing a word vector. Defining a word vector V n of the nth image as formula (6);
Vn＝[w1,w2,w3,......,wj] (6)
Step 34: and (5) calculating similarity scores. Common similarity scoring methods are dot product, L1 norm, L2 norm, KL divergence/relative entropy, papanicolaou distance, chi-square distance, etc. To ensure matching speed, a simpler-to-calculate L1 norm is employed herein. The similarity score S is calculated according to the formula (7), wherein V ij represents the word vector of the image at the j moment of the ith map.
Step 35: similarity score gaussian filtering. Because the RGB-D images are sampled at successive time intervals, the source and target images and their neighboring images can be considered similar. The adjacent similarity scores are weighted through a Gaussian filter algorithm, so that mismatching can be reduced. The calculation formula of the similarity score S guass after Gaussian filtering is shown as formula (8);
step 36: and (5) image matching. Image pairs with similarity scores above a threshold are selected.
Further, the estimating the transformation relation of the two maps by the ICP algorithm for registering comprises the following steps:
step 41: extracting and matching ORB characteristics;
ORB feature extraction and matching. Obtaining a set of matched 3D points P and P' in the similar image as shown in formula (9)
Step 42: constructing an error function;
(1) Assuming that the pose transformation relation rotation matrix of the matched image is R 3×3 and the translation phasor is t 3×1, the ith pair of matching points has a constraint relation of formula (10);
(2) Defining an error term of an ith point pair as formula (11);
ei＝pi-(Rp′i+t) (11)
Step 43: and solving the constructed error function through an ICP algorithm, and registering.
ICP solution can construct a least squares problem as in equation (12), and solve R, t using SVD to minimize the sum of squares error.
The calculation steps are as follows:
(1) Calculating centroids of the two sets of point clouds, denoted μ and μ', respectively, as in formula (13) (14);
(2) Constructing a matrix H after centroid removal, as shown in formula (15);
(3) SVD decomposition is carried out on the matrix H to obtain a formula (16);
H＝UΣVT (16)
(4) The result is calculated, the rotation matrix R is shown as a formula (17), and the translation phasor t is shown as a formula (18). R, t are integrated into a homogeneous transformation matrix as shown in formula (19).
R＝VUT (17)
t＝μ'-Rμ (18)
The method is further applied to the invention, and the steps of fusing two maps and carrying out one-time voxel filtering and statistical filtering processing to construct the multi-robot dense point cloud map comprise the following steps:
Step 51: two map coordinate transformation relations T m are calculated. Two similar images are obtained through a word bag model, and the transformation matrix T cc between the images is calculated through an ICP algorithm under the assumption that the camera external parameters of the images are T c1 and T c2 respectively. Let the coordinates of a point on the map in the two map coordinate systems be p and p', respectively, as shown in fig. 5.
The constraint condition of the two map coordinate systems is represented by a formula (20);
p＝Tmp' (20)
The constraint condition between the two images is represented by formula (21);
Integrating formula (20) and formula (21), and obtaining formula (22);
step 52: the RANSAC algorithm is optimized. Because more than one pair of similar images are judged through the word bag model, mismatching can exist, the obtained T m is not unique, the correctness of the obtained T m is ensured, the outliers are needed to be screened through a RANSAC algorithm, and the optimal T m is calculated.
Step 53: voxel filtering. After the map fusion, two parts which are necessarily overlapped need to be subjected to voxel filtering to remove the repeated parts so as to reduce the memory consumption; for a specific voxel filtering step reference is made to step 13.
Step 54: and (5) statistical filtering. Because infrared light of an RGB-D camera is easily influenced by natural light, a certain number of outliers with noise exist in data, and statistical filtering is needed to remove noise points. The statistical filtering assumes that the average distance between all points and the nearest K points satisfies a gaussian distribution, and the variance of the average distance is calculated to determine a threshold, and when the average distance of a certain point is greater than the threshold, the point is determined to be an outlier. Because the statistical filtering is computationally intensive and has only the effect of removing noise points that are not significantly affected, this is only done before the final output map.
In the embodiment of the invention, a composition scheme based on an ORB-SLAM3 algorithm provides a two-layer filtering algorithm to screen map points, so that the construction of a dense point cloud map of a single robot is efficiently realized, and the problem that the traditional dense point cloud map construction algorithm has high calculation force and memory requirements is solved; and combining the word bag model, providing a scheme for indirectly registering the dense point cloud map by matching the map key frame image, and finally, filtering and denoising the map by utilizing voxel filtering and statistical filtering. Compared with a comparison algorithm, the experimental result shows that the algorithm can reliably realize multi-robot SLAM map construction under different scenes, has better instantaneity and accuracy, and therefore has better application prospect.
It should be noted that, for simplicity of description, the method embodiments are shown as a series of acts, but it should be understood by those skilled in the art that the embodiments are not limited by the order of acts, as some steps may occur in other orders or concurrently in accordance with the embodiments. Further, those skilled in the art will appreciate that the embodiments described in the specification are presently preferred embodiments, and that the acts are not necessarily required by the embodiments of the invention.
Referring to fig. 6, a block diagram of an embodiment of a multi-robot collaborative dense point cloud map building apparatus according to an embodiment of the present invention is shown, and may specifically include the following modules:
the acquisition module 301 is configured to acquire RGB-D images of key frames acquired by two robots respectively, extract ORB feature data according to the RGB-D images, and calculate a pose relationship between the key frames;
The local map adding module 302 is configured to downsample, filter and screen map points of the extracted RGB-D image, perform key frame dense point cloud map construction by using a pinhole camera model, and add a key frame map to the local map through a key frame pose relationship;
The dense point cloud map construction module 303 is configured to process the local map through voxel filtering, and respectively complete the construction of dense point cloud maps of the two robots;
The multi-robot dense point cloud map construction module 304 is configured to calculate and match distances between two dense point cloud map key frame image word vectors by using a word bag model constructed by ORB features, estimate a transformation relationship of the two maps by using an ICP algorithm, fuse the two maps, and perform voxel filtering and statistical filtering processing once to construct a multi-robot dense point cloud map.
Preferably, the local map adding module includes:
The downsampling and filtering sub-module is used for downsampling and filtering the RGB-D image in an interlaced and spaced-column sampling mode;
the local map transformation sub-module is used for converting the downsampled and filtered data into a dense point cloud map of the key frame, and transforming the map into a local map by utilizing the pose of the key frame;
and the voxel filtering sub-module is used for carrying out voxel filtering on the local map.
Preferably, the multi-robot dense point cloud map building module includes:
the K-ary tree dictionary construction submodule is used for constructing a K-ary tree dictionary model;
the word vector matching sub-module is used for matching the word vector aiming at the image characteristics;
and the registration sub-module is used for estimating the transformation relation of the two maps through an ICP algorithm and registering.
Preferably, the K-tree dictionary building submodule includes:
A feature extraction unit for extracting ORB features;
the word generating unit is used for clustering the characteristics by using a K-means clustering method to generate words;
and the construction unit is used for constructing the K-tree dictionary model for storing words.
Preferably, the word vector matching submodule includes:
The statistics unit is used for extracting image characteristics, judging the belonged words and counting the category and frequency of the word;
A weight calculation unit for calculating weights by a TF-IDF method;
A word vector construction unit for constructing a word vector;
and the computing unit is used for carrying out scoring computation on the similarity of the word vectors, carrying out Gaussian filtering on the scores, and identifying image pairs with similarity scores higher than a preset threshold value.
Preferably, the registration submodule includes:
An extraction and matching unit for extracting and matching ORB features;
an error function construction unit for constructing an error function
And the registration unit is used for solving the constructed error function through an ICP algorithm and registering.
The various modules in the multi-robot collaborative dense point cloud map construction apparatus described above may be implemented in whole or in part by software, hardware, and combinations thereof. The above modules may be embedded in hardware or may be independent of a processor in the computer device, or may be stored in software in a memory in the computer device, so that the processor may call and execute operations corresponding to the above modules.
The multi-robot collaborative dense point cloud map construction device provided by the invention can be used for executing the multi-robot collaborative dense point cloud map construction method provided by any embodiment, and has corresponding functions and beneficial effects.
In one embodiment, a computer device is provided, which may be a terminal, and the internal structure of which may be as shown in fig. 7. The computer device includes a processor, a memory, a network interface, a display screen, and an input device connected by a system bus. Wherein the processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface of the computer device is used for communicating with an external terminal through a network connection. The computer program, when executed by a processor, implements a multi-robot collaborative dense point cloud map construction method. The display screen of the computer equipment can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer equipment can be a touch layer covered on the display screen, can also be keys, a track ball or a touch pad arranged on the shell of the computer equipment, and can also be an external keyboard, a touch pad or a mouse and the like.
It will be appreciated by those skilled in the art that the structure shown in FIG. 7 is merely a block diagram of some of the structures associated with the present inventive arrangements and is not limiting of the computer device to which the present inventive arrangements may be applied, and that a particular computer device may include more or fewer components than shown, or may combine some of the components, or have a different arrangement of components.
In one embodiment, a computer device is provided comprising a memory having a computer program stored therein and a processor that when executing the computer program performs the steps of the embodiments of fig. 1-2.
In one embodiment, a computer readable storage medium is provided having a computer program stored thereon, which when executed by a processor, implements the steps of the embodiments of fig. 1-2 below.
In this specification, each embodiment is described in a progressive manner, and each embodiment is mainly described by differences from other embodiments, and identical and similar parts between the embodiments are all enough to be referred to each other.
It will be apparent to those skilled in the art that embodiments of the present invention may be provided as a method, apparatus, or computer program product. Accordingly, embodiments of the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment or an embodiment combining software and hardware aspects. Furthermore, embodiments of the invention may take the form of a computer program product on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.
Embodiments of the present invention are described with reference to flowchart illustrations and/or block diagrams of methods, terminal devices (systems), and computer program products according to embodiments of the invention. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing terminal device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal device, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
While preferred embodiments of the present invention have been described, additional variations and modifications in those embodiments may occur to those skilled in the art once they learn of the basic inventive concepts. It is therefore intended that the following claims be interpreted as including the preferred embodiment and all such alterations and modifications as fall within the scope of the embodiments of the invention.
Finally, it is further noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or terminal that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or terminal. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or terminal device that comprises the element.
The above description of the multi-robot collaborative dense point cloud map construction method, the multi-robot collaborative dense point cloud map construction device, the computer equipment and the storage medium provided by the invention applies specific examples to explain the principles and the implementation of the invention, and the description of the above examples is only used for helping to understand the method and the core ideas of the invention; meanwhile, as those skilled in the art will have variations in the specific embodiments and application scope in accordance with the ideas of the present invention, the present description should not be construed as limiting the present invention in view of the above.
Claims (10)
1. The method for constructing the dense point cloud map by the cooperation of multiple robots is characterized by comprising the following steps of:
Acquiring RGB-D images of key frames respectively acquired by two robots, extracting ORB characteristic data according to the RGB-D images, and calculating to obtain pose relations among the key frames;
The extracted RGB-D image is subjected to downsampling filtering to screen map points, a pinhole camera model is utilized to carry out key frame dense point cloud map construction, and a key frame map is added to a local map through a key frame pose relation;
Processing the local map through voxel filtering to respectively finish the construction of dense point cloud maps of the two robots;
And calculating and matching the distance between the two dense point cloud map key frame image word vectors by using the word bag model constructed by the ORB characteristics, estimating the transformation relation of the two maps by using an ICP algorithm, fusing the two maps, performing voxel filtering and statistical filtering processing once, and constructing the multi-robot dense point cloud map.
2. The method for constructing a dense point cloud map with multi-robot cooperation according to claim 1, wherein the step of downsampling the extracted RGB-D image to filter map points, and then constructing a dense point cloud map of a key frame by using a pinhole camera model, and adding the map of the key frame to the local map by using a pose relationship of the key frame comprises the steps of:
the RGB-D image is subjected to downsampling filtering in an interlaced and spaced-column sampling mode;
converting the downsampled and filtered data into a dense point cloud map of the key frame, and transforming the map into a local map by using the pose of the key frame;
Voxel filtering is performed for the local map.
3. The method for constructing a dense point cloud map with cooperation of multiple robots according to claim 1, wherein the word bag model constructed by using ORB features calculates and matches distances between two dense point cloud map key frame image word vectors, estimates a transformation relationship of the two maps by using ICP algorithm, merges the two maps and performs voxel filtering and statistical filtering processing once, and constructs the dense point cloud map with multiple robots, comprising:
Constructing a K-ary tree dictionary model;
word vector matching is carried out aiming at the image characteristics;
and estimating the transformation relation of the two maps through an ICP algorithm, and registering.
4. A method of multi-robot collaborative dense point cloud map construction according to claim 3, wherein the constructing a K-ary tree dictionary model includes:
Extracting ORB characteristics;
clustering the features by using a K-means clustering method to generate words;
k-ary dictionary models are constructed for storing words.
5. The method for constructing a dense point cloud map with multi-robot collaboration according to claim 3, wherein the performing word vector matching for image features comprises:
Extracting image features, judging the belonged words, and counting the category and frequency of the word;
Calculating weights by a TF-IDF method;
Constructing a word vector;
And performing scoring calculation on the similarity of the word vectors, performing Gaussian filtering on the scores, and identifying image pairs with similarity scores higher than a preset threshold.
6. A method of constructing a dense point cloud map for multi-robot collaboration according to claim 3, wherein estimating the transformation relationship between the two maps by ICP algorithm, and registering, comprises:
Extracting and matching ORB characteristics;
Constructing an error function
And solving the constructed error function through an ICP algorithm, and registering.
7. A multi-robot collaborative dense point cloud map construction apparatus, comprising:
the acquisition module is used for acquiring RGB-D images of the key frames acquired by the two robots respectively, extracting ORB characteristic data according to the RGB-D images and calculating to obtain the pose relation between the key frames;
the local map adding module is used for carrying out downsampling filtering on the extracted RGB-D image to screen map points, carrying out key frame dense point cloud map construction by utilizing a pinhole camera model, and adding a key frame map to the local map through a key frame pose relation;
The dense point cloud map construction module is used for processing the local map through voxel filtering and respectively completing the construction of dense point cloud maps of the two robots;
The multi-robot dense point cloud map construction module is used for calculating and matching the distance between two dense point cloud map key frame image word vectors by utilizing a word bag model constructed by ORB characteristics, estimating the transformation relation of the two maps by utilizing an ICP algorithm, fusing the two maps, performing voxel filtering and statistical filtering processing once, and constructing the multi-robot dense point cloud map.
8. The multi-robot collaboration dense point cloud mapping apparatus of claim 7, wherein the local map adding module comprises:
The downsampling and filtering sub-module is used for downsampling and filtering the RGB-D image in an interlaced and spaced-column sampling mode;
the local map transformation sub-module is used for converting the downsampled and filtered data into a dense point cloud map of the key frame, and transforming the map into a local map by utilizing the pose of the key frame;
and the voxel filtering sub-module is used for carrying out voxel filtering on the local map.
9. A computer device comprising a memory and a processor, the memory storing a computer program, characterized in that the processor, when executing the computer program, implements the steps of the multi-robot collaborative dense point cloud map construction method of any of claims 1-6.
10. A computer readable storage medium having stored thereon a computer program, characterized in that the computer program when executed by a processor implements the steps of the multi-robot collaborative dense point cloud map construction method of any of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210611705.1A CN115018999B (en) | 2022-05-31 | 2022-05-31 | Multi-robot collaboration dense point cloud map construction method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210611705.1A CN115018999B (en) | 2022-05-31 | 2022-05-31 | Multi-robot collaboration dense point cloud map construction method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115018999A CN115018999A (en) | 2022-09-06 |
| CN115018999B true CN115018999B (en) | 2024-10-29 |
Family
ID=83071039
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210611705.1A Active CN115018999B (en) | 2022-05-31 | 2022-05-31 | Multi-robot collaboration dense point cloud map construction method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115018999B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115965673B (en) * | 2022-11-23 | 2023-09-12 | 中国建筑一局(集团)有限公司 | Centralized multi-robot positioning method based on binocular vision |
| CN116030213B (en) * | 2023-03-30 | 2023-06-06 | 千巡科技(深圳)有限公司 | Multi-machine cloud edge collaborative map creation and dynamic digital twin method and system |
| CN118279515B (en) * | 2024-06-04 | 2024-07-30 | 西北工业大学 | Real-time multi-terminal map fusion method and device |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110264563A (en) * | 2019-05-23 | 2019-09-20 | 武汉科技大学 | A kind of Octree based on ORBSLAM2 builds drawing method |
| CN111325843A (en) * | 2020-03-09 | 2020-06-23 | 北京航空航天大学 | Real-time semantic map construction method based on semantic inverse depth filtering |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021097618A1 (en) * | 2019-11-18 | 2021-05-27 | 深圳市大疆创新科技有限公司 | Point cloud segmentation method and system, and computer storage medium |
| CN111795704B (en) * | 2020-06-30 | 2022-06-03 | 杭州海康机器人技术有限公司 | Method and device for constructing visual point cloud map |
-
2022
- 2022-05-31 CN CN202210611705.1A patent/CN115018999B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110264563A (en) * | 2019-05-23 | 2019-09-20 | 武汉科技大学 | A kind of Octree based on ORBSLAM2 builds drawing method |
| CN111325843A (en) * | 2020-03-09 | 2020-06-23 | 北京航空航天大学 | Real-time semantic map construction method based on semantic inverse depth filtering |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115018999A (en) | 2022-09-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110427877B (en) | Human body three-dimensional posture estimation method based on structural information | |
| CN115018999B (en) | Multi-robot collaboration dense point cloud map construction method and device | |
| CN108764048B (en) | Face key point detection method and device | |
| CN106709461B (en) | Activity recognition method and device based on video | |
| CN112446398B (en) | Image classification method and device | |
| CN110363817B (en) | Target pose estimation method, electronic device, and medium | |
| CN113657560B (en) | Weak supervision image semantic segmentation method and system based on node classification | |
| CN111931764B (en) | Target detection method, target detection frame and related equipment | |
| EP4404148A1 (en) | Image processing method and apparatus, and computer-readable storage medium | |
| CN114937083B (en) | Laser SLAM system and method applied to dynamic environment | |
| CN113034592B (en) | Three-dimensional scene target detection modeling and detection method based on natural language description | |
| Chen et al. | A full density stereo matching system based on the combination of CNNs and slanted-planes | |
| WO2023151237A1 (en) | Face pose estimation method and apparatus, electronic device, and storage medium | |
| CN110852327A (en) | Image processing method, image processing device, electronic equipment and storage medium | |
| CN113850136A (en) | Yolov5 and BCNN-based vehicle orientation identification method and system | |
| CN112906520A (en) | Gesture coding-based action recognition method and device | |
| Li et al. | An aerial image segmentation approach based on enhanced multi-scale convolutional neural network | |
| CN104463962B (en) | Three-dimensional scene reconstruction method based on GPS information video | |
| CN112801945A (en) | Depth Gaussian mixture model skull registration method based on dual attention mechanism feature extraction | |
| CN116385660A (en) | Indoor single view scene semantic reconstruction method and system | |
| CN116402976A (en) | Training method and device for three-dimensional target detection model | |
| Shen et al. | ImLiDAR: cross-sensor dynamic message propagation network for 3D object detection | |
| US20050185834A1 (en) | Method and apparatus for scene learning and three-dimensional tracking using stereo video cameras | |
| CN117854155B (en) | Human skeleton action recognition method and system | |
| CN114283265A (en) | Unsupervised face correcting method based on 3D rotation modeling |
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 |