Three-dimensional point cloud map construction method, system and device and medium

Abstract

The invention discloses a three-dimensional point cloud map construction method, system and device and a medium. The method comprises the following steps: acquiring a color image, a depth image and inertial measurement unit data; constructing an image pyramid according to the color image, and extracting feature points; performing dynamic target detection and feature point screening on the image pyramid through target detection model detection; according to the feature points and inertial measurement unit data, performing preliminary estimation and refined optimization on the pose; performing thread tracking and key frame judgment according to the distribution and poses of the feature points to obtain a key frame, and updating a local map; carrying out loopback detection on the current frame, and optimizing a global map; and performing point cloud generation, point cloud splicing and point cloud optimization according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map. By screening the dynamic feature points and fusing the IMU data, high-precision indoor positioning and map modeling are realized, and the method can be widely applied to the technical field of intelligent positioning and mapping.

Description

Technical Field

[0001] The present invention relates to the field of intelligent positioning and mapping technology, and in particular to a three-dimensional point cloud map construction method, system, device and medium. Background Art

[0002] With the development of modern cities, the underground space of large commercial complexes, office buildings, science and technology parks and other buildings is constantly expanding. Underground parking lots are closed, have similar layouts, duplicate landmarks, and cannot receive GPS signals, making traditional positioning methods difficult to apply. This makes it difficult for users to find their vehicles in a timely and accurate manner in complex underground environments, and there is an urgent need for high-precision indoor navigation and positioning technology.

[0003] In the field of indoor positioning, commonly used technical means include Wi-Fi, geomagnetism, Bluetooth and other methods. However, these methods have problems such as the need to deploy a large number of hardware equipment, high costs, and difficult post-maintenance. For example, Wi-Fi positioning requires the deployment of a sufficient number of Wi-Fi hotspots indoors, and the signal is easily interfered with; geomagnetic positioning requires the collection of geomagnetic features of the environment, and its stability is affected by environmental changes; Bluetooth positioning requires the deployment of a large number of Bluetooth beacons, the signal range is limited, and the energy supply and maintenance costs are high.

[0004] Visual Simultaneous Localization and Mapping (VSLAM) technology uses cameras to obtain environmental information. It has the advantages of low cost, rich information, and flexible deployment, and has become the core technology in the field of mobile robots and unmanned driving. However, traditional VSLAM algorithms are prone to problems such as reduced positioning accuracy, system instability, and even tracking failure in dynamic environments, repeated textures, and lighting changes.

[0005] In the existing technical solutions, feature point extraction algorithms (Oriented FAST and Rotated BRIEF, ORB) ORB-SLAM, ORB-SLAM2, and ORB-SLAM3 are classic feature point-based SLAM algorithms with good real-time performance and accuracy, and are widely used. Among them, ORB-SLAM3 introduces inertial measurement unit (IMU) data fusion, supports multiple sensor modes such as monocular, binocular, and RGB-D cameras, and improves the robustness of the system. In static environments, these algorithms can achieve high-precision positioning and map construction, but cannot handle dynamic environments.

[0006] There are two main improvement schemes for the field of SLAM in dynamic environments. The first is dynamic object detection based on geometric methods. Frame difference method and background subtraction method identify dynamic objects in the scene by detecting pixel differences between consecutive frame images. However, these methods are very sensitive to lighting changes, prone to false detection or missed detection, and lack of robustness. Another method is the optical flow method, which analyzes the motion vector of each point in the scene by calculating the optical flow between consecutive frames. Dynamic objects can be identified by detecting points that are inconsistent with the overall motion. However, the optical flow method has high requirements for computing resources and is easily affected by factors such as lack of texture and rapid motion.

[0007] The second is dynamic object detection based on deep learning. DS-SLAM is based on the ORB-SLAM2 framework and introduces the SegNet semantic segmentation network to perform pixel-level semantic segmentation on the image, identify dynamic object areas, and remove corresponding feature points. Although this method improves positioning accuracy, it has a large amount of calculation and is difficult to meet real-time requirements. DynaSLAM combines Mask R-CNN for pixel-level segmentation of dynamic areas and uses multi-view geometry and semantic information to remove dynamic objects. The detection of dynamic objects is more accurate, but the computational complexity is higher and it is difficult to meet the needs of real-time applications. Summary of the invention

[0008] The purpose of the present invention is to solve one of the technical problems existing in the prior art to at least a certain extent.

[0009] To this end, an object of an embodiment of the present invention is to provide a three-dimensional point cloud map construction method, which achieves high-precision indoor positioning and map modeling while meeting real-time and lightweight requirements by eliminating dynamic feature points, fusing IMU data for pose estimation and global optimization, and performing three-dimensional point cloud map modeling based on key frames and depth image information.

[0010] Another object of an embodiment of the present invention is to provide a three-dimensional point cloud map construction system.

[0011] In order to achieve the above technical objectives, the technical solutions adopted by the embodiments of the present invention include:

[0012] In a first aspect, an embodiment of the present invention provides a method for constructing a three-dimensional point cloud map, characterized in that it includes the following steps:

[0013] Acquire color images, depth images, and inertial measurement unit data;

[0014] Constructing an image pyramid according to the color image and extracting feature points;

[0015] Performing dynamic target detection and feature point screening on the image pyramid through target detection model detection;

[0016] Preliminarily estimating and fine-tuning the position and posture according to the feature points and the inertial measurement unit data;

[0017] According to the distribution of the feature points and the position and posture, tracking threads and key frame determination are performed to obtain key frames and update the local map;

[0018] Performing loop detection on the current frame to optimize the global map;

[0019] Point cloud generation, point cloud stitching and point cloud optimization are performed according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map.

[0020] Furthermore, the performing dynamic target detection and feature point screening on the image pyramid through target detection model detection includes:

[0021] Performing target detection on each layer of the image pyramid using the target detection model to obtain a detection frame of the dynamic target;

[0022] According to the detection frame, the feature points in the dynamic frame are removed.

[0023] Further, the posture is preliminarily estimated by the following steps:

[0024] Integrate the angular velocity and acceleration of the inertial measurement unit data to obtain a rotation matrix and a translation vector of the camera;

[0025] Constructing an initial pose estimation matrix according to the rotation matrix and the translation vector;

[0026] The rotation matrix and the translation vector are optimized according to the filtered feature points and their corresponding three-dimensional space points and camera projection model.

[0027] Further, the posture is refined and optimized by the following steps:

[0028] Optimizing the rotation matrix and the translation vector according to the selected feature points and their corresponding three-dimensional space points and the camera intrinsic parameter matrix;

[0029] Performing reprojection error minimization optimization on the initial pose estimation matrix by a nonlinear optimization algorithm;

[0030] An error model is constructed, error terms are solved through an iterative optimization algorithm, and the initial pose estimation matrix is ​​optimized according to the error terms.

[0031] Further, performing tracking thread and key frame determination according to the distribution of the feature points and the position and posture to obtain key frames, and updating the local map according to the key frames, includes:

[0032] Matching the feature points of the current frame with the feature points in the local map, calculating the number and distribution of matching points, and obtaining tracking quality according to the number and distribution of matching points;

[0033] When the disparity change between the current frame and the previous frame exceeds a threshold, the current frame is determined as the key frame;

[0034] When the time interval between the current frame and the previous frame exceeds a threshold, the current frame is determined to be the key frame;

[0035] When the quality of the current frame exceeds a threshold, determining the current frame as the key frame;

[0036] When the current frame is the key frame, the current frame is inserted into the local map, new map points are extracted and added, and the local map is updated.

[0037] Further, performing loop detection on the current frame to optimize the global map includes:

[0038] Extracting a feature descriptor of the current frame, calculating the similarity between the current frame and a historical key frame based on a bag-of-words model and the feature descriptor, and determining whether a loop exists according to the similarity;

[0039] When the loop exists, the key frame pose is optimized according to the relative pose between the current frame and the loop frame.

[0040] Further, the step of generating point cloud, stitching point cloud and optimizing point cloud according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map includes:

[0041] Converting the depth image into a local three-dimensional point cloud;

[0042] According to the position information of the key frame, the transformation matrix of the local three-dimensional point cloud is converted into the camera coordinate system, and the three-dimensional point cloud map is obtained by splicing;

[0043] The three-dimensional point cloud map is filtered, denoised and refined.

[0044] In a second aspect, an embodiment of the present invention provides a three-dimensional point cloud map construction system, characterized by comprising:

[0045] Data acquisition module, used to acquire color images, depth images and inertial measurement unit data;

[0046] A feature extraction module, used to construct an image pyramid based on the color image and extract feature points;

[0047] A feature screening module, used for performing dynamic target detection and feature point screening on the image pyramid through target detection model detection;

[0048] A posture estimation module, used for performing preliminary estimation and refined optimization of the posture according to the feature points and the inertial measurement unit data;

[0049] A tracking thread and key frame determination module, used to perform tracking thread and key frame determination according to the distribution of the feature points and the position and posture, obtain key frames, and update the local map;

[0050] A loop detection and global optimization module, used to perform loop detection on the current frame and optimize the global map;

[0051] The point cloud map construction module is used to generate, stitch and optimize point clouds according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map.

[0052] In a third aspect, an embodiment of the present invention provides a device, including:

[0053] at least one processor;

[0054] at least one memory for storing at least one program;

[0055] When the at least one program is executed by the at least one processor, the at least one processor implements the three-dimensional point cloud map construction method as described above.

[0056] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, which stores a program executable by a processor, and the program executable by the processor is used to execute the above-mentioned three-dimensional point cloud map construction method when executed by the processor.

[0057] The advantages and beneficial effects of the present invention will be partly given in the following description, partly become apparent from the following description, or be understood through the practice of the present invention:

[0058] The present invention integrates a lightweight and more accurate YOLOv8 target detection network in the ORB-SLAM3 framework, identifies and removes dynamic object feature points layer by layer in real time in the image pyramid, and improves the positioning accuracy in dynamic environments; by fusing IMU data, the accuracy and robustness of pose estimation are improved; by optimizing key frame determination and global map optimization, the overall accuracy of indoor scene positioning and mapping is improved; by generating, splicing and optimizing three-dimensional point clouds based on key frames and depth image information, a high-precision, low-error three-dimensional point cloud map is constructed, which ensures real-time performance while meeting the requirements of high-precision mapping. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 A step diagram of a method for constructing a three-dimensional point cloud map provided by an embodiment of the present invention;

[0060] Figure 2 A schematic diagram of a technical solution flow of a method for constructing a three-dimensional point cloud map provided by an embodiment of the present invention;

[0061] Figure 3 A schematic diagram of an image pyramid provided by an embodiment of the present invention;

[0062] Figure 4 A dynamic feature point elimination flow chart provided by an embodiment of the present invention;

[0063] Figure 5 A schematic diagram of a dynamic feature point before removal provided by an embodiment of the present invention;

[0064] Figure 6 A schematic diagram of a dynamic feature point after removal provided by an embodiment of the present invention;

[0065] Figure 7 A process for constructing a three-dimensional point cloud map of an indoor scene provided by an embodiment of the present invention;

[0066] Figure 8 A schematic diagram of the structure of a key frame point cloud before splicing provided by an embodiment of the present invention;

[0067] Fig. 9 A schematic diagram of a structure after key frame point cloud splicing provided by an embodiment of the present invention;

[0068] Fig.10 An indoor three-dimensional laser point cloud map provided by an embodiment of the present invention;

[0069] Fig.11 An indoor three-dimensional color point cloud map provided by an embodiment of the present invention;

[0070] Fig.12 A schematic diagram of a three-dimensional point cloud map construction system provided by an embodiment of the present invention;

[0071] Fig.13 A schematic diagram of the structure of a computer device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0072] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and are not to be construed as limitations of the present invention. For the step numbers in the following embodiments, they are only provided for the convenience of explanation, and the order between the steps is not limited in any way. The execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art.

[0073] In the description of the present invention, the meaning of "a plurality" is two or more than two. If there is a description of "a first" or "a second", it is only used for the purpose of distinguishing technical features, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features or implicitly indicating the order of the indicated technical features. In addition, unless otherwise defined, all technical and scientific terms used in this document have the same meaning as those commonly understood by those skilled in the art.

[0074] Before describing the embodiments of the present invention in detail, some nouns and terms involved in the embodiments of the present invention are first described. The nouns and terms involved in the embodiments of the present invention are subject to the following explanations.

[0075] VSLAM (Visual Simultaneous Localization and Mapping): Visual simultaneous positioning and mapping, which obtains environmental information through cameras, locates its own position in real time and builds an environmental map.

[0076] ORB-SLAM3 (Oriented FAST and Rotated BRIEF Simultaneous Localization and Mapping 3): A third-generation SLAM algorithm based on feature points that supports monocular, binocular, and RGB-D cameras, and can integrate inertial measurement unit (IMU) data to achieve high-precision positioning and mapping.

[0077] YOLOv8 (You Only Look Once version 8): A new deep learning model for real-time target detection, with higher accuracy and lighter weight, suitable for fast recognition of dynamic targets.

[0078] IMU (Inertial Measurement Unit): Inertial measurement unit, including accelerometer and gyroscope, used to measure the acceleration and angular velocity information of the device.

[0079] Image pyramid: A multi-scale image representation method that obtains image sequences of different resolutions by downsampling the image multiple times.

[0080] Point cloud map: A spatial geometric representation composed of a large number of three-dimensional points, used to describe the three-dimensional structure of the environment.

[0081] Figure 1 A step diagram of a method for constructing a three-dimensional point cloud map provided by an embodiment of the present invention, referring to Figure 1 The embodiment of the present invention provides a method for constructing a three-dimensional point cloud map, which is characterized by comprising the following steps:

[0082] Acquire color images, depth images, and inertial measurement unit data;

[0083] Construct an image pyramid based on the color image and extract feature points;

[0084] Perform dynamic target detection and feature point screening on the image pyramid through target detection model detection;

[0085] Based on the feature points and inertial measurement unit data, the pose is preliminarily estimated and refined.

[0086] According to the distribution and position of feature points, the tracking thread and key frame determination are performed to obtain the key frame and update the local map;

[0087] Perform loop detection on the current frame and optimize the global map;

[0088] Point cloud generation, point cloud stitching and point cloud optimization are performed according to the color image and depth image corresponding to the key frame to obtain a three-dimensional point cloud map.

[0089] Specifically, Figure 2 A flow chart of the technical solution of a three-dimensional point cloud map construction method provided in an embodiment of the present invention. The implementation process of this embodiment is that in this embodiment, an RGB camera (such as Azure Kinect DK) is used to obtain color images, depth images and IMU data. The image resolution used in this experiment is 1280×720 pixels, the video frame rate is 30FPS, and the IMU acquisition frequency is 1600HZ. In addition, in order to verify the precision and accuracy of this embodiment, a GeoSLAM backpack laser point cloud acquisition device is used to scan the experimental scene to obtain a high-precision laser point cloud map as an evaluation benchmark. After completing the data acquisition, refer to Figure 3, an image pyramid is constructed for the input color image. The image pyramid includes 1-8 layers to obtain images of different scales for feature extraction. The scaling factor of the pyramid is set to 1.2, which is consistent with the default value of ORB-SLAM3. The original image is downsampled in turn. Then ORB visual feature points are extracted for each image, and the number is set to 1000. After the image pyramid is constructed, the target detection model YOLOv8 is used to detect dynamic targets in each layer of the image pyramid, and the feature points in the dynamic target area are removed, and only the feature points in the static area are retained. Based on the filtered static feature points and IMU data, the initial pose of the camera is obtained through preliminary estimation, and the pose is finely adjusted in combination with the nonlinear optimization algorithm. According to the distribution and pose information of the static feature points, the tracking thread is performed, and the local map is updated according to whether the current frame is a key frame. In the tracking thread, whether it is a key frame is determined according to the distribution of feature points, parallax changes, time intervals and other standards, and the key frame is inserted, and the new map points are extracted to update the local map. Perform loop detection on the local map to verify the matching relationship between the current frame and the historical keyframe. When the loop is established, optimize the global map through global posture graph optimization and map point adjustment. Generate local point cloud using keyframe color image and depth image, transform local point cloud to global coordinate system through keyframe pose, and splice to form a global 3D point cloud map.

[0090] The stitched point cloud is filtered and refined to generate a high-precision scene map.

[0091] In this embodiment, the three-dimensional point cloud map generated by this embodiment is compared with the laser point cloud map. The overall point cloud error is within 20 cm. In the actual test scene of the underground garage, the average positioning error is within 1.5 cm, which is 53.4% ​​higher than the positioning accuracy of the monocular inertial navigation ORB-SLAM3 method before the improvement.

[0092] It can be recognized that the embodiments of the present invention improve the positioning accuracy in dynamic environments by integrating a lightweight and more accurate YOLOv8 target detection network in the ORB-SLAM3 framework, identifying and removing dynamic object feature points layer by layer in the image pyramid in real time; improving the accuracy and robustness of pose estimation by fusing IMU data; improving the overall accuracy of indoor scene positioning and mapping by optimizing key frame determination and global map optimization; and constructing a high-precision, low-error three-dimensional point cloud map by generating, stitching and optimizing three-dimensional point clouds based on key frames and depth image information, thereby ensuring real-time performance while meeting the requirements of high-precision mapping.

[0093] In some optional embodiments, dynamic target detection and feature point screening are performed on the image pyramid through target detection model detection, including:

[0094] The target detection model is used to detect the target on each layer of the image pyramid to obtain the detection frame of the dynamic target.

[0095] According to the detection frame, the feature points in the dynamic frame are removed.

[0096] Specifically, in each layer of the image pyramid, the trained YOLOv8 target detection model is used to detect dynamic objects (such as vehicles and pedestrians) in real time. As a latest lightweight target detection model, YOLOv8 has the characteristics of high accuracy, real-time and lightweight, and is suitable for embedded and real-time applications. The feature points in the detected dynamic object area are removed, and only the feature points in the static background are retained. The specific process is as follows: the process of removing dynamic feature points is as follows Figure 4 As shown in the figure, the first step is to use the YOLOv8 model to detect the target in each layer of the image and obtain the detection box (Bounding Box) of the dynamic object. Then, according to the position of the detection box, the feature points in the dynamic object area are removed, and only the feature points in the static area are retained to complete the screening of feature points. The schematic diagram before the removal of dynamic feature points is shown in the figure. Figure 5 As shown, the schematic diagram after the dynamic feature points are removed is as follows Figure 6 shown.

[0097] In some optional embodiments, the pose is initially estimated by the following steps:

[0098] Integrate the angular velocity and acceleration of the inertial measurement unit data to obtain the camera's rotation matrix and translation vector;

[0099] Construct the initial pose estimation matrix based on the rotation matrix and translation vector;

[0100] According to the filtered feature points and their corresponding three-dimensional space points and camera projection model, the rotation matrix and translation vector are optimized.

[0101] Specifically, in this embodiment, the initial pose estimation can be obtained by using IMU data for attitude solution. IMU data is collected at a high frequency (1600Hz), providing high-frequency rotation and translation information. First, the rotation matrix R obtained by IMU integration is IMU and the translation vector t IMU As the parameter of the initial pose estimation, the initial pose estimation matrix T initial It is expressed as formula (1):

[0102] T initial =[R IMU |t IMU ] (1)

[0103] In some optional embodiments, the posture is refined and optimized by the following steps:

[0104] According to the filtered feature points and their corresponding three-dimensional space points and the camera intrinsic parameter matrix, the rotation matrix and translation vector are optimized;

[0105] The reprojection error of the initial pose estimation matrix is ​​minimized through a nonlinear optimization algorithm.

[0106] Construct an error model, solve the error term through an iterative optimization algorithm, and optimize the initial pose estimation matrix based on the error term.

[0107] Specifically, in this embodiment, although the initial estimation of the posture provides high-frequency posture change information, it needs to be further optimized in combination with visual information to eliminate the integral error. i and its corresponding three-dimensional space point P i Perform pose refinement. Through the camera projection model, establish the relationship between feature points and three-dimensional space points:

[0108] p i =π(K[R|t]P i ) (2)

[0109] In formula (2), π is the projection function, K is the camera intrinsic parameter matrix, R and t are the rotation matrix and translation vector to be optimized.

[0110] At the same time, the embodiment of the present invention also uses a nonlinear optimization algorithm (such as the Levenberg-Marquardt algorithm) to refine the posture. The objective function is to minimize the reprojection error of static feature points:

[0111]

[0112] In order to more accurately describe the posture change, the system uses Lie group SE(3) and Lie algebra se(3) to introduce the error model:

[0113] T estimated =T true ·exp(ξ) (4)

[0114] In formula (4), T estimated is the estimated pose transformation matrix, T true is the real pose transformation matrix, exp(ξ) is the exponential mapping from Lie algebra to Lie group, and ξ is the error term represented by Lie algebra. Solved by iterative optimization algorithm:

[0115]

[0116] Equation (5) finally obtains the optimal error term ξ, which is used to correct the initial pose estimation. This method effectively combines the high-frequency data of IMU and the precise positioning capability of visual features to achieve robust and accurate pose recovery in dynamic environments.

[0117] In some optional embodiments, tracking threads and key frame determination are performed according to the distribution and position of feature points to obtain key frames, and the local map is updated according to the key frames, including:

[0118] Match the feature points of the current frame with the feature points in the local map, calculate the number and distribution of matching points, and obtain the tracking quality based on the number and distribution of matching points;

[0119] When the parallax change between the current frame and the previous frame exceeds a threshold, the current frame is determined as a key frame;

[0120] When the time interval between the current frame and the previous frame exceeds the threshold, the current frame is determined as a key frame;

[0121] When the quality of the current frame exceeds the threshold, the current frame is determined as a key frame;

[0122] When the current frame is a key frame, the current frame is inserted into the local map, new map points are extracted and added, and the local map is updated.

[0123] Specifically, in the framework of ORB-SLAM3, the tracking thread is responsible for estimating the camera pose of the current frame and deciding whether to insert a keyframe. The determination of keyframes is crucial to the stability of the system and the integrity of map construction. The tracking thread mainly includes the following steps:

[0124] 1) Feature point matching: Perform feature matching on the static feature points of the current frame and the map points in the local map, calculate the number and distribution of matching points, and use them to evaluate the tracking quality of the current frame.

[0125] 2) Posture optimization: Based on the feature point matching results, the pose of the current frame is optimized through a nonlinear optimization algorithm to minimize the reprojection error of the feature points, thereby updating the camera's pose estimate.

[0126] 3) Tracking status evaluation: The tracking status of the current frame is evaluated based on the number of feature matches, feature point distribution, and reprojection error. When the tracking quality is poor, the system will try to recover the tracking through global relocalization or find matching keyframes in the loop detection module to correct the pose.

[0127] The key frame determination mainly includes the following criteria:

[0128] 1) Parallax change: When the parallax between the current frame and the previous keyframe exceeds a preset threshold, it indicates that the camera position has moved significantly and a keyframe needs to be inserted to expand the map.

[0129] 2) Time interval: The time interval between the current frame and the previous key frame exceeds the set threshold, which is used to prevent key frames from being inserted too frequently and affecting system efficiency.

[0130] 3) Tracking quality: When the tracking quality between the current frame and the local map is good, the number of feature matches is sufficient, the matching distribution is reasonable, and the reprojection error is within an acceptable range, the current frame is determined to be a key frame.

[0131] When the above judgment conditions are met, the current frame is inserted into the local map as a keyframe, and new map points are extracted and added to expand the scope of the local map. After inserting the keyframe, the local map will be optimized accordingly, updating the pose of the keyframe and the position of the map point to improve the accuracy of the map and maintain its consistency.

[0132] In some optional embodiments, loop closure detection is performed on the current frame to optimize the global map, including:

[0133] Extract the feature descriptor of the current frame, calculate the similarity between the current frame and the historical keyframes based on the bag-of-words model and the feature descriptor, and determine whether there is a loop based on the similarity;

[0134] When a loop exists, the key frame pose is optimized based on the relative pose between the current frame and the loop frame.

[0135] Specifically, when loop detection is performed in this embodiment, the ORB feature descriptor of the current frame is first extracted, and then the descriptor is encoded using the Bag-of-Words (BoW) model to convert the feature information of the current frame into a bag-of-words vector form.

[0136] Then, the similarity between the current frame and the historical key frames is calculated based on the bag-of-words model, and the key frames with higher similarity are selected as loop candidate frames. The geometric consistency of the candidate frames is verified, and the ORB feature points between the current frame and the candidate frames are matched to determine whether their spatial distribution meets the loop condition.

[0137] When it is determined that a loop exists, the relative pose transformation matrix (including rotation matrix and translation vector) between the two frames is calculated using the least squares method based on the matching feature points of the current frame and the loop frame. The relative pose transformation between the current frame and the loop frame is used as the loop constraint. The global pose graph optimization method is used to minimize the relative pose error and loop constraint error between key frames, and the poses of all key frames are adjusted at the same time. Based on the optimized key frame poses, the positions of global map points and the poses of all key frames are jointly optimized to minimize the reprojection error, and the optimization of key frame poses and map points is completed. Finally, the optimized key frames and map points are updated to the map to complete the global optimization.

[0138] In some optional embodiments, point cloud generation, point cloud stitching and point cloud optimization are performed according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map, including:

[0139] Convert the depth image into a local 3D point cloud;

[0140] According to the pose information of the key frame, the local 3D point cloud is transformed into the camera coordinate system through transformation matrix, and the 3D point cloud map is obtained by splicing;

[0141] In this embodiment, the construction process of the indoor scene three-dimensional point cloud map is as follows: Figure 7 As shown, firstly, the depth image corresponding to the RGB key frame is extracted, and the image is converted into local three-dimensional point cloud data according to the camera intrinsic parameters and depth information.

[0142] In this embodiment, when the camera is positioned and the trajectory is restored in real time by the SLAM method, a rotation and translation parameter file of each frame relative to the first frame will be obtained. In addition, a rotation and translation parameter file will be output separately for the key frame, tx, ty, tz represents the position of the current frame, qx, qy, qz, qw is the composition of the quaternion Q, and the posture of the current frame is expressed by formula (6). The coordinate system uses the camera coordinate system of the frame in which the ORB-SLAM3 system is successfully initialized, and that frame is the origin of the coordinate system.

[0143] Q=qx*i+qy*j+qz*k+qw (6)

[0144] After obtaining the rotation and translation relationship of each key frame relative to the first frame, the point cloud generated by the RGB-D camera of each frame can be obtained. The structural diagram of some key frame point clouds is shown as follows: Figure 8 As shown, multiply by the transformation matrix to uniformly transform into the camera coordinate system, and splice into a complete scene point cloud map. The spliced ​​scene point cloud map is as follows Fig. 9 shown.

[0145] The stitched point cloud is filtered, noise removed, and refined to construct a complete 3D point cloud map. Finally, compared with the laser point cloud, the overall point cloud error is within 20cm, with high 3D mapping accuracy and successful restoration of the actual scale of the scene. Fig.10 As shown in the figure, the final indoor 3D color point cloud map is as follows Fig.11 shown.

[0146] Reference Fig.12 The embodiment of the present invention provides a three-dimensional point cloud map construction system, characterized in that it includes:

[0147] Data acquisition module, used to acquire color images, depth images and inertial measurement unit data;

[0148] A feature extraction module is used to construct an image pyramid based on a color image and extract feature points;

[0149] Feature screening module, used to perform dynamic target detection and feature point screening on the image pyramid through target detection model detection;

[0150] The pose estimation module is used to make preliminary estimation and fine optimization of the pose based on feature points and inertial measurement unit data;

[0151] Tracking thread and key frame determination module, used to perform tracking thread and key frame determination according to the distribution and position of feature points, obtain key frames, and update local maps;

[0152] The loop detection and global optimization module is used to perform loop detection on the current frame and optimize the global map;

[0153] The point cloud map construction module is used to generate, stitch and optimize point clouds based on the color images and depth images corresponding to the key frames to obtain a three-dimensional point cloud map.

[0154] Reference Fig.13 , an embodiment of the present invention provides a computer device, including:

[0155] at least one processor;

[0156] at least one memory for storing at least one program;

[0157] When the at least one program is executed by the at least one processor, the at least one processor implements the three-dimensional point cloud map construction method.

[0158] The contents of the above method embodiments are all applicable to the present device embodiments. The functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0159] An embodiment of the present invention further provides a computer-readable storage medium, in which a program executable by a processor is stored. When the program executable by the processor is executed by the processor, it is used to execute the above-mentioned three-dimensional point cloud map construction method.

[0160] A computer-readable storage medium according to an embodiment of the present invention can execute a three-dimensional point cloud map construction method provided by an embodiment of the method of the present invention, can execute any combination of implementation steps of the method embodiment, and has the corresponding functions and beneficial effects of the method.

[0161] The embodiment of the present invention also discloses a computer program product or a computer program, wherein the computer program product or the computer program includes computer instructions, and the computer instructions are stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium, and the processor executes the computer instructions, so that the computer device executes Figure 1 The three-dimensional point cloud map construction method shown.

[0162] In some selectable embodiments, the function / operation mentioned in the block diagram may not occur in the order mentioned in the operation diagram. For example, depending on the function / operation involved, the two boxes shown in succession can actually be executed substantially simultaneously or the above-mentioned boxes can sometimes be executed in reverse order. In addition, the embodiment presented and described in the flow chart of the present invention is provided by way of example, for the purpose of providing a more comprehensive understanding of technology. The disclosed method is not limited to the operation and logic flow presented herein. Selectable embodiments are expected, wherein the order of various operations is changed and the sub-operation of a part for which is described as a larger operation is performed independently.

[0163] In addition, although the present invention is described in the context of functional modules, it should be understood that, unless otherwise specified to the contrary, one or more of the above-mentioned functions and / or features can be integrated into a single physical device and / or software module, or one or more functions and / or features can be implemented in a separate physical device or software module. It is also understood that a detailed discussion of the actual implementation of each module is unnecessary for understanding the present invention. More specifically, in view of the properties, functions and internal relationships of the various functional modules in the device disclosed herein, the actual implementation of the module will be understood within the conventional skills of the engineer. Therefore, those skilled in the art can implement the present invention set forth in the claims without excessive experimentation using ordinary techniques. It is also understood that the specific concepts disclosed are merely illustrative and are not intended to limit the scope of the present invention, which is determined by the full scope of the appended claims and their equivalents.

[0164] If the above functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the above methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0165] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in conjunction with such instruction execution systems, devices or apparatuses. For the purposes of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in conjunction with such instruction execution systems, devices or apparatuses.

[0166] More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and editable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be a paper or other suitable medium on which the above-mentioned program is printed, since the above-mentioned program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or processing in other suitable ways as necessary, and then stored in a computer memory.

[0167] It should be understood that the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, a plurality of steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0168] In the above description of this specification, the description with reference to the terms "one embodiment / example", "another embodiment / example" or "certain embodiments / examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0169] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

[0170] The above is a specific description of the preferred implementation of the present invention, but the present invention is not limited to the above embodiments. Those skilled in the art may make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A method for constructing a three-dimensional point cloud map, characterized in that: The following steps are involved: Acquire color images, depth images, and inertial measurement unit data; Constructing an image pyramid according to the color image and extracting feature points; Performing dynamic target detection and feature point screening on the image pyramid through target detection model detection; Preliminarily estimating and refining the position and posture according to the feature points and the inertial measurement unit data; According to the distribution of the feature points and the position and posture, tracking threads and key frame determination are performed to obtain key frames and update the local map; Performing loop detection on the current frame to optimize the global map; Point cloud generation, point cloud stitching and point cloud optimization are performed according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map.

2. A three-dimensional point cloud map construction method according to claim 1, characterized in that: The performing dynamic target detection and feature point screening on the image pyramid through target detection model detection includes: Performing target detection on each layer of the image pyramid using the target detection model to obtain a detection frame of the dynamic target; According to the detection frame, the feature points in the dynamic frame are removed.

3. A three-dimensional point cloud map construction method according to claim 1, characterized in that: The pose is initially estimated by the following steps: Integrate the angular velocity and acceleration of the inertial measurement unit data to obtain a rotation matrix and a translation vector of the camera; Constructing an initial pose estimation matrix according to the rotation matrix and the translation vector; The rotation matrix and the translation vector are optimized according to the filtered feature points and their corresponding three-dimensional space points and camera projection model.

4. A three-dimensional point cloud map construction method according to claim 3, characterized in that: The posture is refined and optimized by the following steps, including: Optimizing the rotation matrix and the translation vector according to the selected feature points and their corresponding three-dimensional space points and the camera intrinsic parameter matrix; Performing reprojection error minimization optimization on the initial pose estimation matrix by a nonlinear optimization algorithm; An error model is constructed, error terms are solved through an iterative optimization algorithm, and the initial pose estimation matrix is ​​optimized according to the error terms.

5. A three-dimensional point cloud map construction method according to claim 1, characterized in that: The tracking thread and key frame determination are performed according to the distribution of the feature points and the position and posture to obtain the key frame, and the local map is updated according to the key frame, including: Matching the feature points of the current frame with the feature points in the local map, calculating the number and distribution of matching points, and obtaining tracking quality according to the number and distribution of matching points; When the disparity change between the current frame and the previous frame exceeds a threshold, the current frame is determined as the key frame; When the time interval between the current frame and the previous frame exceeds a threshold, the current frame is determined to be the key frame; When the quality of the current frame exceeds a threshold, determining the current frame as the key frame; When the current frame is the key frame, the current frame is inserted into the local map, new map points are extracted and added, and the local map is updated.

6. A three-dimensional point cloud map construction method according to claim 1, characterized in that: The performing loop detection on the current frame and optimizing the global map includes: Extracting a feature descriptor of the current frame, calculating the similarity between the current frame and a historical key frame based on a bag-of-words model and the feature descriptor, and determining whether a loop exists according to the similarity; When the loop exists, the key frame pose is optimized according to the relative pose between the current frame and the loop frame.

7. A three-dimensional point cloud map construction method according to claim 1, characterized in that: The step of generating point cloud, stitching point cloud and optimizing point cloud according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map includes: Converting the depth image into a local three-dimensional point cloud; According to the position information of the key frame, the transformation matrix of the local three-dimensional point cloud is converted into the camera coordinate system, and the three-dimensional point cloud map is obtained by splicing; The three-dimensional point cloud map is filtered, denoised and refined.

8. A three-dimensional point cloud map construction system, characterized in that: include: Data acquisition module, used to acquire color images, depth images and inertial measurement unit data; A feature extraction module, used to construct an image pyramid based on the color image and extract feature points; A feature screening module, used for performing dynamic target detection and feature point screening on the image pyramid through target detection model detection; A posture estimation module, used for performing preliminary estimation and refined optimization of the posture according to the feature points and the inertial measurement unit data; A tracking thread and key frame determination module, used to perform tracking thread and key frame determination according to the distribution of the feature points and the position and posture, obtain key frames, and update the local map; A loop detection and global optimization module, used to perform loop detection on the current frame and optimize the global map; The point cloud map construction module is used to generate, stitch and optimize point clouds according to the color image and the depth image corresponding to the key frame to obtain a three-dimensional point cloud map.

9. A device, characterized in that: include: at least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the three-dimensional point cloud map construction method as described in claim 8.

10. A computer-readable storage medium storing a program executable by a processor, characterized in that: The processor-executable program is used to execute a three-dimensional point cloud map construction system as described in claim 8 when executed by the processor.

Images