A 3D grid map construction method based on instant appearance mapping algorithm

Abstract

The present invention discloses a method for constructing a three-dimensional grid map based on an instant appearance mapping algorithm: an RGB-D camera is mounted on an intelligent agent to scan data of a target scene and construct a data set; an RTAB-Map algorithm is run based on the data set to track the movement of the intelligent agent and finally generate point cloud map information; downsampling filtering is used on the point cloud map information to reduce the point cloud data, and then a point cloud filtering algorithm is used to filter out outliers or erroneous points in the point cloud map to obtain a filtered point cloud map; an octree data structure is created, a resolution is set, and then points in the resulting point cloud map are inserted into the octree data structure to convert the point cloud map into a three-dimensional grid map, wherein the three-dimensional grid map is an octree map. The present invention constructs a three-dimensional grid map suitable for three-dimensional navigation by optimizing and supplementing the existing RTAB-Map algorithm.

Description

Technical Field

[0001] The present invention belongs to the field of visual SLAM, and specifically relates to a three-dimensional grid map construction method based on an instant appearance mapping algorithm. Background Art

[0002] The current mainstream VSLAM framework mainly consists of the following modules: sensor data acquisition, visual odometry (VO), backend optimization (Optimization), loop closure detection (Loop Closing) and mapping (Mapping).

[0003] As early as the development of SLAM technology in the last century, people considered using cameras as sensors to achieve simultaneous localization and mapping based on vision. However, computer performance at the time was too weak, the amount of information stored in images was too large, and the amount of information required to be processed was also too large, so little research attention was paid to this approach. It wasn't until 2007 that Professor Davison of Imperial College London proposed the first real-time monocular visual SLAM system, Mono-SLAM. Professor Davison is a pioneer in visual SLAM research, and his many groundbreaking studies have had a profound impact on subsequent visual SLAM. To adapt to the performance of computers at the time, Mono-SLAM achieved simultaneous localization and navigation by tracking very sparse feature points on the front end as state variables and using an EKF as the back end. This was the first time that a mobile robot could achieve real-time and stable operation in small scenes based on visual SLAM technology. In 2007, Klein and Murray of Oxford University proposed PTAM (Parallel Tracking and Mapping), a visual SLAM system based on parallel tracking and mapping. This algorithm incorporates many innovative concepts and algorithmic frameworks and is still used today. For example, it pioneered the parallelization of tracking and mapping, allowing for parallel image tracking and back-end optimization, distinguishing between the front-end and back-end concepts. This approach is now widely adopted in most SLAM systems. It also abandoned the then-dominant back-end filter scheme and adopted nonlinear optimization for back-end optimization. At the time, the EKF method or its variants were widely used because it was widely believed that nonlinear optimization could not process large amounts of data in real time, without recognizing the inherent sparsity of back-end optimization. Subsequent visual SLAM technologies have largely been continuously improved upon PTAM.

[0004] In 2014, Engel et al. from the Technical University of Munich proposed the large-scale direct visual SLAM system LSD-SLAM (Large-Scale Direct Monocular SLAM), proposing the application of direct methods to visual SLAM systems to reduce time loss during feature extraction. LSD-SLAM proposed the relationship between pixel gradients and direct methods, as well as the angular relationship between pixel gradients and epipolar directions in dense reconstruction, which can achieve reconstruction of semi-dense scenes on the CPU. In 2015, R. Mur-Artal et al. from the University of Zaragoza in Spain proposed the monocular-based ORB-SLAM system, and iterated two versions in 2017 and 2021 respectively. ORB-SLAM inherits PTAM and is the pinnacle of visual SLAM technology based on feature point methods. After ORB-SLAM2, it supports three sensor modes: monocular, binocular, and RGB-D, and has excellent versatility. At the same time, a closed-loop detection and relocalization method based on the Bag of Word (BoW) model was proposed, which has very good real-time performance, stability and robustness. It can also run in real time relying solely on the CPU and is considered to be the most outstanding visual SLAM algorithm currently available.

[0005] Currently, there are two major research directions in visual SLAM: SLAM solutions that integrate visual-inertial navigation and semantic SLAM solutions combined with deep learning. Inertial measurement units (IMUs) can measure the angular velocity and acceleration of a vehicle, and are highly complementary to camera sensors. Fusion can create a more complete SLAM system. In 2018, Professor Shaojie Shen of the Chinese University of Hong Kong and others proposed the Vins-Mono system, which uses a monocular camera and an IMU to achieve tightly coupled visual and inertial joint state estimation, ultimately achieving promising results. With the global popularity of artificial intelligence and machine learning, semantic SLAM solutions have become a hot topic in current research. Representative achievements include the VSO (Visual Semantic Odometry) algorithm, proposed by Lianos et al. in 2018, which uses semantic information to establish constraints; and the SOF-SLAM (Semantic Optical Flow SLAM) algorithm, proposed by Cui Linyan et al. in 2019, which utilizes dynamic features hidden in semantic and geometric information to remove dynamic environmental interference. Summary of the Invention

[0006] Purpose of the invention: In order to realize three-dimensional navigation of specific scenes, based on the above-mentioned existing technologies, a three-dimensional grid map construction method based on an instant appearance mapping algorithm is proposed.

[0007] Technical solution: A method for constructing a three-dimensional grid map based on an instant appearance mapping algorithm, comprising the following steps:

[0008] Step 1: Equip the agent with an RGB-D camera and use a remote control to move around the target scene, or have a person hold the RGB-D camera to scan the target scene and build a dataset.

[0009] Step 2: Run the RTAB-Map algorithm based on the dataset constructed in step 1 to track the movement of the agent and finally generate point cloud map information;

[0010] Step 3: Use downsampling filtering to reduce the point cloud data of the point cloud map information generated in step 2, and then use the point cloud filtering algorithm to filter out outliers or erroneous points in the point cloud map to obtain a filtered point cloud map;

[0011] Step 4: Create an octree data structure and set the resolution. Then, insert the points in the point cloud map obtained in step 3 into the octree data structure to realize the conversion of the point cloud map to a three-dimensional grid map. The three-dimensional grid map is an octree map.

[0012] Furthermore, the intelligent agent includes an intelligent car or a drone.

[0013] Furthermore, in step 1, the RGB-D camera measures the distance between the object and the camera using infrared structured light or Time-of-Flight (ToF) technology. This measurement method is purely physical, so it does not require additional computing resources. Furthermore, this method is more reliable than distance information obtained through calculation using monocular or binocular cameras.

[0014] Furthermore, the data set information in step 1 includes one or more of image synchronization information, depth image information, color (grayscale) image information, and odometry information;

[0015] The odometer information includes one or more of a wheel speed odometer, an inertial navigation odometer, and an image odometer. If there is no reliable odometer information source, empty odometer information is sent. The RTAB-Map algorithm will capture this loss state and calculate the image odometer information through an algorithm based on the image information.

[0016] Furthermore, in order to realize the positioning of the camera position and the construction of the point cloud map based on the RTAB-Map algorithm, compared with other VSLAM algorithms, a major breakthrough of the RTAB-Map algorithm is the use of the WM and LTM memory management mechanism to limit the number of positions used for loop closure detection and graphics optimization, thereby improving the processing speed under large data sets.

[0017] The specific memory management mechanism involves transferring some nodes from the WM to the LTM when the RTAB-Map update time exceeds a fixed time threshold. Nodes transferred to the LTM are no longer applicable to the WM module. In addition to the time threshold, there is also a memory threshold, which indicates the maximum number of nodes that can be accommodated in the WM. The weight of a new node is initialized to 0 and compared with the last node to see if a loop occurs. This is done by using a bag-of-words model to determine image similarity. If a loop occurs, the weight of the new node is equal to the weight of the last node plus 1, and the weight of the last node is reset to 0. When either the time threshold or the memory threshold is reached, the oldest, lowest-weighted node is transferred to the LTM first. When a loop occurs at a location in the WM, its neighboring nodes are brought back from the LTM to the WM for further loop closure and similarity detection. As the robot moves through previously visited areas, it can gradually remember past locations, expanding the currently constructed map and using past locations for localization.

[0018] The specific execution steps of the RTAB-Map algorithm described in step 2 are:

[0019] 21) First, the information in the dataset is passed to the synchronization module in the RTAB-Map framework, which synchronizes the timestamps of each type of information.

[0020] 22) The synchronized sensor data is then passed to the STM module, which observes the temporal similarity of consecutive images and updates the weights of the positioning points.

[0021] 23) Then, loop closure detection is performed on the color image data to eliminate the accumulated error.

[0022] Loop closure detection uses the bag-of-words algorithm, which extracts visual feature points from color images and quantizes them into a cyclically increasing visual vocabulary. These vocabulary words usually include the size of pixel values ​​and the characteristics of neighboring points. Based on the vocabulary, each image can be represented by a vector, where the dimension of the vector is equal to the number of words in the vocabulary. The words in the vocabulary are denoted as w1, w2, w3..., then for image A, based on this vocabulary, it can be represented as

[0023] A=n1w1+n2w2+n3w3+…

[0024] where n xis the number of times the word appears in the image, so the image can be represented as [n1,n2,n3…]. Since this representation only indicates the number of words that appear, rather than the position of the words, similar images can still be detected when the camera position changes slightly. Suppose there are two images A and B, and vectors a and b are obtained based on the same vocabulary. For Their similarity can be expressed as

[0025]

[0026] Where W represents the number of elements in vectors a and b, that is, the number of words in the vocabulary; the norm is the L1 norm, which is 1 when the two vectors are the same, indicating that the two images can be regarded as taken at the same location; when the two vectors are completely opposite, the value is 0, indicating that the two images are completely unrelated; setting a loop threshold can determine whether the agent has looped under a certain error condition;

[0027] 24) Then, a graph optimization algorithm is used to uniformly optimize the odometer information and loop detection information. The optimization strategy is GTSAM.

[0028] 25) Finally, the point cloud map is constructed.

[0029] Furthermore, the construction of the point cloud map in step 25) specifically includes the following steps:

[0030] Assume that a confined space to be modeled is represented by a point cloud map X. Within X are several point cloud data points, described as follows: Each point cloud data point consists of six components: its spatial position x, y, z, and its color r, g, b. An RGB-D camera reads two types of data: a color image and a dark image. The color image provides the color data for the point cloud, while the spatial data must be calculated from these two images along with the camera model and pose.

[0031] For the pinhole camera model, a spatial point [x, y, z] and its corresponding coordinates in the image [x', y', d] have the following correspondence, where d represents the depth data of the spatial point and the camera is the origin of the spatial point coordinates;

[0032]

[0033] where f x and f y refers to the focal length of the camera on the x and y axes, c x and c y Refers to the center of the camera's aperture;

[0034] f x 、fy 、c x and c y Defined as the camera's intrinsic parameter matrix C, the spatial position and pixel coordinates of each point are represented by the following matrix model:

[0035]

[0036] in

[0037]

[0038] Each pixel in the image is transformed according to the above matrix formula and displayed in space to obtain a 3D point cloud map corresponding to the image. In SLAM, due to the existence of odometer and back-end optimization, it is very convenient to know the movement between two adjacent frames of images. Then, starting from the origin, the point cloud coordinates corresponding to the next frame of image are

[0039]

[0040] Among them, R and t represent the camera's posture transformation, R is the rotation transformation matrix, and t is the displacement vector. As the camera moves, the point cloud data is continuously enriched and spliced ​​together, and finally all the point cloud data of the target space are obtained to form a point cloud map of the target space.

[0041] Furthermore, the downsampling filtering in step 3 is mainly implemented by a voxel filtering algorithm. The main purpose of voxel filtering is to reduce the number of point clouds, specifically:

[0042] First, the point cloud map is divided into several voxel grids, and then the centroid of each voxel is calculated. Finally, the centroid point is used to replace all the point cloud data of the modified voxel, so as to achieve the function of downsampling without destroying the mechanical structure of the point cloud itself. Furthermore, the point cloud filtering algorithm described in step 3 includes a straight-through filtering algorithm or a statistical filtering algorithm;

[0043] The main purpose of pass-through filtering is to quickly filter out points within a custom range. The main method is to set a range value for the coordinate system and then detect the point cloud data in the file. If the coordinates of the point cloud data are within this range (or not within this range), the point cloud data is directly removed.

[0044] The main purpose of the Statistical Outlier Removal algorithm is to remove outliers from the point cloud map. It determines whether a point is an outlier by calculating the mean of the distance between each point and its k neighboring points. If the mean is large, the point is considered an outlier and the point cloud data is deleted.

[0045] Furthermore, the octree map in step 4 is a map represented by an octree, specifically:

[0046] First, the entire environment to be modeled is abstracted into a large cube, which is then divided into eight smaller cubes. These eight cubes are organized in the form of an octree. The recursive division continues until a pre-restricted or extreme level is reached. When all child nodes have the same probability of being occupied, the child nodes are pruned and only the parent node is retained. At this point, the parent node's level reaches its limit. Compared to point cloud data, the octree is a more efficient modeling method and consumes less space.

[0047] Each node in the octree stores a floating-point data x∈[0,1], which is used to represent the probability information of its occupancy. The specific implementation process is as follows:

[0048] set up is the logarithmic probability value, and its transformation with respect to x is described by the logit change:

[0049]

[0050] Its inverse transform is:

[0051] In this way, the mapping from (-∞,+∞) to [0,1] is realized;

[0052] Import the point cloud map obtained in step 3 into the octree map. Initially, y is 0 and x is 0.5. When point cloud data appears in the space corresponding to the node, increase the value of y, otherwise decrease the value of y, thereby indirectly realizing the change of x, that is, the probability.

[0053] Beneficial Effects: This paper proposes a 3D grid map construction method based on a real-time appearance mapping algorithm. By optimizing and supplementing the existing RTAB-Map algorithm, it constructs a 3D grid map based on an octree data structure. This method reduces memory usage while maintaining a basic spatial structure. This significantly reduces the complexity of collision detection and improves algorithm efficiency in subsequent 3D navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 This is the overall method structure diagram;

[0055] Figure 2 This is a pinhole camera model diagram;

[0056] Figure 3 This is a schematic diagram of an octree map. DETAILED DESCRIPTION

[0057] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be interpreted as limiting the present invention.

[0058] Step 1: First construct the dataset of the target scene, which should include Figure 1 The input information section includes image synchronization information, depth image information, color (grayscale) image information, and odometry information (optional). If other reliable odometry information, such as wheel speed odometers or inertial navigation odometers, is unavailable, image odometry information can be generated directly from the image. Multi-sensor information can also be integrated through information fusion algorithms such as the extended Kalman filter. To achieve loop closure detection and eliminate accumulated errors, the data collection must be closed-loop.

[0059] Step 2: Based on the dataset, the camera position trajectory positioning and point cloud map construction are realized through the RTAB-Map algorithm. The pixel points on each frame of the image correspond to the world coordinates

[0060]

[0061] Where R and t represent the camera's pose transformation, R is the rotation transformation matrix, and t is the displacement vector. C is the camera's intrinsic parameter matrix, which is only related to the camera's structure and has nothing to do with the external environment. It is expressed as follows:

[0062]

[0063] By mapping each pixel in the image to a point in space, and then scanning the entire target scene, we can obtain a 3D point cloud map of the entire target scene. Therefore, the prerequisite for accurate point cloud map construction is the accurate positioning of the camera trajectory.

[0064] Camera trajectory positioning information can be obtained through two methods: passive positioning and active positioning. Passive positioning, which relies on satellites, landmarks, and other methods, has high environmental requirements and is generally less accurate, making it unsuitable for autonomous navigation. Therefore, active positioning is generally used to achieve camera trajectory positioning, based on odometry information. This odometry information can include wheel speed odometers, inertial navigation odometers, or a fusion of multiple odometers.

[0065] However, odometry alone lacks feedback. Over time, errors from one moment inevitably accumulate, leading to cumulative errors in the entire system and making the final position information unreliable. To address this issue, the RTAB-Map algorithm introduces loop detection. The key idea is that when the camera detects an image that is similar or identical to a previously detected image, the system assumes that the smart car has moved to a previously visited location or a location near a previously visited location. At this point, repositioning is performed, eliminating accumulated errors. In the RTAB-Map algorithm, the bag-of-words model is used to determine the similarity between images.

[0066] 21) First, the information in the dataset is passed to the synchronization module in the RTAB-Map framework, which synchronizes the timestamps of each type of information;

[0067] 22) The synchronized sensor data is then passed to the STM module, which observes the temporal similarity of consecutive images and updates the weights of the positioning points.

[0068] 23) Then, loop closure detection is performed on the color image data. The loop closure detection uses the bag-of-words algorithm, that is, the STM extracts visual feature points from the color image and quantizes them into a visual vocabulary that can be cyclically increased. These vocabularies usually include the size of pixel values ​​and the features of neighboring points; based on the vocabulary, each image can be represented by a vector, where the dimension of the vector is equal to the number of words in the vocabulary; the words in the vocabulary are recorded as w1, w2, w3..., then for image A, based on the vocabulary, it is represented as

[0069] A=n1w1+n2w2+n3w3+……

[0070] where n x is the number of times the word appears in the image, so the image is represented as [n1,n2,n3…]; since this representation only indicates the number of word occurrences, but not the position of the word, similar images can still be detected when the camera position changes slightly; suppose there are two images A and B, and vectors a and b are obtained based on the same vocabulary. Their similarity is expressed as

[0071]

[0072] Where W represents the number of elements in vectors a and b, that is, the number of words in the vocabulary; the norm is the L1 norm, which is 1 when the two vectors are the same, indicating that the two images can be regarded as taken at the same location; when the two vectors are completely opposite, the value is 0, indicating that the two images are completely unrelated; setting a loop threshold can determine whether the agent has looped under a certain error condition;

[0073] 24) Then, a graph optimization algorithm is used to uniformly optimize the odometer information and loop detection information. The optimization strategy is GTSAM.

[0074] 25) Finally, construct the point cloud map;

[0075] Assume that a confined space to be modeled is represented by a point cloud map X. Within X are several point cloud data points, described as follows: Each point cloud data point consists of six components: its spatial position x, y, z, and its color r, g, b. An RGB-D camera reads two types of data: a color image and a dark image. The color image provides the color data for the point cloud, while the spatial data must be calculated from these two images along with the camera model and pose.

[0076] For the pinhole camera model, a spatial point [x, y, z] and its corresponding coordinates in the image [x', y', d] have the following correspondence, where d represents the depth data of the spatial point and the camera is the origin of the spatial point coordinates;

[0077]

[0078] where f x and f y refers to the focal length of the camera on the x and y axes, c x and c y Refers to the center of the camera's aperture;

[0079] f x 、f y 、c x and c y Defined as the camera's intrinsic parameter matrix C, the spatial position and pixel coordinates of each point are represented by the following matrix model:

[0080]

[0081] in

[0082]

[0083] Each pixel in the image is transformed according to the above matrix formula and displayed in space to obtain a 3D point cloud map corresponding to the image. In SLAM, due to the existence of odometer and back-end optimization, it is very convenient to know the movement between two adjacent frames of images. Then, starting from the origin, the point cloud coordinates corresponding to the next frame of image are

[0084]

[0085] Among them, R and t represent the camera's posture transformation, R is the rotation transformation matrix, and t is the displacement vector. As the camera moves, the point cloud data is continuously enriched and spliced ​​together, and finally all the point cloud data of the target space are obtained to form a point cloud map of the target space.

[0086] Step 3: After obtaining the point cloud map of the target scene, the point cloud map needs to be filtered. The filtering methods include downsampling and discrete point removal.

[0087] Among them, downsampling filtering is mainly implemented through the voxel filtering algorithm. The main purpose of voxel filtering is to reduce the number of point clouds. It first divides the point cloud map into several voxel grids, then calculates the center of mass of each voxel grid, and finally uses the center of mass point to replace all the point cloud data of the modified voxel grid, so as to achieve the downsampling function without destroying the mechanical structure of the point cloud itself.

[0088] In order to filter out outliers or erroneous points, you can use the pass-through filtering algorithm (Pass Through) or the statistical filtering algorithm (Statistical Outlier Removal). The main purpose of the pass-through filtering is to quickly filter out points within a custom interval. The main method is to set a range value of a coordinate system, and then detect the point cloud data in the file. If the coordinates of the point cloud data are within this range (or not within this range), the point cloud data is directly removed. The main purpose of the statistical filtering algorithm is to remove outliers in the point cloud map. It determines whether the point is an outlier by calculating the mean of the distance between each point and k adjacent points. If the mean is large, then the point is judged to be an outlier, and then the point cloud data is deleted.

[0089] Step 4. Finally, the filtered point cloud map data is converted into an octree map. The octree map is a construction method of a three-dimensional grid map, mainly based on the octree data structure. The basic idea is to first abstract the entire environment to be modeled into a large square, and then divide the square into eight small squares. These eight squares are organized in the form of an octree; and continue to divide recursively until the pre-restricted level or the limit level is reached; when the probability of all child nodes being occupied is the same, the child nodes are cut off and only the parent node is retained. At this time, the level of the parent node reaches the limit; compared to point cloud data, octree is a more efficient modeling method and consumes less space.

[0090] Each node in the octree stores a floating-point data x∈[0,1], which is used to represent the probability information of its occupancy. The specific implementation process is as follows:

[0091] set up is the logarithmic probability value, and its transformation with respect to x is described by the logit change:

[0092]

[0093] Its inverse transform is:

[0094] In this way, the mapping from (-∞,+∞) to [0,1] is realized;

[0095] Import the point cloud map obtained in step 3 into the octree map. Initially, y is 0 and x is 0.5. When point cloud data appears in the space corresponding to the node, increase the value of y, otherwise decrease the value of y, thereby indirectly realizing the change of x, that is, the probability.

[0096] Example:

[0097] Step 1: Use a handheld RGB-D camera and walk to construct a laboratory dataset. The dataset lasts 16 minutes and 41 seconds. The camera publishes image-related information at a 30Hz frequency, and odometry information is empty. The laboratory is a closed indoor space approximately 10m x 10m. To ensure loop closure, drive around the laboratory for one and a half weeks.

[0098] Step 2: Run the RTAB-Map algorithm based on the dataset to obtain the original point cloud map of the laboratory, with a total of 35.7 million point cloud data.

[0099] Step 3: Downsample the original point cloud map and remove outliers to obtain a point cloud map containing 1.66 million point cloud data.

[0100] Step 4: Construct a 3D grid map based on the filtered point cloud map.

[0101] The above embodiments are only for illustrating 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 present invention.

Claims

1. A three-dimensional grid map construction method based on an instant appearance mapping algorithm, characterized in that: The main steps include: Step 1: Equip the agent with an RGB-D camera and use a remote control to move around the target scene, or have a person hold the RGB-D camera to scan the target scene and build a dataset. Step 2: Run the RTAB-Map algorithm based on the dataset constructed in step 1 to track the movement of the agent and finally generate point cloud map information; Step 3: Use downsampling filtering to reduce the point cloud data of the point cloud map information generated in step 2, and then use the point cloud filtering algorithm to filter out outliers or erroneous points in the point cloud map to obtain a filtered point cloud map; Step 4: Create an octree data structure, set the resolution, and then insert the points in the point cloud map obtained in step 3 into the octree data structure to convert the point cloud map into a three-dimensional grid map. The three-dimensional grid map is an octree map. The specific execution steps of the RTAB-Map algorithm described in step 2 are: 21) First, the information in the dataset is passed to the synchronization module in the RTAB-Map framework, which synchronizes the timestamps of each type of information; 22) The synchronized sensor data is then passed to the STM module, which observes the temporal similarity of consecutive images and updates the weights of the positioning points. 23) Then, loop closure detection is performed on the color image data. The loop closure detection uses the bag-of-words algorithm, that is, the STM extracts visual feature points from the color image and quantizes them into a visual vocabulary that can be cyclically increased. These vocabularies usually include the size of the pixel value and the characteristics of the neighboring points; Based on the vocabulary, each image can be represented by a vector, where the dimension of the vector is equal to the number of words in the vocabulary; the words in the vocabulary are denoted as w1, w2, w3…, then for image A, based on the vocabulary, it can be represented as A=n1w1+n2w2+n3w3+… where n x is the number of times the word appears in the image, so the image is represented as [n1,n2,n3…]; since this representation only indicates the number of word occurrences, but not the position of the word, similar images can still be detected when the camera position changes slightly; suppose there are two images A and B, and vectors a and b are obtained based on the same vocabulary. Their similarity is expressed as Where W represents the number of elements in vectors a and b, that is, the number of words in the vocabulary; the norm is the L1 norm, which is 1 when the two vectors are the same, indicating that the two images can be regarded as taken at the same location; when the two vectors are completely opposite, the value is 0, indicating that the two images are completely unrelated; setting a loop threshold can determine whether the agent has looped under a certain error condition; 24) Then, a graph optimization algorithm is used to uniformly optimize the odometer information and loop detection information. The optimization strategy is GTSAM. 25) Finally, the point cloud map is constructed.

2. The three-dimensional grid map construction method according to claim 1, wherein: The intelligent agent includes an intelligent car or a drone.

3. The three-dimensional grid map construction method according to claim 1, wherein: In step 1, the RGB-D camera measures the distance between the object and the camera using infrared structured light or Time-of-Flight principle.

4. The three-dimensional grid map construction method according to claim 1, wherein: The data set information in step 1 includes one or more of image synchronization information, depth image information, color or grayscale image information, and odometry information; The odometer information includes one or more of a wheel speed odometer, an inertial navigation odometer, and an image odometer. If there is no reliable odometer information source, empty odometer information is sent. The RTAB-Map algorithm will capture this loss state and calculate the image odometer information through an algorithm based on the image information.

5. The three-dimensional grid map construction method according to claim 1, wherein: Step 25) The construction of the point cloud map specifically includes the following steps: Assume that a confined space to be modeled is represented by a point cloud map X. Within X are several point cloud data points, described as follows: Each point cloud data point consists of six components: its spatial position x, y, z, and its color r, g, b. An RGB-D camera reads two types of data: a color image and a dark image. The color image provides the color data for the point cloud, while the spatial data must be calculated from these two images along with the camera model and pose. For the pinhole camera model, a spatial point [x, y, z] and its corresponding coordinates in the image [x', y', d] have the following correspondence, where d represents the depth data of the spatial point and the camera is the origin of the spatial point coordinates; where f x and f y refers to the focal length of the camera on the x and y axes, c x and c y Refers to the center of the camera's aperture; f x 、f y 、c x and c y Defined as the camera's intrinsic parameter matrix C, the spatial position and pixel coordinates of each point are represented by the following matrix model: in Each pixel in the image is transformed according to the above matrix formula and displayed in space to obtain a 3D point cloud map corresponding to the image; In SLAM, due to the existence of odometer and backend optimization, it is very convenient to know the movement between two adjacent frames of images. Then, starting from the origin, the point cloud coordinates corresponding to the next frame of image are Among them, R and t represent the camera's posture transformation, R is the rotation transformation matrix, and t is the displacement vector. As the camera moves, the point cloud data is continuously enriched and spliced ​​together, and finally all the point cloud data of the target space are obtained to form a point cloud map of the target space.

6. The three-dimensional grid map construction method according to claim 1, wherein: The downsampling filtering described in step 3 is mainly implemented by the voxel filtering algorithm, which can reduce the number of point clouds. Specifically: First, the point cloud map is divided into several voxel grids, then the center of mass of each voxel is calculated, and finally the center of mass point is used to replace all the point cloud data of the modified voxel, so as to achieve the downsampling function without destroying the mechanical structure of the point cloud itself.

7. The three-dimensional grid map construction method according to claim 1, wherein: The point cloud filtering algorithm in step 3 includes a straight-through filtering algorithm or a statistical filtering algorithm; The straight-through filtering algorithm can quickly filter out points within a custom range. Specifically, a coordinate range value is set, and then the point cloud data in the file is detected. If the coordinates of the point cloud data are within or outside the range, the point cloud data is directly removed. The statistical filtering algorithm can remove outliers in the point cloud map. Specifically, it determines whether a point is an outlier by calculating the mean of the distance between each point and its k neighboring points; if the mean is large, the point is determined to be an outlier and the point cloud data is deleted.

8. The three-dimensional grid map construction method according to claim 1, wherein: The octree map described in step 4 is a map represented by an octree, specifically: First, the entire environment to be modeled is abstracted into a large cube. This cube is then divided into eight smaller cubes, which are organized into an octree. This recursive division continues until a pre-restricted or limit level is reached. When all child nodes have the same probability of being occupied, the child nodes are pruned, leaving only the parent node. At this point, the parent node's level reaches its limit. Each node in the octree stores a floating-point data x∈[0,1], which is used to represent the probability information of its occupancy. The specific implementation process is as follows: set up is the logarithmic probability value, and its transformation with respect to x is described by the logit change: Its inverse transform is: In this way, the mapping from (-∞,+∞) to [0,1] is realized; Import the point cloud map obtained in step 3 into the octree map. Initially, y is 0 and x is 0.

5. When point cloud data appears in the space corresponding to the node, increase the value of y, otherwise decrease the value of y, thereby indirectly realizing the change of x, that is, the probability.

Images