A method and system for constructing a terrain semantic map in a field environment

Abstract

The application discloses a terrain semantic map construction method and system in a field environment, relates to the technical field of map construction, inputs a current key frame into a trained target detection model, obtains dynamic object recognition results and terrain segmentation results, removes three-dimensional points in a surrounding box corresponding to each dynamic object in the current key frame based on the dynamic object recognition results, obtains a current processed key frame, then performs feature extraction and feature matching on the current processed key frame and a last processed key frame, obtains a pose transformation matrix, and further constructs an environment map, so that the influence of dynamic objects can be removed in the process of constructing the environment map, the mapping precision is improved, finally, the terrain segmentation results and the environment map are superimposed and fused to obtain a terrain semantic map, so that terrain semantic information can be included in the finally constructed environment map, a terrain semantic map is obtained, the positioning of the unmanned vehicle is more accurate, and the autonomous movement of the unmanned vehicle is facilitated.

Description

Technical Field

[0001] This invention relates to the field of map construction technology, and in particular to a method and system for constructing terrain semantic maps in a field environment. Background Technology

[0002] With the continuous development of science and technology and the progress of the times, new requirements have been placed on intelligent agents. Autonomous vehicles (RVs), as typical intelligent agents, have attracted much attention from researchers due to their application scenarios. The basic driving tasks of RVs can generally be divided into three parts: environmental perception, path planning, and vehicle control. Environmental perception refers to the RV using its onboard sensors (such as cameras and lidar) to perceive the surrounding unknown environment, understand the objects present in the unknown environment and the RV's current position, and process and analyze the data collected by the sensors to obtain the current environmental map. Path planning refers to the RV rationally planning the path from its current position to the target point after obtaining the target point's location, involving tasks such as dynamic obstacle avoidance, local path planning, and global path planning. Vehicle control refers to the RV controlling its own motion units based on the path planning results, such as controlling the motor speed, so that the RV can safely reach the target point. However, in extreme environments in the wild, the constant changes in the spatial environment lead to a decrease in the applicability of prior maps or the complete absence of environmental maps, resulting in extremely poor autonomous operation capabilities for RVs in wild environments.

[0003] Currently, there are two main methods for constructing environmental maps in the field: (1) Using a monocular camera or depth camera as a sensor for unmanned vehicles to perceive the environment. After aligning the depth map, feature extraction and feature matching are performed on the two consecutive frames of images to calculate the camera rotation matrix. By transforming the camera rotation matrix, the pose transformation of the unmanned vehicle can be obtained, and then a visual odometry can be constructed to obtain the environmental map. (2) Using a lidar as a sensor for unmanned vehicles to perceive the environment. Feature extraction is performed on the two consecutive frames of point cloud data collected by the lidar. The pose transformation matrix of the lidar is calculated using the extracted features, and then the pose transformation matrix of the unmanned vehicle itself can be obtained. The lidar odometry can be constructed through the pose transformation matrix of the unmanned vehicle, and then the environmental information of the unknown environment can be obtained and an environmental map can be constructed. However, both of the above existing methods have the defects that dynamic objects in the environment will greatly affect the mapping accuracy and that terrain semantic information is not included in the final constructed environmental map.

[0004] Therefore, there is an urgent need for a new type of terrain semantic map construction technology in the field environment. Summary of the Invention

[0005] The purpose of this invention is to provide a method and system for constructing terrain semantic maps in the field environment. This method can remove the influence of dynamic objects during the construction of the environmental map, improve the mapping accuracy, and incorporate terrain semantic information into the final constructed environmental map to obtain a terrain semantic map. This makes the autonomous vehicle more accurate in its own positioning and is conducive to the autonomous movement of the autonomous vehicle.

[0006] To achieve the above objectives, the present invention provides the following solution:

[0007] A method for constructing a terrain semantic map in a field environment, the method comprising:

[0008] Obtain the current keyframe; the current keyframe is a frame of three-dimensional environmental point cloud data obtained by scanning the surrounding environment of the unmanned vehicle with the lidar on the unmanned vehicle when the unmanned vehicle is moving in the field environment.

[0009] The current keyframe is input into the trained target detection model to obtain dynamic object recognition results and terrain segmentation results; the dynamic object recognition results include the position of the bounding box corresponding to each dynamic object in the current keyframe, and the dynamic objects include people and vehicles; the terrain segmentation results include terrain semantic information corresponding to the three-dimensional points located on the ground in the current keyframe, and the terrain semantic information includes grassland, land, and concrete.

[0010] Based on the dynamic object recognition results, remove the three-dimensional points in the bounding box corresponding to each dynamic object in the current keyframe to obtain the current processed keyframe.

[0011] Feature extraction and feature matching are performed on the current processed keyframe and the previous processed keyframe to obtain the pose transformation matrix between the current processed keyframe and the previous processed keyframe, and an environment map is constructed based on the pose transformation matrix.

[0012] The terrain segmentation results and the environment map are overlaid and fused to obtain a terrain semantic map.

[0013] In some embodiments, obtaining the current keyframe specifically includes:

[0014] During the movement of the unmanned vehicle in the field environment, the three-dimensional environmental point cloud data obtained by the LiDAR on the unmanned vehicle scanning the surrounding environment is acquired in real time; the LiDAR is a solid-state LiDAR.

[0015] Every N frames, one frame of the 3D environment point cloud data is selected as the current keyframe.

[0016] In some embodiments, before inputting the current keyframe into the trained target detection model, the construction method further includes: preprocessing the current keyframe, specifically including:

[0017] The current keyframe is denoised using a joint clustering method based on Euclidean clustering and K-Means clustering to obtain a denoised keyframe.

[0018] The coordinate transformation of the denoised keyframe is performed using the pose transformation matrix between the unmanned vehicle and the lidar obtained in the pre-calibration. The coordinates of each three-dimensional point in the denoised keyframe are transformed into the coordinate system of the unmanned vehicle to obtain the transformed keyframe, and the transformed keyframe is used as the new current keyframe.

[0019] In some embodiments, the step of performing feature extraction and feature matching on the current processed keyframe and the previous processed keyframe to obtain the pose transformation matrix between the current processed keyframe and the previous processed keyframe specifically includes:

[0020] Feature extraction is performed on the current processed keyframe to obtain the features of the current processed keyframe; the features of the current processed keyframe include the type of each three-dimensional point in the current processed keyframe, and the type includes corner feature points and surface feature points;

[0021] The features of the current processed keyframe are matched with the features of the previous processed keyframe to obtain a feature matching result; the feature matching result includes successfully matched feature point pairs, and the feature point pairs include a three-dimensional point in the current processed keyframe and a three-dimensional point in the previous processed keyframe.

[0022] Based on the feature matching results, calculate the pose transformation matrix between the current processed keyframe and the previous processed keyframe.

[0023] In some embodiments, calculating the pose transformation matrix between the current processed keyframe and the previous processed keyframe based on the feature matching result specifically includes: calculating the pose transformation matrix between the current processed keyframe and the previous processed keyframe using the ICP algorithm based on the feature matching result.

[0024] In some embodiments, constructing an environment map based on the pose transformation matrix specifically includes:

[0025] The coordinates of each 3D point in the current processed keyframe are transformed to the coordinate system corresponding to the first keyframe using the pose transformation matrix to obtain the coordinate transformation keyframe.

[0026] The coordinate transformation keyframe is overlaid with the historical environment map corresponding to the previous processed keyframe to obtain the environment map.

[0027] In some embodiments, before overlaying and fusing the terrain segmentation results and the environment map, the construction method further includes:

[0028] The NDT algorithm is used to match the current processed keyframe with the environment map to obtain a matching score;

[0029] Determine whether the matching score is greater than a preset threshold;

[0030] If so, the environment map is optimized using graph optimization methods to obtain an optimized map, and the optimized map is used as the new environment map.

[0031] In some embodiments, after obtaining the terrain semantic map, the construction method further includes:

[0032] The terrain semantic map was clustered using the Euclidean clustering algorithm to obtain multiple clusters.

[0033] Calculate the centroid of each cluster;

[0034] In the terrain semantic map, only the centroid of each cluster is retained to obtain a new terrain semantic map.

[0035] A terrain semantic map construction system for a field environment, the system comprising:

[0036] The keyframe acquisition module is used to acquire the current keyframe; the current keyframe is a frame of three-dimensional environmental point cloud data obtained by the lidar on the unmanned vehicle scanning the environment around the unmanned vehicle when the unmanned vehicle is moving in the field environment.

[0037] The object detection module is used to input the current keyframe into a trained object detection model to obtain dynamic object recognition results and terrain segmentation results. The dynamic object recognition results include the position of the bounding box corresponding to each dynamic object in the current keyframe, and the dynamic objects include people and vehicles. The terrain segmentation results include terrain semantic information corresponding to three-dimensional points located on the ground in the current keyframe, and the terrain semantic information includes grassland, land, and concrete.

[0038] The dynamic object removal module is used to remove the three-dimensional points in the bounding box corresponding to each dynamic object in the current key frame based on the dynamic object recognition result, so as to obtain the current processed key frame.

[0039] The map building module is used to extract and match features from the current processed keyframe and the previous processed keyframe to obtain the pose transformation matrix between the current processed keyframe and the previous processed keyframe, and to build an environment map based on the pose transformation matrix.

[0040] The terrain information overlay module is used to overlay and fuse the terrain segmentation results and the environmental map to obtain a terrain semantic map.

[0041] According to specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0042] This invention provides a method and system for constructing a terrain semantic map in a field environment. The method involves inputting the current keyframe into a trained target detection model to obtain dynamic object recognition results and terrain segmentation results. Based on the dynamic object recognition results, 3D points within the bounding boxes corresponding to each dynamic object are removed from the current keyframe to obtain a processed keyframe. Feature extraction and feature matching are then performed on the current and previous processed keyframes to obtain a pose transformation matrix, which is used to further construct an environmental map. This process removes the influence of dynamic objects during environmental map construction, improving mapping accuracy. Finally, the terrain segmentation results and the environmental map are overlaid and fused to obtain a terrain semantic map. This allows terrain semantic information to be incorporated into the final constructed environmental map, resulting in a terrain semantic map that enables more accurate localization of unmanned vehicles (UAVs) and facilitates autonomous vehicle movement. Attached Figure Description

[0043] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0044] Figure 1 This is a flowchart of the construction method provided in Embodiment 1 of the present invention;

[0045] Figure 2 This is a schematic diagram of the construction method provided in Embodiment 1 of the present invention;

[0046] Figure 3 This is a schematic diagram of the structure of the unmanned vehicle mobile platform provided in Embodiment 1 of the present invention;

[0047] Figure 4 This is a schematic diagram of the field operation scenario of the unmanned vehicle provided in Embodiment 1 of the present invention;

[0048] Figure 5This is a schematic diagram of the three-dimensional environmental point cloud data acquired by the lidar provided in Embodiment 1 of the present invention;

[0049] Figure 6 This is a schematic diagram of the dynamic object recognition result provided in Embodiment 1 of the present invention;

[0050] Figure 7 This is a schematic diagram of the terrain segmentation results provided in Embodiment 1 of the present invention;

[0051] Figure 8 This is a system block diagram of the construction system provided in Embodiment 2 of the present invention.

[0052] Symbol explanation:

[0053] 1-Unmanned vehicle body; 2-LCD display; 3-Industrial control computer system; 4-Solid-state lidar. Detailed Implementation

[0054] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0055] The purpose of this invention is to provide a method and system for constructing terrain semantic maps in the field environment. This method can remove the influence of dynamic objects during the construction of the environmental map, improve the mapping accuracy, and incorporate terrain semantic information into the final constructed environmental map to obtain a terrain semantic map. This makes the autonomous vehicle more accurate in its own positioning and is conducive to the autonomous movement of the autonomous vehicle.

[0056] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0057] Currently, when constructing environmental maps in the wild, autonomous vehicles (RVs) perceive unknown environments by using their onboard cameras (monocular or depth cameras) as sensors. The acquired data is in the form of images, which possess rich color and texture information. Therefore, images can output the geometric features of the scene. When using a monocular camera, the depth information of objects in the scene is recovered by triangulation of the acquired color image using polar geometry. When using a depth camera, the depth information of objects in the scene is directly obtained from the color and depth images captured by the depth camera. Then, using the color image and the corresponding depth image as input, the world coordinates of objects in the environment can be derived using their relationship for depth map alignment. Feature extraction and feature matching are then performed on the aligned images, and the camera rotation matrix is ​​calculated. By transforming the camera rotation matrix, the pose transformation of the RV can be obtained, thus enabling the construction of a visual odometry system to generate an environmental map of the unknown environment. However, the main drawbacks of the above-mentioned camera-based environmental map construction method are: (1) It is difficult to extract image features stably when the illumination changes greatly. The effective range of the depth camera is within 10m. In the field environment, it is very common for objects to exceed the range. This leads to inaccurate depth information collected by the depth camera. In the field environment, the lack of environmental features of the monocular camera will lead to triangulation difficulties, which in turn leads to map construction failure; (2) Dynamic objects in the environment will greatly affect the accuracy of map construction; (3) The terrain semantic information is not included in the final constructed environmental map, which greatly affects the autonomous movement of the unmanned vehicle.

[0058] To avoid the unstable feature extraction issues inherent in camera-based environmental mapping methods, which rely on cameras as sensors, using LiDAR (Light Detection and Ranging) as an onboard sensor for the autonomous vehicle (RV) is a solution. LiDAR collects 3D point cloud data, which directly reflects the depth information of objects in the environment. It avoids the problem of a short effective range and eliminates the need for subsequent triangulation. Therefore, LiDAR-based environmental mapping improves the accuracy of the raw data. This method primarily involves: using LiDAR as the RV's environmental sensor; extracting environmental features from two consecutive frames of 3D point cloud data acquired by the LiDAR; performing feature matching on the extracted environmental features from the two frames; calculating the LiDAR's pose transformation matrix; further solving for the RV's own pose transformation matrix; constructing a LiDAR odometry using the RV's own pose transformation matrix; and then mapping the unknown environment using the constructed LiDAR odometry to obtain environmental information and a map. However, the main drawbacks of the above-mentioned method of constructing environmental maps based on lidar are: (1) The lidar used is generally mechanical lidar, and the price of general mechanical lidar is relatively high; (2) The large amount of point cloud data collected by lidar in the field environment leads to the presence of noise, which may cause the subsequent environmental map construction to fail; (3) Dynamic objects in the environment will greatly affect the accuracy of the map construction; (4) The constructed environmental map does not contain terrain semantic information, that is, the terrain semantic information is not included in the final constructed environmental map, which leads to the unmanned vehicle's inaccurate positioning and greatly affects the autonomous movement of the unmanned vehicle.

[0059] Based on the above analysis, it is evident that both existing methods suffer from drawbacks: dynamic objects in the environment significantly impact mapping accuracy, and the final environmental map lacks terrain semantic information. The purpose of this invention is to overcome these shortcomings by proposing a terrain semantic map construction technique for outdoor environments. This technique addresses the issues of dynamic objects affecting mapping accuracy and the absence of terrain semantic information in the resulting environmental maps when unmanned vehicles construct maps in unknown environments.

[0060] Example 1:

[0061] This embodiment provides a method for constructing terrain semantic maps in a field environment, such as... Figure 1 and Figure 2 As shown, the construction method includes:

[0062] S1: Obtain the current key frame; the current key frame is a frame of three-dimensional environmental point cloud data obtained by the lidar on the unmanned vehicle scanning the environment around the unmanned vehicle when the unmanned vehicle is moving in the field environment.

[0063] like Figure 3 As shown, this is a schematic diagram of the structure of an unmanned vehicle mobile platform in the field, and also a schematic diagram of the hardware system structure on which the construction method of this embodiment is based. It includes the unmanned vehicle body 1 and the LCD display 2, industrial control computer system 3, and solid-state LiDAR 4 mounted on the unmanned vehicle body 1. The unmanned vehicle body 1 drives the solid-state LiDAR 4 to work in the field environment. Figure 4 A schematic diagram of a field environment is provided. During the movement of the unmanned vehicle 1 in this environment, the solid-state LiDAR 4 continuously scans the surrounding environment of the unmanned vehicle 1's current location, acquiring real-time 3D environmental point cloud data. The 3D environmental point cloud data is shown below. Figure 5 As shown, the industrial control computer system 3 processes the 3D environmental point cloud data using S1-S5 to construct a terrain semantic map.

[0064] Considering the frame rate of the solid-state lidar 4 and the processing speed of the industrial control computer system 3, this embodiment will not process every frame of 3D environmental point cloud data acquired by the solid-state lidar 4. Instead, it will select key frames and process only the selected key frames. Key frame selection includes: starting from the first frame, a key frame is selected every 10 frames. This key frame represents the past 10 frames. That is, the first frame of 3D environmental point cloud data is used as the first key frame, the eleventh frame of 3D environmental point cloud data is used as the second key frame, and so on. Specifically, S1 may include: during the movement of the unmanned vehicle in the field environment, acquiring in real time the 3D environmental point cloud data obtained by the lidar 4 on the unmanned vehicle scanning the environment around the vehicle. Every N frames, a frame of 3D environmental point cloud data is selected as the current key frame.

[0065] This embodiment uses a solid-state lidar 4, and proposes a novel method for constructing terrain semantic maps based on solid-state lidar 4 in the field. This method uses the solid-state lidar 4 mounted on the unmanned vehicle body 1 to collect point cloud information in the environment. The advantage of solid-state lidar 4 is that it collects data by scanning the environment irregularly. Compared with mechanical lidar, which performs a 360° all-round scan of the environment, solid-state lidar 4 only scans the environment within its field of view, and its cost is much lower than that of mechanical lidar.

[0066] To address the issue of widespread noise in the large amount of point cloud data collected by LiDAR in field environments, which can lead to the failure of subsequent environmental map construction, the construction method in this embodiment, prior to S2, further includes: preprocessing the current keyframe, specifically including:

[0067] (1) Use a joint clustering method based on Euclidean clustering and K-Means clustering to denoise the current keyframe and obtain the denoised keyframe.

[0068] Specifically, denoising the current keyframe using a joint clustering method based on Euclidean clustering and K-Means clustering can include: clustering the current keyframe using Euclidean clustering to obtain isolated points not in any cluster; clustering the current keyframe using K-Means clustering to obtain isolated points not in any cluster; identifying duplicate points among the isolated points determined by Euclidean and K-Means clustering, and removing all duplicate points from the current keyframe to obtain the denoised keyframe. Specifically, the Euclidean clustering algorithm initially sets a clustering threshold and a minimum number of points in each cluster, using the spatial distance between different points to determine isolated points in the entire space. The K-Means clustering algorithm initially sets the number of clusters and uses the distance between different points to determine isolated points in the entire space. Both Euclidean and K-Means clustering algorithms are existing algorithms and will not be elaborated upon here.

[0069] Combining Euclidean clustering and K-Means clustering algorithms can reliably remove outliers and complete the noise reduction process.

[0070] (2) Use the pose transformation matrix between the unmanned vehicle and the lidar obtained by pre-calibration to perform coordinate transformation on the denoised key frame, transform the coordinates of each three-dimensional point in the denoised key frame to the coordinate system of the unmanned vehicle, obtain the transformed key frame, and use the transformed key frame as the new current key frame.

[0071] The pose transformation matrix between the autonomous vehicle and the LiDAR can be obtained by using any mature robot hand-eye calibration method. That is, in this embodiment, the pose transformation matrix T1 between the solid-state LiDAR 4-coordinate system and the autonomous vehicle body 1-coordinate system will be pre-calibrated. 4 .

[0072] The transformation formula used to convert the coordinates of each 3D point in the denoised keyframe to the coordinate system of the autonomous vehicle is as follows:

[0073] K = kT1 4 ;

[0074] Where K is the coordinate of the 3D point in the autonomous vehicle coordinate system; k is the coordinate of the 3D point in the lidar coordinate system.

[0075] Using the above conversion formula, the coordinates of each three-dimensional point in the denoised keyframe in the lidar coordinate system can be converted into the coordinates in the autonomous vehicle coordinate system. The resulting coordinates of each three-dimensional point in the transformed keyframe are the coordinates in the autonomous vehicle coordinate system.

[0076] It should be noted that in this embodiment, keyframes are selected first and then preprocessed. However, in actual operation, each 3D environmental point cloud data collected by the lidar can be preprocessed first, and then keyframes can be selected from the preprocessed 3D environmental point cloud data. The order of keyframe selection and preprocessing should not be limited to the scope of protection of this invention.

[0077] S2: Input the current keyframe into the trained target detection model to obtain dynamic object recognition results and terrain segmentation results; the dynamic object recognition results include the position of the bounding box corresponding to each dynamic object in the current keyframe, and the dynamic objects include people and vehicles; the terrain segmentation results include terrain semantic information corresponding to the three-dimensional points located on the ground in the current keyframe, and the terrain semantic information includes grassland, land, and concrete.

[0078] This embodiment utilizes a pre-trained object detection model to simultaneously perform terrain segmentation and dynamic object recognition. The current keyframe is input into the trained object detection model, which then outputs terrain segmentation results and dynamic object recognition results, respectively. For example, ... Figure 5 The current keyframe shown is input to the trained object detection model, which will output... Figure 6 The dynamic object recognition results shown are Figure 7 The terrain segmentation results are shown below. Dynamic object recognition involves using bounding boxes to enclose dynamic objects in the current keyframe. Each dynamic object is surrounded by a bounding box, thus each dynamic object corresponds to a bounding box. The bounding box can be a 3D rectangle. The dynamic object recognition result also includes the position of each bounding box. When the bounding box is a rectangle, its position P can be represented as P(x,y,z,h,w,c), where x, y, and z represent the 3D coordinates of the bounding box's center, and h, w, and c represent the length, width, and height of the bounding box. Terrain segmentation identifies the terrain semantic information corresponding to each 3D point on the ground in the current keyframe. 3D points with the same terrain semantic information are grouped together to form a terrain region, thus segmenting the ground of the current keyframe into multiple terrain regions and separating different terrain regions. Figure 7The terrain segmentation result shown is for a partial area. The left side is a concrete road, the right side is grass, and there are five static obstacles on the left side, which are not dynamic objects and therefore were not removed.

[0079] The pre-trained object detection model used in this embodiment is a 3D object detection model, which can employ existing convolutional neural network models, such as PointPillars. The training method can be as follows: A training set is constructed, comprising multiple samples and corresponding labels for each sample. A sample can be a frame of 3D environment point cloud data, and the labels can be the location of the bounding box corresponding to a dynamic object in the 3D environment point cloud data and the terrain semantic information corresponding to 3D points on the ground. The initial object detection model is then trained using the training set to obtain the pre-trained object detection model.

[0080] S3: Based on the dynamic object recognition result, remove the three-dimensional points in the bounding box corresponding to each dynamic object in the current key frame to obtain the current processed key frame;

[0081] In the point cloud data collected by LiDAR, the data type is a series of three-dimensional points based on their own three-dimensional spatial coordinate system. Therefore, in this embodiment, the point cloud in the current key frame that is in the bounding box of the obtained dynamic object is directly removed by using the position of the bounding box of the dynamic object. This will obtain the current processed key frame and complete the process of removing dynamic objects in the key frame, thereby avoiding the influence of dynamic objects in the environmental map construction process and improving the mapping accuracy.

[0082] S4: Perform feature extraction and feature matching on the current processed keyframe and the previous processed keyframe to obtain the pose transformation matrix between the current processed keyframe and the previous processed keyframe, and construct an environment map based on the pose transformation matrix.

[0083] In S4, feature extraction and feature matching are performed on the current processed keyframe and the previous processed keyframe to obtain the pose transformation matrix between the current processed keyframe and the previous processed keyframe, which may include:

[0084] (1) Extract features from the current processed keyframe to obtain the features of the current processed keyframe. The features of the current processed keyframe include the type of each three-dimensional point in the current processed keyframe. The types include corner feature points and surface feature points.

[0085] In the current processed keyframe, each 3D point must belong to either a corner feature point or a surface feature point. The feature extraction process in this embodiment may include: based on 8 consecutive non-blind zone points in the current processed keyframe, using consistency to determine whether the 8 non-blind zone points form a plane. If they form a plane, all 8 non-blind zone points are set as plane points to be determined, and the plane formed by the 8 non-blind zone points is the current plane. The normal vectors of the previous plane (the previous plane refers to the plane formed by the 8 non-blind zone points determined in the previous iteration) and the current plane are calculated. If the angle between the two normal vectors is between 45 degrees and 135 degrees, the above 8 plane points to be determined are considered to be points on the edges of the two planes, which are essentially corner feature points; otherwise, the above 8 plane points to be determined are considered to be true plane points, which are essentially surface feature points. Then, the next 8 consecutive non-blind zone points are selected, and the step of "based on 8 consecutive non-blind zone points in the current processed keyframe, using consistency to determine whether the 8 non-blind zone points form a plane" is returned to perform the next iteration until the number of remaining non-blind zone points in the current processed keyframe is less than 8.

[0086] In this context, all regions in space should be continuous, and each three-dimensional point within a sufficiently small radius can be considered a plane. The process of determining whether eight non-blind zone points form a plane using consistency can include: calculating the distance between two points (let's say a and b) among the eight non-blind zone points; then calculating the distances from the remaining six points to the line formed by a and b (let's say this point is c). This allows us to calculate the area of ​​a parallelogram with the line formed by a and b as its base and the distance from c to the line formed by a and b as its height. This yields six areas (each of the remaining six points corresponds to one area). The three points used for the parallelogram with the largest area are selected as the plane fitting points, and the plane equation is calculated to determine the plane. A plane is formed by three plane fitting points. The distances from the other five points (excluding the three plane fitting points) to this plane are calculated. If the distances from all five points to the plane are less than a preset value, then eight non-blind zone points constitute a plane. If there are N (N less than or equal to 5) points whose distances to the plane are not less than the preset value, then these N points are considered corner feature points. N new points are selected from the vicinity of the eight non-blind zone points. The N new points and the remaining points from the previous eight non-blind zone points (excluding the N points whose distances to the plane are not less than the preset value) form a new set of eight non-blind zone points. The process returns to the step of "based on the eight consecutive non-blind zone points in the current processed keyframe, use consistency to determine whether the eight non-blind zone points constitute a plane" and proceeds to the next iteration.

[0087] It should be noted that the data obtained by the solid-state lidar 4 when collecting data from points very close to it is inaccurate. Therefore, the area formed by points very close to the solid-state lidar 4 in the current processed keyframe is called the "blind zone". The range of the blind zone is usually described in the user manual and is a predetermined known range. In the current processed keyframe, the points located within the blind zone are the blind zone points, and the points located outside the blind zone are the non-blind zone points.

[0088] (2) Perform feature matching between the features of the current processed keyframe and the features of the previous processed keyframe to obtain the feature matching result. The feature matching result includes successfully matched feature point pairs. The feature point pairs include a three-dimensional point in the current processed keyframe and a three-dimensional point in the previous processed keyframe.

[0089] The feature matching process in this embodiment can employ existing keyframe-based nearest neighbor matching methods for feature points. This can include: for each 3D point in the current processed keyframe, regardless of whether the 3D point is a corner feature point or a polygon feature point, using the KD-tree method to find the corresponding point of the 3D point in the previous processed keyframe, calculating the distance between the 3D point and the corresponding point, and if the distance is less than a preset threshold, then the 3D point and its corresponding point are considered to be successfully matched. All successfully matched 3D points and their corresponding points constitute the feature matching result. However, it should be noted that the methods for determining the corresponding point and calculating the distance are different for corner feature points and polygon feature points.

[0090] (3) Calculate the pose transformation matrix between the current processed keyframe and the previous processed keyframe based on the feature matching results.

[0091] This embodiment is based on feature matching results. Specifically, it uses the coordinates of each 3D point (belonging to the current processed keyframe) and the coordinates of the corresponding point (belonging to the previous processed keyframe) in the feature matching results as input, and uses the ICP algorithm to calculate the pose transformation matrix between the current processed keyframe and the previous processed keyframe.

[0092] In this embodiment, features are first extracted from the current processed keyframe after removing dynamic objects. Then, the features of the current processed keyframe are matched with the features of the previous processed keyframe (i.e., the processed keyframe before the current processed keyframe). The pose transformation matrix can be obtained using the feature matching results, and then the pose of the unmanned vehicle can be tracked.

[0093] In S4, constructing an environment map based on the pose transformation matrix can include:

[0094] (1) Use the pose transformation matrix to transform the coordinates of each three-dimensional point in the current processed keyframe to the coordinate system corresponding to the first keyframe to obtain the coordinate transformation keyframe.

[0095] The pose transformation matrix T1 between the solid-state lidar 4-coordinate system and the unmanned vehicle body 1-coordinate system in S1 is used. 4 During preprocessing, the coordinates of the keyframes have been transformed into the coordinate system of the autonomous vehicle. The autonomous vehicle records its own position information during the movement, so it is only necessary to multiply by a pose transformation matrix of the autonomous vehicle's movement relative to the initial point (this pose transformation matrix is ​​provided by the program and calculated by accumulating the initial movement). That is, multiply all the pose transformation matrices obtained from the first keyframe to the current keyframe, and then multiply by the current processed keyframe to transform the coordinates of each 3D point in the current processed keyframe into the coordinate system corresponding to the first keyframe, thus obtaining the coordinate transformation keyframe.

[0096] (2) Overlay the coordinate transformation keyframe with the historical environment map corresponding to the previous processed keyframe to obtain the environment map.

[0097] The construction method in this embodiment uses the coordinate system of the first keyframe acquired by the lidar during operation as the world coordinate system (at this time, the coordinate system of the first keyframe is also the coordinate system through T1). 4 The initial coordinate system of the autonomous vehicle is determined by transformation. Then, the point cloud data of the first processed keyframe is used as map points, and the coordinates of these map points are all in the world coordinate system, thus constructing the environment map corresponding to the first keyframe. The second keyframe is then processed to obtain the pose transformation matrix between the second and first processed keyframes. The coordinates of each 3D point in the second processed keyframe are multiplied by this pose transformation matrix to transform the coordinates of each 3D point in the second processed keyframe to the world coordinate system, resulting in a coordinate-transformed keyframe. The 3D points of the coordinate-transformed keyframe are then superimposed onto the environment map corresponding to the first keyframe to obtain the environment map corresponding to the second keyframe. Then, the third keyframe is processed to obtain the pose transformation matrix between the processed third keyframe and the processed second keyframe. This matrix is ​​then multiplied by the pose transformation matrix between the processed second and first keyframes, and further multiplied by the coordinates of each 3D point in the processed third keyframe. This transforms the coordinates of each 3D point in the processed third keyframe to the world coordinate system, resulting in a coordinate transformation keyframe. The 3D points of this coordinate transformation keyframe are then superimposed onto the environment map corresponding to the second keyframe, yielding the environment map for the third keyframe. This process is repeated sequentially to incrementally construct the environment map for each keyframe. Using the pose transformation matrix, the pose transformation of the autonomous vehicle in 3D space can be obtained, allowing for the recovery of the vehicle's motion and the construction of a LiDAR odometry system. This, in turn, enables the construction of the environment map, thus completing the 3D environment map construction process.

[0098] S5: Overlay and fuse the terrain segmentation results with the environmental map to obtain a terrain semantic map.

[0099] Based on the above steps, this embodiment has obtained an environmental map and terrain segmentation results constructed in three-dimensional space. Therefore, by using the overlay and fusion method to fuse the environmental map and terrain segmentation results, a three-dimensional point cloud map with terrain semantic information is output, thus obtaining a terrain semantic map.

[0100] Preferably, in this embodiment, after obtaining the environmental map using S4 and before S5, loop closure detection and local map optimization can be further performed. This step is mainly to reduce the distortion in the pose transformation matrix calculation caused by some observations and environmental noise. Loop closure detection of the autonomous vehicle refers to the fusion of the previously constructed map when the autonomous vehicle returns to a previously visited position. At the same time, it is necessary to reduce the large mapping error generated after long-term operation. If the loop closure detection is successful, the continuous pose transformation matrix of the autonomous vehicle over a period of time is optimized to obtain a more accurate autonomous vehicle pose transformation matrix. Specifically, before overlaying and fusing the terrain segmentation results and the environmental map to obtain the terrain semantic map, the construction method of this embodiment also includes:

[0101] (1) Use the NDT algorithm to match the current processed keyframe with the environment map to obtain a matching score.

[0102] This embodiment performs probability-based matching between the currently processed keyframe and the already constructed environment map to complete the loop closure detection process. Specifically, the matching can be based on the NDT algorithm. The process includes: dividing the currently processed keyframe into three-dimensional meshes to obtain multiple three-dimensional meshes of the same size; calculating the normal distribution parameters (mean and covariance) of each three-dimensional mesh to construct a probability distribution model for each three-dimensional mesh; constructing a formula for calculating the matching score based on the probability distribution model of each three-dimensional mesh. This formula for calculating the matching score is the likelihood function used by the NDT algorithm. The LM method is used to solve the formula for calculating the matching score to obtain the matching score between the currently processed keyframe and the already constructed environment map.

[0103] (2) Determine whether the matching score is greater than the preset threshold; if yes, then use the graph optimization method to optimize the environment map to obtain the optimized map, and use the optimized map as the new environment map; if no, then do not optimize the map.

[0104] When the matching score is greater than the preset threshold, the matching is considered successful, i.e., a loop is detected. At this time, the pose transformation matrix is ​​corrected using graph optimization methods to obtain a more accurate pose transformation matrix, thereby obtaining more accurate autonomous vehicle trajectory information. At the same time, the global environment map is further optimized and corrected, i.e., the LiDAR odometry is reconstructed to reduce the drift of the autonomous vehicle's own motion. The corrected and optimized map is more accurate than the previous environment map, improving the accuracy of the environment map.

[0105] This embodiment performs graph optimization on the current processed keyframe and its N preceding keyframes after successful loop closure detection matching. The graph optimization method may include: (1) Constructing a factor graph: Select the current processed keyframe and its N preceding keyframes as N+1 nodes, and use the pose transformation matrix between each node and the first keyframe (obtained by multiplying the pose transformation matrices between all adjacent keyframes between the node and the first keyframe) as the node state, and use the relative pose transformation matrix between two adjacent nodes (for nodes without T1) as the relative pose transformation matrix between two adjacent nodes (for nodes without T1) as the relative pose transformation matrix between two adjacent nodes. 4 (1) The original three-dimensional environment point cloud data corresponding to the two adjacent nodes that are subjected to coordinate transformation can be obtained by feature extraction and feature matching as shown in S4) as the constraint factor between adjacent nodes; (2) Factor graph optimization: Based on the constraint factor between adjacent nodes, the factor graph is corrected using the GTSAM library to complete the optimization objective and obtain the optimized node state of N+1 nodes to optimize the pose transformation matrix. (3) Update map: Use the optimized pose transformation matrix to update the environment map information.

[0106] This embodiment allows setting different preset thresholds for loop closure detection for different tasks.

[0107] Considering the redundancy of point clouds resulting from fusing terrain segmentation results into the environmental map, this embodiment performs Euclidean clustering on the terrain semantic map. The specific method of Euclidean clustering is to set a clustering threshold t and a maximum number of point clouds N in a single cluster, obtaining multiple clusters based on spatial distance. For each cluster, the centroid of that cluster is calculated and used as the representative of that cluster, thus avoiding the problem of excessive point clouds in the terrain semantic map. This allows for the output of a 3D spatial map (i.e., a terrain semantic map) with terrain semantic information, which is beneficial for autonomous vehicle path planning and ontology control. Specifically, after obtaining the terrain semantic map, the construction method of this embodiment further includes: clustering the terrain semantic map using the Euclidean clustering algorithm to obtain multiple clusters; calculating the centroid of each cluster, which is the mean of the 3D coordinates of all points in each cluster; and retaining only the centroid of each cluster in the terrain semantic map to obtain a new terrain semantic map.

[0108] This embodiment proposes a method for constructing a terrain semantic map based on solid-state LiDAR in a field environment. Using 3D environmental point cloud data acquired by LiDAR as input, it automatically identifies dynamic objects and terrain in the scene and outputs a robust terrain semantic map with terrain information for unknown environments. Specifically, a joint clustering method based on Euclidean clustering and K-means clustering is used to remove noise and outliers from the acquired 3D environmental point cloud data. Stable environmental point cloud information can be obtained by clustering the 3D environmental point cloud data. The denoised 3D environmental point cloud data is used as input to a 3D object detection model. This model can detect the spatial location of dynamic objects (such as people and vehicles) and different terrain information (sand, grassland, etc.) in the environment. The 3D environmental point cloud data after removing dynamic objects is used as new input. A representative frame, or keyframe, is selected from the 3D environmental point cloud data over a period of time. Features are extracted from this keyframe, and feature matching is performed between the features of the preceding and following frames to calculate the corresponding pose transformation matrix. This pose transformation matrix can be used to reconstruct the pose transformation matrix of the unmanned vehicle, thus obtaining an environmental map of the unknown environment. The terrain semantic information output by the 3D object detection model is fused with this environmental map to output a final environmental map with terrain semantic information. This embodiment proposes a method for superimposing and fusing the terrain semantic information output by the neural network with the constructed environmental map. This solves the problem of low geometric texture in the wild causing the construction of unknown environment maps to fail. It also solves the problem of dynamic objects affecting the accuracy of mapping in the wild environment, resulting in a map lacking terrain semantic information. A robust map with terrain semantic information enables the unmanned vehicle to perform better path planning and body control.

[0109] Compared to existing technologies, the construction method of this embodiment has the following advantages:

[0110] (1) This embodiment can perform terrain segmentation and dynamic object recognition simultaneously based on the collected point cloud data. In the current semantic segmentation and 3D target detection neural networks, the two are treated as different tasks. If different tasks need to be performed on the point cloud data, different neural networks are required, which greatly increases the system's computing resources. This embodiment integrates the two tasks of segmentation and recognition, and a single convolutional neural network can simultaneously output the segmentation status and recognition results of the point cloud, saving the system's computing resources and speeding up the subsequent point cloud data processing.

[0111] (2) Redundant point cloud data and terrain semantics Figure 3A method for removing 3D point cloud information. This embodiment utilizes a joint clustering method based on Euclidean clustering and K-Means clustering to preprocess the initially acquired point cloud information to remove noise points and isolated points from the acquired data. Compared with traditional point cloud data processing methods, the point cloud segmentation accuracy and noise removal stability of this embodiment are significantly improved, making it applicable to complex, large-scale, and harsh outdoor scenarios. Simultaneously, in the terrain semantic map output, to avoid excessively large amounts of environmental point cloud data, Euclidean clustering is used to cluster the terrain map points in the 3D environment. Representative terrain map points from each cluster are selected using this clustering method. This method solves the problem of excessive initial terrain points leading to a large amount of terrain data in the constructed 3D map and can output a stable map.

[0112] (3) Construction of a 3D environment map based on the removal of 3D dynamic objects. If the collected point cloud is directly used to reconstruct the map of the unknown 3D environment, then dynamic objects in the 3D space (such as vehicles, people, etc.) will greatly affect the calculation of the pose transformation matrix of the autonomous vehicle body 1, and an inaccurate pose transformation matrix will lead to the failure of 3D map construction. In this embodiment, a convolutional neural network is used to remove dynamic objects. Feature extraction and feature matching of the image after removing dynamic objects can robustly obtain the pose transformation matrix of the autonomous vehicle, thereby realizing the reconstruction of the 3D environment.

[0113] (4) Fusion method of 3D reconstructed map and terrain map. The terrain segmentation results output by the convolutional neural network are superimposed and fused with the environment map to obtain the terrain point cloud information in 3D space. This method can output a 3D spatial map with terrain semantic information, overcoming the problem that the reconstructed environment map does not have terrain semantic information.

[0114] Current methods for autonomous vehicles to perceive unknown environments rely on cameras as their own sensors, collecting data in image form. This approach struggles to reliably extract environmental features when lighting conditions vary significantly. Furthermore, the effective range of depth images acquired by depth cameras is limited to within 10 meters, easily leading to unreliable depth information in harsh environments. Monocular cameras, on the other hand, often fail to triangulate in unfavorable terrain, causing the entire environment mapping algorithm to fail. Utilizing mechanical LiDAR as an onboard sensor for autonomous vehicles is one approach. LiDAR data directly reflects the depth information of objects within a 360° range. However, this method suffers from several drawbacks: mechanical LiDAR is expensive, has a short lifespan, and generates relatively high noise levels in the acquired data. Dynamic objects in the environment can also significantly impact the perception results, leading to map construction failures in unknown environments. To address the above shortcomings, this embodiment proposes a novel method for constructing terrain semantic maps based on solid-state LiDAR in field environments. By utilizing convolutional neural networks to perform semantic segmentation and 3D target detection on the collected point cloud information, terrain semantic information can be stably segmented and dynamic objects can be identified. This solves the problem of dynamic objects affecting the accuracy of map construction in field environments, achieving robust 3D map construction and outputting a 3D environmental map with terrain semantic information.

[0115] Example 2:

[0116] This embodiment provides a terrain semantic map construction system for a field environment, such as... Figure 8 As shown, the construction system includes:

[0117] The keyframe acquisition module M1 is used to acquire the current keyframe; the current keyframe is a frame of three-dimensional environmental point cloud data obtained by the lidar on the unmanned vehicle scanning the environment around the unmanned vehicle when the unmanned vehicle is moving in the field environment.

[0118] The target detection module M2 is used to input the current keyframe into the trained target detection model to obtain dynamic object recognition results and terrain segmentation results. The dynamic object recognition results include the position of the bounding box corresponding to each dynamic object in the current keyframe, and the dynamic objects include people and vehicles. The terrain segmentation results include terrain semantic information corresponding to three-dimensional points located on the ground in the current keyframe, and the terrain semantic information includes grassland, land, and concrete.

[0119] The dynamic object removal module M3 is used to remove the three-dimensional points in the bounding box corresponding to each dynamic object in the current key frame based on the dynamic object recognition result, so as to obtain the current processed key frame.

[0120] The map building module M4 is used to extract and match features from the current processed keyframe and the previous processed keyframe to obtain the pose transformation matrix between the current processed keyframe and the previous processed keyframe, and to build an environment map based on the pose transformation matrix.

[0121] The terrain information overlay module M5 is used to overlay and fuse the terrain segmentation results and the environmental map to obtain a terrain semantic map.

[0122] Each embodiment in this specification focuses on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be found in the method section.

[0123] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of the present invention. Furthermore, those skilled in the art will recognize that, based on the ideas of the present invention, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of the present invention.

Claims

1. A method for constructing a terrain semantic map in a field environment, characterized by, The construction method comprises: obtaining a current key frame; the current key frame is a frame of three-dimensional environment point cloud data obtained by a laser radar carried on the unmanned vehicle scanning the environment around the unmanned vehicle when the unmanned vehicle moves in a wild environment; inputting the current key frame into a trained target detection model to obtain a dynamic object recognition result and a terrain segmentation result; the dynamic object recognition result comprises the position of a bounding box corresponding to each dynamic object in the current key frame, and the dynamic object comprises a person and a vehicle; the terrain segmentation result comprises terrain semantic information corresponding to a three-dimensional point located on the ground in the current key frame, and the terrain semantic information comprises grassland, land and cement ground; based on the dynamic object recognition result, removing the three-dimensional points in the bounding box corresponding to each dynamic object in the current key frame to obtain a current processed key frame; performing feature extraction and feature matching on the current processed key frame and a previous processed key frame to obtain a pose transformation matrix between the current processed key frame and the previous processed key frame, and constructing an environment map based on the pose transformation matrix; superimposing and fusing the terrain segmentation result and the environment map to obtain a terrain semantic map; wherein the feature extraction and feature matching on the current processed key frame and the previous processed key frame to obtain the pose transformation matrix between the current processed key frame and the previous processed key frame specifically comprises: performing feature extraction on the current processed key frame to obtain features of the current processed key frame; the features of the current processed key frame comprise the type of each three-dimensional point in the current processed key frame, and the type comprises an angular feature point and a surface feature point; performing feature matching on the features of the current processed key frame and the features of the previous processed key frame to obtain a feature matching result; the feature matching result comprises a feature point pair that matches successfully, and the feature point pair comprises one three-dimensional point in the current processed key frame and one three-dimensional point in the previous processed key frame; calculating the pose transformation matrix between the current processed key frame and the previous processed key frame based on the feature matching result.

2. The construction method of claim 1, wherein, The current key frame is obtained specifically as follows: in the process of the unmanned vehicle moving in a wild environment, three-dimensional environment point cloud data obtained by a laser radar carried on the unmanned vehicle scanning the environment around the unmanned vehicle is obtained in real time; the laser radar is a solid-state laser radar; every N frames, a frame of the three-dimensional environment point cloud data is selected as the current key frame.

3. The construction method of claim 1, wherein, Before the current key frame is input into the trained target detection model, the construction method further comprises: preprocessing the current key frame, specifically comprising: performing denoising processing on the current key frame by using a joint clustering method based on Euclidean clustering and K-Means clustering to obtain a denoised key frame; The denoised key frame is subjected to coordinate transformation by using a pose transformation matrix between the unmanned vehicle and the laser radar obtained through pre-calibration, so as to convert the coordinates of each three-dimensional point in the denoised key frame into the coordinate system of the unmanned vehicle, and obtain a transformed key frame, and the transformed key frame is taken as a new current key frame.

4. The construction method of claim 1, wherein, The pose transformation matrix between the current processed key frame and the last processed key frame is calculated based on the feature matching result, specifically, the pose transformation matrix between the current processed key frame and the last processed key frame is calculated based on the feature matching result by using an ICP algorithm.

5. The construction method of claim 1, wherein, The environment map is constructed based on the pose transformation matrix, specifically: The coordinates of each three-dimensional point in the current processed key frame are converted into the coordinate system corresponding to the first frame key frame by using the pose transformation matrix, to obtain a coordinate conversion key frame; The coordinate conversion key frame is superimposed with the historical environment map corresponding to the last processed key frame, to obtain an environment map.

6. The construction method of claim 1, wherein, Before superimposing and fusing the terrain segmentation result and the environment map, the construction method further includes: The current processed key frame and the environment map are matched by using an NDT algorithm, to obtain a matching score; It is judged whether the matching score is greater than a preset threshold value; If yes, the environment map is optimized by using a graph optimization method, to obtain an optimized map, and the optimized map is taken as a new environment map.

7. The construction method of claim 1, wherein, After obtaining the terrain semantic map, the construction method further includes: The terrain semantic map is clustered by using a Euclidean clustering algorithm, to obtain a plurality of clusters; The center of gravity of each cluster is calculated; Only the center of gravity of each cluster is retained in the terrain semantic map, to obtain a new terrain semantic map.

8. A system for constructing a terrain semantic map in a field environment, characterized by, The construction system includes: A key frame acquisition module is configured to acquire a current key frame; the current key frame is a frame of three-dimensional environment point cloud data obtained by scanning the environment around an unmanned vehicle by a laser radar carried on the unmanned vehicle when the unmanned vehicle moves in a wild environment; A target detection module is configured to input the current key frame into a trained target detection model, to obtain a dynamic object recognition result and a terrain segmentation result; the dynamic object recognition result includes the position of a bounding box corresponding to each dynamic object in the current key frame, and the dynamic object includes a person and a vehicle; the terrain segmentation result includes terrain semantic information corresponding to a three-dimensional point located on the ground in the current key frame, and the terrain semantic information includes grassland, land and cement ground; A dynamic object removal module is configured to remove, based on the dynamic object recognition result, the three-dimensional points in the bounding box corresponding to each dynamic object in the current key frame, to obtain a current processed key frame; A map construction module is configured to perform feature extraction and feature matching on the current processed key frame and a last processed key frame, to obtain a pose transformation matrix between the current processed key frame and the last processed key frame, and construct an environment map based on the pose transformation matrix; A terrain information superimposition module is configured to superimpose and fuse the terrain segmentation result and the environment map, to obtain a terrain semantic map. The feature extraction and the feature matching on the current processed key frame and the previous processed key frame to obtain a pose transformation matrix between the current processed key frame and the previous processed key frame specifically include: The feature extraction on the current processed key frame to obtain the feature of the current processed key frame, wherein the feature of the current processed key frame includes the type of each three-dimensional point in the current processed key frame, and the type includes an angular feature point and a surface feature point; The feature matching of the feature of the current processed key frame with the feature of the previous processed key frame to obtain a feature matching result, wherein the feature matching result includes a matched feature point pair, and the feature point pair includes one three-dimensional point in the current processed key frame and one three-dimensional point in the previous processed key frame; The pose transformation matrix between the current processed key frame and the previous processed key frame is calculated based on the feature matching result.

Images