Three-dimensional mapping method and system

Abstract

The invention relates to the technical field of synchronous positioning and mapping, in particular to a three-dimensional mapping method and system. Extracting feature points of the obtained target map image by using an ORB-SLAM system, and performing three-dimensional visual reconstruction based on the feature points to generate an initial sparse point cloud map; fusing the sparse point cloud data of the target map image acquired by the plurality of sensors to obtain dense point cloud data, and processing the obtained dense point cloud data by using a point cloud segmentation algorithm and a target detection algorithm to obtain target information in the dense point cloud data; and performing spatial segmentation and organization on the dense point cloud data according to the target information in the dense point cloud data by using a tree data structure algorithm to obtain a point cloud map with a hierarchical structure. According to the method, the map data is accurate, and the management of the map data can be efficiently realized.

Description

Background Art

[0002] In recent years, visual SLAM system, as an important autonomous navigation technology, has gradually become the focus of attention in various fields. As a visual SLAM system based on feature points, the design and operation principle of ORB-SLAM has become a milestone achievement in the field of visual navigation and environmental map construction. ORB-SLAM analyzes and extracts feature points in the video sequence captured by the camera sensor, and uses the ORB feature descriptor to describe these feature points. The uniqueness of this feature descriptor lies in its combination of the FAST key point detector and the BRIEF descriptor, which gives it the advantages of fast calculation, low memory consumption and strong robustness. In addition, the directional design of the ORB descriptor enables it to effectively adapt to image rotation and affine transformation, improving the accuracy and stability of matching.

[0003] In terms of real-time positioning and map construction, the ORB-SLAM system achieves efficient real-time positioning and map construction functions by utilizing the video sequences collected by the camera and extracting and matching feature points. Its algorithm design enables the system to respond quickly in a changing environment and effectively track the position of the camera and build an accurate environmental map. These functions are of great significance for applications in unmanned systems, autonomous navigation robots, virtual reality, and other fields, providing strong support for the development of these fields.

[0004] However, the current ORB-SLAM system has shortcomings in map construction, such as insufficient processing of large-scale environments, inability to improve positioning accuracy, and lack of effective management of point cloud map data.

[0005] In response to the challenges of large-scale environments, traditional ORB-SLAM and other systems may face problems such as high computing resource consumption and slow processing speed when processing large amounts of point cloud data. This makes it difficult for the system to achieve fast and accurate positioning and map construction in complex, large-scale environments. In addition, in a dynamic environment, real-time updating and management of point cloud data is also a challenge, because changes in the environment may cause inaccuracy or outdated map data, affecting the positioning accuracy and stability of the system.

[0006] Another challenge is to improve positioning accuracy. Existing visual SLAM systems may be affected by factors such as lighting changes, occlusions, or sensor errors in some cases, resulting in reduced positioning accuracy. Especially in complex indoor or urban environments, positioning errors may accumulate and affect the stability and accuracy of the system.

[0007] In addition, effectively managing point cloud map data is also an important issue. Large-scale point cloud data requires effective storage, indexing, and retrieval mechanisms so that the system can quickly access and process this data. However, traditional data structures and algorithms may not be sufficient to handle data of this scale, which may lead to problems such as storage waste and inefficient query. Summary of the invention

[0008] The technical problem to be solved by the present invention is to provide a three-dimensional mapping method and system for the above-mentioned deficiencies in the prior art, which solves at least one of the problems of low accuracy of map data and inability to efficiently manage map data.

[0009] The objective of the present invention is achieved by the following technical solutions:

[0010] A three-dimensional mapping method, comprising:

[0011] The ORB-SLAM system is used to extract the feature points of the acquired target map image, and three-dimensional visual reconstruction is performed based on the feature points to generate an initial sparse point cloud map;

[0012] Fusing sparse point cloud data of a target map image acquired by a sensor combination device to obtain dense point cloud data, wherein the sparse point cloud data is obtained based on a sparse point cloud map;

[0013] The dense point cloud data is processed using the point cloud segmentation algorithm and the target detection algorithm to obtain the target information in the dense point cloud data;

[0014] By using a tree data structure algorithm and according to target information in the dense point cloud data, the dense point cloud data is spatially segmented and organized to obtain a point cloud map with a hierarchical structure.

[0015] As a further improvement of the present invention, the method of extracting feature points of the acquired target map image using the ORB-SLAM system and performing three-dimensional visual reconstruction based on the feature points to generate an initial sparse point cloud map specifically includes:

[0016] Use image pyramids to align and match target map images of different scales and perspectives;

[0017] Split each layer of image into grids of different sizes, extract the FAST corner points in each grid, and iterate and filter the corner points continuously;

[0018] The grayscale centroid method is used to calculate the rotation angle of the corner point, and the BRIEF descriptor is calculated for each filtered FAST corner point based on the rotation angle to complete the extraction of ORB features and obtain feature points.

[0019] Visual reconstruction is performed based on feature points to generate a sparse point cloud map.

[0020] As a further improvement of the present invention, the visual reconstruction based on feature points to generate a sparse point cloud map specifically includes:

[0021] Initialize the position of the sensor assembly device using the matching information of the feature points, and estimate the motion trajectory of the sensor assembly device;

[0022] By matching feature points and estimating the motion trajectory of the sensor combination device, the ORB-SLAM system begins to perform three-dimensional reconstruction. According to the motion of the sensor combination device and the geometric information of the scene, the three-dimensional position of the point cloud in the scene is inferred. The collection of these point clouds constitutes a sparse point cloud map.

[0023] As a further improvement of the present invention, the sparse point cloud data of the target map image acquired by multiple sensors are fused to obtain dense point cloud data, which specifically includes:

[0024] Based on the set distance constraint, the Hamming distance between feature points is used to construct the accuracy constraint matching quality score, and based on the quality score, a large number of feature point pairs that are mismatched between point cloud map frames are eliminated;

[0025] Use the sensor combination device to obtain the RGB image and depth map of the sparse point cloud map, match the RGB image and the depth map, estimate the pose of the sensor combination device according to the front end, pass the pose information and depth information to the dense point cloud construction thread when adding a key frame, calculate the spatial coordinates of all pixels on the target map image, and generate a dense point cloud map;

[0026] The point cloud information is colored using RGB image information. As key frames are continuously added, point cloud splicing and global optimization are continuously performed with the generated point cloud map. Finally, a colored point cloud map composed of discrete points is obtained, realizing the transition from sparse to dense.

[0027] As a further improvement of the present invention, the dense point cloud data is processed by using a point cloud segmentation algorithm and a target detection algorithm to obtain target information in the dense point cloud data, specifically including: the point cloud segmentation algorithm and the target detection algorithm adopt a DeeplabV3+ neural network algorithm; the DeeplabV3+ neural network algorithm includes a downsampling module and an upsampling module; the specific steps are:

[0028] Use the DeeplabV3+ neural network algorithm to segment the indoor scene images in the dense point cloud map image and obtain the scene segmentation model;

[0029] A downsampling module is used to extract features from the dense point cloud map to obtain a feature map; and an upsampling module is used to restore the size of the feature map;

[0030] The scene segmentation model is used to segment the scene image into image frames to obtain a semantic segmentation result image with the same size as the original scene image;

[0031] After filtering, segmenting and clustering the dense point cloud map, the dense point cloud map is decomposed into independent point cloud clusters, and then the point cloud clusters are back-projected onto the RGB image plane. The 2D rectangular box of the point cloud cluster corresponding to the projection point is calculated, and then the similarity is calculated with the 2D rectangular box of the target detection. Each detection box matches a point cloud cluster and then the 3D information of the point cloud cluster is calculated. The 3D target database is constructed by combining the results of 2D target detection and the 3D information of the point cloud cluster.

[0032] As a further improvement of the present invention, the downsampling module is used to extract features from the dense point cloud map to obtain a feature map; and the upsampling module is used to restore the size of the feature map, specifically including:

[0033] The downsampling module downsamples the dense point cloud map image four times, and the resulting initial effective feature layer is input into the parallel dilated convolutional layer.

[0034] In each branch of ASPP, dilated convolution with different sampling rates is used to further extract features;

[0035] The feature extraction results of each branch of ASPP are combined to obtain a combined feature layer;

[0036] The merged feature layer is convolved point by point with 1x1 to obtain the green feature map, completing the preliminary extraction of high-resolution features of the scene graph;

[0037] The upsampling module uses 1x1 point-by-point convolution to adjust the number of channels of the initial effective feature layer that has been compressed twice, and stacks the adjusted effective feature layer with the green feature map in the downsampling module.

[0038] Performing two depth-wise separable convolutions on the stacked feature layer to obtain a final effective feature layer, and using point-by-point convolution of a set size to adjust the channels of the final effective feature layer to the total number of categories;

[0039] After the predicted feature map is output according to the final effective feature layer, the output predicted feature map is resized so that the size of the output predicted feature map is aligned with the size of the acquired target map image.

[0040] As a further improvement of the present invention, the tree data structure algorithm includes an octree data structure and an R-tree data structure.

[0041] As a further improvement of the present invention, the tree data structure algorithm is used to spatially segment and organize the dense point cloud data to obtain a point cloud map with a hierarchical structure, specifically including:

[0042] After obtaining the 3D target database and dense point cloud map, the Octo-Map 3D occupancy grid probability map construction task is added to the local map thread, and the octree data structure algorithm is used to spatially segment and organize the point cloud data;

[0043] The three-dimensional space is divided into eight cube nodes of equal size, and the point cloud data is allocated to the corresponding cube nodes according to its distribution, forming an octree map with a hierarchical structure, wherein the leaf nodes in the octree map are determined by the resolution parameter.

[0044] As a further improvement of the present invention, after forming the hierarchical octree map, the depth and precision of the nodes in the octree map are adjusted, and the adjustment method includes static adjustment and dynamic adjustment, specifically including:

[0045] The static adjustment method is as follows: for dense point cloud areas, the current node is further subdivided into smaller subnodes to increase the depth of the node; for sparse point cloud areas, the current node is merged with its subnodes to reduce the depth of the node;

[0046] The dynamic adjustment method is: determine the dynamic adjustment strategy based on the statistical characteristics of the point cloud, the number or density of points in the node, and the adjacent node information. The adjustment strategy includes setting threshold adjustment, adaptive strategy based on map distribution, or machine learning-based method to feedback and adjust node depth.

[0047] In a second aspect, the present invention further provides a three-dimensional mapping system for implementing the three-dimensional mapping method, comprising:

[0048] The sparse point cloud map generation module is used to extract the feature points of the acquired target map image using the ORB-SLAM system, and perform three-dimensional visual reconstruction based on the feature points to generate an initial sparse point cloud map;

[0049] A dense point cloud generation module, used for fusing sparse point cloud data of a target map image acquired by multiple sensors to obtain dense point cloud data, wherein the sparse point cloud data is obtained based on a sparse point cloud map;

[0050] A point cloud data processing module is used to process the dense point cloud data using a point cloud segmentation algorithm and a target detection algorithm to obtain target information in the dense point cloud data;

[0051] The three-dimensional map generation module is used to utilize a tree data structure algorithm to spatially segment and organize the dense point cloud data according to target information in the dense point cloud data, thereby obtaining a point cloud map with a three-dimensional hierarchical structure.

[0052] The beneficial effects of the present invention are as follows: the three-dimensional mapping method of the present invention can generate a more accurate and complete point cloud map by fusing multi-sensor data and optimization algorithms. The ORB-SLAM system can estimate the initial pose of the camera and initialize the position of the map points, thereby constructing a sparse point cloud map. The ORB-SLAM system has real-time performance and can generate a dense point cloud map in a short time to meet the needs of real-time applications. In-depth analysis, target extraction and map construction of point cloud data are achieved, so that the robot can achieve efficient navigation and positioning in a complex environment, providing a reliable foundation for its autonomous movement. Using the ORB-SLAM system to perform three-dimensional visual reconstruction and generate point cloud maps is a complex but efficient process, combining the advantages of multiple technologies and algorithms, and can generate accurate, complete and easy-to-analyze three-dimensional maps.

[0053] Furthermore, the present invention sets the accuracy constraint of feature point matching, and filters out unstable matching feature point pairs between different frames when the camera angle changes greatly. This can better adapt to feature point matching at different angles, reduce the risk of mismatching, and improve the robustness of the algorithm.

[0054] Furthermore, the newly added dense point cloud construction thread proposed in the present invention uses an RGB-D camera for dense reconstruction, which enhances the system's environmental perception capability, improves positioning and mapping accuracy, enriches visual information, enhances real-time performance and efficiency, and better integrates multi-sensor information, comprehensively optimizing the performance of the SLAM system.

[0055] Furthermore, the point cloud segmentation and 3D target information acquisition method proposed in the present invention uses a point cloud segmentation algorithm and target detection technology to accurately identify and extract information about different targets in a dense point cloud. The extracted 3D target information helps the system understand the three-dimensional spatial position and characteristics of each object in the scene, and helps the system understand and interpret the scene more accurately, so that the position and characteristics of different targets in the environment can be understood more accurately, and decisions can be made and paths planned more effectively. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0057] Figure 1 It is a flowchart for realizing three-dimensional mapping in the present invention;

[0058] Figure 2 is a flow chart of dense point cloud map construction in the present invention;

[0059] FIG3( a ) is an original image in the sparse point cloud map constructed in the present invention;

[0060] FIG3( b ) is a point cloud image in the sparse point cloud map constructed in the present invention;

[0061] Figure 4 It is the matching accuracy model diagram in the present invention;

[0062] FIG5( a ) is an example diagram of a dense point cloud map constructed in the present invention;

[0063] FIG5( b ) is a dense point cloud image of an example image of a dense point cloud map constructed in the present invention;

[0064] Figure 6 It is the Mask RCNN target detection network diagram in the present invention;

[0065] Figure 7 is the point cloud segmentation map in the present invention;

[0066] Figure 8 It is the point cloud 3D target information acquisition diagram in the present invention;

[0067] Fig. 9 is the octree map constructed in the present invention;

[0068] FIG10( a) is an example diagram of the DeeplabV3+ neural network model input in the present invention;

[0069] FIG10( b ) is a semantic segmentation result of an example image output by the DeeplabV3+ neural network model of the present invention;

[0070] Fig.11 It is a schematic diagram of the structure of the DeeplabV3+ network in the present invention. DETAILED DESCRIPTION

[0071] In order to make the purpose and technical solution of the present invention clearer and easier to understand, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. The specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0072] Explanation of relevant terms:

[0073] SLAM (Simultaneous Localization and Mapping): simultaneous positioning and map building algorithm;

[0074] ORB-SLAM: A 3D positioning and mapping algorithm based on ORB features;

[0075] ORB (Oriented FAST and Rotated BRIEF): An algorithm for fast feature point extraction and description, combining the FAST (corner detection) feature point detection algorithm and the BRIEF (feature description operator) feature description algorithm;

[0076] ASPP: spatial pyramid pooling structure;

[0077] Atrous Convolution: Atrous convolution;

[0078] The concept of the present invention is to provide a three-dimensional mapping method and system, the mapping method mainly includes:

[0079] The ORB-SLAM system is used to extract the feature points of the acquired target map image, and three-dimensional visual reconstruction is performed based on the feature points to generate an initial sparse point cloud map;

[0080] Fusing sparse point cloud data of a target map image acquired by multiple sensors to obtain dense point cloud data, wherein the sparse point cloud data is obtained based on a sparse point cloud map;

[0081] The dense point cloud data is processed using the point cloud segmentation algorithm and the target detection algorithm to obtain the target information in the dense point cloud data;

[0082] By using a tree data structure algorithm and according to target information in the dense point cloud data, the dense point cloud data is spatially segmented and organized to obtain a point cloud map with a three-dimensional hierarchical structure.

[0083] Based on the above mapping method, the accuracy of map data is improved, and point cloud map data can be effectively managed.

[0084] The technical solution of the present invention will be clearly and completely described below in conjunction with the accompanying drawings and specific embodiments, wherein the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments.

[0085] Example 1

[0086] like Figures 1 to 11 As shown, this embodiment provides a 3D mapping method, the main principles of which are:

[0087] The ORB-SLAM system is used to extract feature points and perform visual reconstruction to generate an initial sparse point cloud map. Then, based on the generation of the sparse point cloud map, a multi-sensor fusion method is used. Combining the depth information from sensors such as lidar and RGB-D cameras, the data from these different sensors are fused to obtain more accurate and rich point cloud data. Subsequently, the feature point matching and 3D reconstruction algorithms in the ORB-SLAM system are improved to expand the original sparse feature points and optimize the 3D reconstruction process, thereby generating richer and denser point cloud data, making the final point cloud map more dense and accurate.

[0088] After obtaining the dense point cloud map, the first step is to segment the point cloud and obtain 3D target information. This step involves processing the dense point cloud data, using point cloud segmentation algorithms and target detection techniques to identify and extract different target information in the point cloud, such as objects, dynamic elements in the scene, etc. Through these algorithms and techniques, the point cloud data can be segmented to obtain target information in each area, such as movable objects such as chairs, cups, and people. Subsequently, after obtaining the target information, the point cloud data is spatially segmented and organized using an octree data structure. This process divides the three-dimensional space into eight equal-sized cube nodes, and assigns them to corresponding cube nodes according to the distribution of the point cloud data, forming an octree map with a hierarchical structure. The octree map has the ability to dynamically adjust the depth and precision of the nodes, and can adaptively adjust the hierarchical structure and fineness of the nodes according to the changes in the density and spatial distribution of the point cloud to meet the needs of different resolutions and spatial scales. This ensures the real-time and accuracy of the map, and provides efficient environmental perception and positioning capabilities for the robot's autonomous navigation.

[0089] The specific implementation of the three-dimensional mapping method in this embodiment is as follows:

[0090] S1: Use the ORB-SLAM system to extract the feature points of the acquired target map image, and perform 3D visual reconstruction based on the feature points to generate an initial sparse point cloud map. In step S1, the ORB-SLAM system is used to process the input image data. First, the FAST corner points affected by each layer of the image are extracted and continuously filtered. Then, the BRIEF descriptors corresponding to the corner points are calculated, and the ORB feature descriptors are used to extract the key points in the image. Map construction and positioning are performed based on these feature points. The specific implementation is as follows:

[0091] Use image pyramids to align and match target map images of different scales and perspectives;

[0092] Split each layer of image into grids of different sizes, extract the FAST corner points in each grid, and iterate and filter the corner points continuously;

[0093] The grayscale centroid method is used to calculate the rotation angle of the corner point, and the BRIEF descriptor is calculated for each filtered FAST corner point based on the rotation angle to complete the extraction of ORB features and obtain feature points.

[0094] Based on the feature points, visual reconstruction is performed to generate a sparse point cloud map. The matching information of the feature points is used to initialize the camera position and estimate the camera's motion trajectory. Through the matching of feature points and the estimation of the camera's motion trajectory, the ORB-SLAM system begins to perform three-dimensional reconstruction. According to the camera's motion and the scene's geometric information, the three-dimensional position of the point cloud in the scene is inferred. The collection of these point clouds constitutes a sparse point cloud map.

[0095] Specifically, the image pyramid is first used to align and match images of different scales and perspectives. Then each layer of the image is split into grids of different sizes, and the FAST corner points are extracted in each grid. Finally, the non-maximum suppression method is used to iteratively filter the FAST corner points.

[0096] The non-maximum suppression method first calculates the sum of the absolute values ​​of the grayscale differences between the central pixel and the surrounding pixels, and then uses a self-set threshold to limit the absolute value and achieve the purpose of screening corner points.

[0097] Calculate the sum c of the absolute value of the grayscale difference between a central pixel p and the surrounding 16 pixels:

[0098]

[0099] Among them, I p is the grayscale of the center pixel p, I i is the grayscale of the pixels around p.

[0100] Secondly, the grayscale centroid method is used to calculate the rotation angle of the FAST corner point, and the angle and the filtered corner point are used to obtain the BRIEF descriptor:

[0101] First, select the grayscale centroid of the pixels in a circular area, connect the centroid and the center of the circle to form a vector, and the angle of this vector is the angle of the corner point. The radius of the circle is 15, because the entire patch is generally 31×31. Then the grayscale centroid (cx, cy) is:

[0102]

[0103] Where I(x, y) is the grayscale value of the pixel with coordinates (x, y).

[0104] Then, the angle of the corner point is:

[0105] θ=arctan2(c y ,c x )

[0106] At this point, not only the FAST corner points are extracted, but also the angle of the corner points is found. This angle can be used to guide the extraction of descriptors, ensuring that the descriptors are calculated in the same direction each time, achieving angle invariance, and thus completing the extraction of ORB features.

[0107] Based on the feature points and camera viewpoints extracted by the ORB-SLAM system, the spatial position information of the key points of the environment is used to perform 3D visual reconstruction, thereby generating an initial sparse point cloud map. The specific implementation is as follows:

[0108] Initialize the camera position and scene geometry: Using the matching information of feature points, the ORB-SLAM system attempts to initialize the camera position and estimate the camera pose.

[0109] The camera's pose change during motion is solved through the relationship between the two frames. The ORB feature is used to match the corresponding points between frames. The matching points obtained have depth information, and the pose change during motion is calculated through the depth information of the matching points. Assume there is a set of matched 3D points:

[0110] P=p1,p2,...,p n

[0111] P'=p1',p'2,...,p' n

[0112] For any matching point, there is the same Euclidean transformation:

[0113]

[0114] Among them, R is the rotation transformation matrix between frames, and t is the translation transformation matrix between frames. The above formula is solved using the ICP (Iterative Clotest Point) algorithm to obtain the motion transformation posture of the camera.

[0115] 3D reconstruction and point cloud generation: Through the matching of feature points and the estimation of camera motion, the ORB-SLAM system begins to build maps and infers the 3D position of points in the scene based on the camera motion and the geometric information of the scene. The collection of these points constitutes the initial sparse point cloud map, as shown in Figure 3(a) and Figure 3(b).

[0116] SLAM mapping is divided into keyframe insertion, map point cloud screening, new map point cloud creation, local constraint adjustment and local keyframe screening. Specifically:

[0117] Keyframe insertion: Detect whether there is a keyframe in the sequence. If so, calculate the bag-of-words descriptor of the new keyframe and insert it into the map.

[0118] Map point cloud screening: When a map point appears in enough key frames, it is considered to be a stable point, otherwise it is removed.

[0119] Creation of new map point cloud: Based on the triangulated ORB feature vector, calculate the position of the new point cloud in the map and update the map.

[0120] Bundle Adjustment: Use bundle adjustment to optimize the pose of the current keyframe.

[0121] Local keyframe filtering: To reduce the number of keyframes, if the current keyframe can be seen by the other three keyframes and 90% of the point clouds are the same, delete the keyframe.

[0122] After generating the initial sparse point cloud map, map optimization is usually required. This embodiment uses the incremental update strategy of SLAM to continuously process new sensor data and gradually update the map and the robot's position based on this data. This strategy allows the SLAM system to continuously improve map accuracy and positioning accuracy during operation without reprocessing all previous data. Incremental updates improve the consistency and accuracy of the map.

[0123] S2: Fuse the sparse point cloud data of the target map image acquired by the sensor combination device to obtain dense point cloud data. For sparse point cloud maps, the feature point matching and 3D reconstruction algorithms in the ORB-SLAM system are improved and optimized, aiming to expand the original sparse feature points and optimize the reconstruction process of point cloud data, thereby generating richer and denser point cloud data. It mainly includes: based on the set distance constraint, the Hamming distance between feature points is used to construct the accuracy constraint matching quality score, and based on the quality score, a large number of feature point pairs that are mismatched between point cloud map frames are eliminated;

[0124] Use the sensor combination device to obtain the RGB image and depth map of the sparse point cloud map, match the RGB image and the depth map, estimate the camera pose based on the front end, pass the pose information and depth information to the dense point cloud construction thread when adding a key frame, calculate the spatial coordinates of all pixels on the target map image, and generate a dense point cloud map;

[0125] The point cloud information is colored using RGB image information. As key frames are continuously added, point cloud splicing and global optimization are continuously performed with the generated point cloud map. Finally, a colored point cloud map composed of discrete points is obtained, realizing the transition from sparse to dense.

[0126] Sensor combination devices include RGB-D cameras, monocular cameras + IMUs, and lidar + cameras. Among them, a monocular camera and an inertial measurement unit (IMU) are combined to generate dense point clouds and perform positioning through the visual inertial odometer (VIO) method. Lidar + camera combines the advantages of lidar and camera, using lidar to provide high-precision distance information and camera to provide rich texture information to generate more accurate three-dimensional point cloud data.

[0127] This embodiment uses an RGB-D camera. The specific implementation of this step is as follows:

[0128] S2.1, using matching accuracy constraints (such as Figure 4 As shown in the figure, the problem of insignificant corresponding features in two adjacent frames caused by large changes in camera angles is solved. Based on the distance constraint, the Hamming distance between feature points is first used to construct the accuracy constraint matching quality score, and then based on the quality score, a large number of feature point pairs that are mismatched between frames are eliminated. Specifically, it includes:

[0129] S2.1.1: In the image feature point matching stage, this paper sets the matching point pairs whose maximum Hamming distance is much larger than the next closest Hamming distance as reliable matches. This embodiment calculates the closest Hamming distance d1 and the next closest Hamming distance d2 between each pair of feature points and other feature points, and the ratio between them is λ, that is:

[0130] λ=d1 / d2

[0131] The smaller λ is, the closer the Hamming distance of the pair of matching points is to the nearest Hamming distance, and the more reliable the matching is.

[0132] S2.1.2: The reliability of feature point matching is determined by setting the precision constraint factor α. If the ratio of the Hamming distance satisfies: λ<α, the feature point matching pair currently determined is considered to be a reliable match. The concept of matching quality is introduced. The higher the matching quality of a pair of feature points, the greater the probability of correct matching. Therefore, a precision constraint score q can be defined. a To quantify the quality of the match:

[0133] q a =λd1

[0134] Among them, q a It represents the accuracy constraint matching quality score. The smaller the accuracy constraint score, the higher the matching quality. The concept diagram of feature matching based on distance constraint is shown in Figure 3. Compared with point pairs with low matching quality, high-quality matching point pairs can provide a more robust inter-frame relationship for the SLAM system and are more conducive to the camera pose estimation.

[0135] S2.2, ORB-SLAM can only generate sparse point cloud maps, so this embodiment adds a dense point cloud construction thread to the original algorithm framework and uses the RGB-D camera for dense reconstruction. That is, add one dense point cloud construction thread, first match the RGB image and the depth map, then estimate the camera pose based on the front end, pass the pose information and depth information to the dense point cloud construction thread when adding a key frame, calculate the spatial coordinates of all pixels on the image, and generate a point cloud map.

[0136] S2.3, use RGB image information to color the point cloud. As key frames are continuously added, the point cloud is continuously spliced ​​and globally optimized with the generated map. Finally, a colored point cloud map composed of discrete points is obtained (as shown in Figure 5(a) and Figure 5(b)), realizing the transition from sparse to dense.

[0137] S3: After obtaining the dense point cloud map, first perform point cloud segmentation and obtain 3D target information. That is, use the point cloud segmentation algorithm and the target detection algorithm to process the dense point cloud data to obtain the target information in the dense point cloud data. The point cloud segmentation algorithm and the target detection algorithm use the DeeplabV3+ neural network algorithm; the DeeplabV3+ neural network algorithm includes a downsampling module and an upsampling module; the DeeplabV3+ neural network algorithm is used to perform regional segmentation on the indoor scene image in the dense point cloud map image to obtain a scene segmentation model; the downsampling module is used to extract features from the dense point cloud map to obtain a feature map; the upsampling module is used to restore the size of the feature map; the scene segmentation model is used to segment the scene image frame to obtain a semantic segmentation result map with the same size as the original scene map. The goal of this step is to identify and extract different target information in the point cloud, such as objects, dynamic elements in the scene, etc. These algorithms and techniques can segment the point cloud data to obtain target information in each area, such as movable objects such as chairs, cups, and people.

[0138] Among them, the downsampling module downsamples the dense point cloud map image four times, and the initial effective feature layer obtained is input into the parallel hole convolution layer;

[0139] In each branch of ASPP, dilated convolution with different sampling rates is used to further extract features;

[0140] The feature extraction results of each branch of ASPP are combined to obtain a combined feature layer;

[0141] The merged feature layer is convolved point by point with 1x1 to obtain the green feature map, completing the preliminary extraction of high-resolution features of the scene graph;

[0142] The upsampling module uses 1x1 point-by-point convolution to adjust the number of channels of the initial effective feature layer that has been compressed twice, and stacks the adjusted effective feature layer with the green feature map in the downsampling module.

[0143] Performing two depth-wise separable convolutions on the stacked feature layer to obtain a final effective feature layer, and using point-by-point convolution of a set size to adjust the channels of the final effective feature layer to the total number of categories;

[0144] After the predicted feature map is output according to the final effective feature layer, the output predicted feature map is resized so that the size of the output predicted feature map is aligned with the size of the acquired target map image.

[0145] After filtering, segmenting and clustering the dense point cloud map, the dense point cloud map is decomposed into independent point cloud clusters, and then the point cloud clusters are back-projected onto the RGB image plane, and the 2D rectangular box of the point cloud cluster corresponding to the projection point is calculated, and then the similarity is calculated with the 2D rectangular box of the target detection. Each detection box matches a point cloud cluster and then calculates the 3D information of the point cloud cluster. Combined with the results of 2D target detection and the 3D information of the point cloud cluster, a 3D target database is constructed. The specific steps are as follows:

[0146] First, use the DeeplabV3+ neural network to segment the indoor scene image and obtain the scene segmentation model. The specific implementation is as follows:

[0147] like Fig.11 As shown in the figure, the DeeplabV3+ neural network includes a downsampling module Encoder and an upsampling module Decoder, where the Encoder is responsible for feature extraction and the Decoder is responsible for restoring the size of the feature map by interpolation and transposed convolution. The Encoder and Decoder modules are symmetrical in structure, and high-resolution features are obtained through the Encoder-Decoder structure. The specific implementation is as follows:

[0148] S3.1, downsampling module Encoder performs feature extraction;

[0149] Specifically, the downsampling module Encoder first uses parallel atrous convolution on the initial effective feature layer compressed four times to expand the receptive field and capture the environmental information of multi-scale images:

[0150] The Atrous spatial pyramid pooling structure ASPP with different sampling rates is used to extract features respectively; the feature extraction results are then concat-merged to obtain a merged feature layer;

[0151] The merged feature layer is convolved point by point with 1x1 to obtain the green feature map, completing the preliminary extraction of high-resolution features of the scene graph.

[0152] S3.2, the size of the upsampling module Decoder restores the feature map:

[0153] The upsampling module Decoder uses 1x1 point-by-point convolution to adjust the number of channels of the initial effective feature layer compressed twice, and then stacks the adjusted effective feature layer with the effective feature layer processed by ASPP in the Encoder;

[0154] The stacked feature layer is subjected to two depth-wise separable convolutions to obtain a final effective feature layer, and a 1x1 point-by-point convolution is used to adjust the channel of the final effective feature layer and adjust it to the total number of categories.

[0155] Among them, the depthwise separable convolution is mainly divided into two processes: channel-by-channel convolution Depthwise Convolution and pointwise convolution Pointwise Convolution. Ordinary neural networks use channel-by-channel convolution, but the number of feature maps after channel-by-channel convolution is the same as the number of channels in the input layer, and the feature map cannot be expanded subsequently. In addition, this operation performs convolution operations on each channel of the input layer independently, and does not effectively utilize the feature information of different channels at the same spatial position. Therefore, pointwise convolution is needed to recombine these feature maps to obtain more feature information. Figure 6 Three forms of depth-wise separable convolution are given, among which Figure 6 (a) is the existing channel-by-channel convolution, (b) is the existing point-by-point convolution, and (c) is the channel-by-channel convolution of the present invention fused with the dilated convolution.

[0156] After the adjusted effective feature layer outputs the predicted feature map, the image is resized to restore the output prediction map to the original size.

[0157] S3.3, scene segmentation model based on DeeplabV3+ neural network to segment picture frames:

[0158] The network is first used to iteratively train the model of the scene image dataset until a scene segmentation model is obtained. The segmentation model is then used to predict the segmentation of the scene image and output a semantic segmentation result map that is consistent with the size of the original scene map, as shown in Figure 10(a) and Figure 10(b).

[0159] S3.4, first filter, segment and cluster the point cloud, then decompose the point cloud into independent point cloud clusters, then back-project the point cloud clusters onto the RGB image plane, calculate the 2D rectangular box of the point cloud cluster corresponding to the projection point, and then calculate the similarity (IOU intersection-over-union ratio) with the 2D rectangular box of the target detection. Each detection box matches a point cloud cluster and then calculates the 3D information of the point cloud cluster. The specific method is as follows Figure 7 Finally, by combining the results of 2D target detection and the 3D information of the above point cloud cluster, a 3D target database is constructed, as shown in Figure 8 shown.

[0160] S4: Using a tree data structure algorithm, spatially segment and organize the dense point cloud data according to target information in the dense point cloud data to obtain a point cloud map with a three-dimensional hierarchical structure. The tree data structure algorithm can use any one of an octree data structure, an R-tree data structure, and a KD-tree data structure.

[0161] KD tree (k-dimensional tree) is used to manage and organize 3D point cloud data. KD tree can efficiently perform nearest neighbor search and point cloud data query by recursively dividing k-dimensional space.

[0162] The R-tree can efficiently manage and query 3D point cloud data through hierarchical rectangular region segmentation.

[0163] This embodiment uses an octree data structure to perform spatial segmentation and management on dense point clouds to achieve efficient storage and query.

[0164] After obtaining the 3D target database and dense point cloud map, the Octo-Map 3D occupancy grid probability map construction task is added to the local map thread, and the octree data structure algorithm is used to spatially segment and organize the point cloud data;

[0165] The three-dimensional space is divided into eight cube nodes of equal size, and the point cloud data are allocated to the corresponding cube nodes according to their distribution, forming an octree map with a hierarchical structure, such as Fig. 9 As shown. The leaf nodes in the octree map are determined by the resolution parameter. The specific implementation is as follows:

[0166] Octree map storage: Octree is a hierarchical data structure. Each Node in the Octree represents a cube in space, also known as a voxel. This cube will be recursively divided into eight blocks until each block reaches the minimum size.

[0167] In addition, the update strategy of the octree map is:

[0168] Octo-Map stores the map in an octree structure. The minimum leaf node size is determined by the resolution parameter. Due to the existence of dynamic objects in the environment, the same space is sometimes occupied and becomes idle again. Therefore, the occupancy probability of a leaf should be determined by the joint probability of multiple observations. This embodiment performs the following logit transformation on the joint probability to obtain a simplified method for updating the probability p:

[0169] α=log(p / 1-p)

[0170] The inverse transformation of the above logit transformation is in the form of a sigmoid function, as shown below:

[0171] p = 1 / (1+exp(-α))

[0172] A simplified way of updating the probability is as follows:

[0173] L(n|Z 1:T )=L(n|Z 1:T-1 )+L(n|Z T )

[0174] Where L(n|Z1:T-1) is the α value of the occupied grid at the previous moment. Adding the most recent α value L(n|ZT) can get the current comprehensive α value. Then, through the inverse transformation of the joint probability, the occupation probability value of the occupied grid can be obtained. L(n|ZT) is determined by the following formula:

[0175]

[0176] Wherein, locc and lfree are predefined values. In this embodiment, locc=0.85, lfree=-0.4 correspond to occupancy probability values ​​of 0.7 and 0.4 respectively, and the probability value corresponding to 0 is 0.5. If node n is frequently observed, its occupancy probability will increase, and the occupancy probability of those points that are not observed and not considered (ground and dynamic points obtained by point cloud segmentation) will be relatively reduced.

[0177] After forming an octree map with a hierarchical structure, the depth and precision of the nodes in the octree map are adjusted. The adjustment method includes static adjustment and dynamic adjustment, specifically including:

[0178] The static adjustment method is: for areas with dense point clouds, the current node is further subdivided into smaller subnodes to increase the depth of the node; for areas with sparse point clouds, the current node is merged with its subnodes to reduce the depth of the node.

[0179] Specifically, for dense areas, the system may observe that more point cloud data falls within the same area. In this case, it may be considered to increase the depth of the node. Increasing the depth of a node means further subdividing the current node into smaller subnodes to allow more detail information to be stored in this area. For sparse areas, it may appear that there are only a few point cloud data in some areas. In this case, in order to reduce the complexity of the map, the system may consider reducing the depth of the node. Reducing the depth of a node means merging the current node with its subnodes to reduce the details and complexity of the map.

[0180] The dynamic adjustment method is: determine the dynamic adjustment strategy based on the statistical characteristics of the point cloud, the number or density of points in the node, and the adjacent node information. The adjustment strategy includes setting threshold adjustment, adaptive strategy based on map distribution, or machine learning-based method to feedback and adjust node depth.

[0181] Specifically, the dynamic adjustment strategy is defined based on the statistical characteristics of the point cloud, the number or density of points in the node, the information of adjacent nodes, etc. The dynamic adjustment strategy may include threshold setting, adaptive strategy based on map distribution, or machine learning-based method to automatically adjust the node depth. The following are the specific contents of each dynamic adjustment strategy:

[0182] Threshold setting: The system makes adjustments based on pre-set thresholds. These thresholds are based on indicators such as point cloud density, the number of points in a node, and changes in spatial areas. For example, when the number of points in a node exceeds a certain threshold, you can choose to increase the depth of the node; conversely, if the number of points in a node is below the threshold, you can consider reducing the depth of the node.

[0183] Adaptive strategy based on map distribution: By analyzing and monitoring the map distribution in real time, the system can adaptively adjust the node depth, including statistics and analysis of the point cloud density of the entire map or a specific area, and then dynamically adjust the node depth based on the analysis results. For example, the division and merging of nodes can be determined based on a certain density indicator.

[0184] Feedback control and real-time adjustment: Another method is to implement a feedback control system to adjust the node depth and accuracy in real time according to the changes in the map. By monitoring the performance of the actual map, the information of node distribution and density is collected in real time, and then the node depth and accuracy are adjusted based on this information to keep the map in an optimized state.

[0185] In addition, the update mechanism in this embodiment can also use an incremental update algorithm to gradually update the point cloud data in the map. By detecting environmental changes, only the changed parts are updated, thereby improving the update efficiency.

[0186] Alternatively, the update mechanism in this embodiment may also use a sliding window technique to manage and update only the point cloud data within the current sensing range. The sliding window can be dynamically resized according to environmental changes, ensuring real-time updates while reducing computational complexity.

[0187] The ultimate goal of the entire implementation scheme is to provide efficient environmental perception and positioning capabilities for the robot's autonomous navigation. Through these steps, in-depth analysis of point cloud data, target extraction and map construction are achieved, enabling the robot to achieve efficient navigation and positioning in complex environments, providing a reliable foundation for its autonomous movement.

[0188] Example 2

[0189] This embodiment provides a 3D mapping system for implementing the 3D mapping method described in Embodiment 1. The system includes:

[0190] The sparse point cloud map generation module is used to extract the feature points of the acquired target map image using the ORB-SLAM system, and perform three-dimensional visual reconstruction based on the feature points to generate an initial sparse point cloud map;

[0191] A dense point cloud generation module, used for fusing sparse point cloud data of a target map image acquired by multiple sensors to obtain dense point cloud data, wherein the sparse point cloud data is obtained based on a sparse point cloud map;

[0192] A point cloud data processing module is used to process the dense point cloud data using a point cloud segmentation algorithm and a target detection algorithm to obtain target information in the dense point cloud data;

[0193] The three-dimensional map generation module is used to utilize a tree data structure algorithm to spatially segment and organize the dense point cloud data according to target information in the dense point cloud data, thereby obtaining a point cloud map with a three-dimensional hierarchical structure.

[0194] It will be appreciated by those skilled in the art that various aspects of the present invention may be implemented as a system, method or program product. Therefore, various aspects of the present invention may be specifically implemented in the following forms, namely: a complete hardware implementation, a complete software implementation (including firmware, microcode, etc.), or a combination of hardware and software, which may be collectively referred to herein as a "circuit", "module" or "platform".

[0195] The above contents are only for explaining the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the claims of the present invention.

Claims

1. A three-dimensional mapping method, characterized in that: include: The ORB-SLAM system is used to extract the feature points of the acquired target map image, and three-dimensional visual reconstruction is performed based on the feature points to generate an initial sparse point cloud map; Fusing sparse point cloud data of a target map image acquired by a sensor combination device to obtain dense point cloud data, wherein the sparse point cloud data is obtained based on a sparse point cloud map; The dense point cloud data is processed using the point cloud segmentation algorithm and the target detection algorithm to obtain the target information in the dense point cloud data; By using a tree data structure algorithm and according to target information in the dense point cloud data, the dense point cloud data is spatially segmented and organized to obtain a point cloud map with a three-dimensional hierarchical structure.

2. The three-dimensional mapping method according to claim 1, characterized in that: The method of extracting feature points of the acquired target map image by using the ORB-SLAM system and performing three-dimensional visual reconstruction based on the feature points to generate an initial sparse point cloud map specifically includes: Use image pyramids to align and match target map images of different scales and perspectives; Split each layer of image into grids of different sizes, extract the FAST corner points in each grid, and iterate and filter the corner points continuously; The grayscale centroid method is used to calculate the rotation angle of the corner point, and the BRIEF descriptor is calculated for each filtered FAST corner point based on the rotation angle to complete the extraction of ORB features and obtain feature points. Visual reconstruction is performed based on feature points to generate a sparse point cloud map.

3. The three-dimensional mapping method according to claim 2, characterized in that: The visual reconstruction based on the feature points to generate a sparse point cloud map specifically includes: Initialize the position of the sensor assembly device using the matching information of the feature points, and estimate the motion trajectory of the sensor assembly device; By matching feature points and estimating the motion trajectory of the sensor combination device, the ORB-SLAM system begins to perform three-dimensional reconstruction. According to the motion of the sensor combination device and the geometric information of the scene, the three-dimensional position of the point cloud in the scene is inferred. The collection of these point clouds constitutes a sparse point cloud map.

4. The three-dimensional mapping method according to claim 2, characterized in that: The sparse point cloud data of the target map image acquired by multiple sensors are fused to obtain dense point cloud data, specifically including: Based on the set distance constraint, the Hamming distance between feature points is used to construct the accuracy constraint matching quality score, and based on the quality score, a large number of feature point pairs that are mismatched between point cloud map frames are eliminated; Use the sensor combination device to obtain the RGB image and depth map of the sparse point cloud map, match the RGB image and the depth map, estimate the pose of the sensor combination device according to the front end, pass the pose information and depth information to the dense point cloud construction thread when adding a key frame, calculate the spatial coordinates of all pixels on the target map image, and generate a dense point cloud map; The point cloud information is colored using RGB image information. As key frames are continuously added, point cloud splicing and global optimization are continuously performed with the generated point cloud map. Finally, a colored point cloud map composed of discrete points is obtained, realizing the transition from sparse to dense.

5. The three-dimensional mapping method according to claim 1, characterized in that: The point cloud segmentation algorithm and the target detection algorithm are used to process the dense point cloud data to obtain the target information in the dense point cloud data, which specifically includes: the point cloud segmentation algorithm and the target detection algorithm use the DeeplabV3+ neural network algorithm; the DeeplabV3+ neural network algorithm includes a downsampling module and an upsampling module; the specific steps are: Use the DeeplabV3+ neural network algorithm to segment the indoor scene images in the dense point cloud map image and obtain the scene segmentation model; A downsampling module is used to extract features from the dense point cloud map to obtain a feature map; and an upsampling module is used to restore the size of the feature map; The scene segmentation model is used to segment the scene image into image frames to obtain a semantic segmentation result image with the same size as the original scene image; After filtering, segmenting and clustering the dense point cloud map, the dense point cloud map is decomposed into independent point cloud clusters, and then the point cloud clusters are back-projected onto the RGB image plane. The 2D rectangular box of the point cloud cluster corresponding to the projection point is calculated, and then the similarity is calculated with the 2D rectangular box of the target detection. Each detection box matches a point cloud cluster and then the 3D information of the point cloud cluster is calculated. The 3D target database is constructed by combining the results of 2D target detection and the 3D information of the point cloud cluster.

6. The three-dimensional mapping method according to claim 5, characterized in that: The downsampling module is used to extract features from the dense point cloud map to obtain a feature map; the upsampling module is used to restore the size of the feature map, specifically including: The downsampling module downsamples the dense point cloud map image four times, and the resulting initial effective feature layer is input into the parallel dilated convolutional layer. In each branch of ASPP, dilated convolution with different sampling rates is used to further extract features; The feature extraction results of each branch of ASPP are combined to obtain a combined feature layer; The merged feature layer is convolved point by point with 1x1 to obtain the green feature map, completing the preliminary extraction of high-resolution features of the scene graph; The upsampling module uses 1x1 point-by-point convolution to adjust the number of channels of the initial effective feature layer that has been compressed twice, and stacks the adjusted effective feature layer with the green feature map in the downsampling module. Performing two depth-wise separable convolutions on the stacked feature layer to obtain a final effective feature layer, and using point-by-point convolution of a set size to adjust the channels of the final effective feature layer to the total number of categories; After the predicted feature map is output according to the final effective feature layer, the output predicted feature map is resized so that the size of the output predicted feature map is aligned with the size of the acquired target map image.

7. The three-dimensional mapping method according to claim 1, characterized in that: The tree data structure algorithm includes an octree data structure, an R-tree data structure and a KD-tree data structure.

8. The three-dimensional mapping method according to claim 7, characterized in that: The tree data structure algorithm is used to spatially segment and organize the dense point cloud data to obtain a point cloud map with a hierarchical structure, specifically including: After obtaining the 3D target database and dense point cloud map, the Octo-Map 3D occupancy grid probability map construction task is added to the local map thread, and the octree data structure algorithm is used to spatially segment and organize the point cloud data; The three-dimensional space is divided into eight cube nodes of equal size, and the point cloud data is allocated to the corresponding cube nodes according to its distribution, forming an octree map with a hierarchical structure, wherein the leaf nodes in the octree map are determined by the resolution parameter.

9. The three-dimensional mapping method according to claim 8, characterized in that: After forming the hierarchical octree map, the method further includes adjusting the depth and precision of the nodes in the octree map, wherein the adjustment method includes static adjustment and dynamic adjustment, specifically including: The static adjustment method is as follows: for dense point cloud areas, the current node is further subdivided into smaller subnodes to increase the depth of the node; for sparse point cloud areas, the current node is merged with its subnodes to reduce the depth of the node; The dynamic adjustment method is: determine the dynamic adjustment strategy according to the statistical characteristics of the point cloud, the number or density of points in the node, and the adjacent node information. The adjustment strategy includes setting threshold adjustment, adaptive strategy based on map distribution, or machine learning-based method to feedback and adjust node depth.

10. A three-dimensional mapping system, used to implement the three-dimensional mapping method according to any one of claims 1 to 9, characterized in that: include: The sparse point cloud map generation module is used to extract the feature points of the acquired target map image using the ORB-SLAM system, and perform three-dimensional visual reconstruction based on the feature points to generate an initial sparse point cloud map; A dense point cloud generation module, used for fusing sparse point cloud data of a target map image acquired by multiple sensors to obtain dense point cloud data, wherein the sparse point cloud data is obtained based on a sparse point cloud map; A point cloud data processing module is used to process the dense point cloud data using a point cloud segmentation algorithm and a target detection algorithm to obtain target information in the dense point cloud data; The three-dimensional map generation module is used to utilize a tree data structure algorithm to spatially segment and organize the dense point cloud data according to target information in the dense point cloud data, thereby obtaining a point cloud map with a three-dimensional hierarchical structure.

Images