A method for high-altitude, large-scale realistic scene reconstruction based on laser and vision fusion

Abstract

This invention discloses a method for reconstructing large-scale realistic scenes at high altitudes based on laser and vision fusion, belonging to the field of 3D reconstruction technology. It solves the technical problems of easy failure and long reconstruction cycles in large-scale scene reconstruction. The key technical points are: utilizing the complementary characteristics of LiDAR and visible light cameras to reconstruct the scene in real time; using the visible light camera to acquire texture information and visual features of the real scene; using LiDAR to acquire spatial structure information of the scene; using adaptive attention downsampling to obtain stable backbone points; and using an adaptive iterative optimization method with feature constraints to estimate robust poses. This algorithm, while meeting real-time requirements, exhibits high accuracy and robustness, and has good application prospects in robotics and 3D reconstruction.

Description

Technical Field

[0001] This application relates to the field of 3D reconstruction technology, and in particular to a method for high-altitude, large-scale realistic scene reconstruction based on laser and vision fusion. Background Technology

[0002] Digital cities, as a pioneering practice in the construction of Digital China, have always been a focus of attention across the market. Large-scale scene 3D reconstruction is one of the key technologies in digital city construction, providing innovative solutions for urban development planning, smart transportation, emergency rescue, and other fields through highly realistic 3D reconstruction. Research on large-scale scene 3D reconstruction technology based on multi-source information tightly coupled SLAM (Simultaneous Localization and Mapping) is of great significance for guiding multi-scale and multi-level urban reconstruction and accelerating the construction of digital smart cities.

[0003] The origins of 3D reconstruction are closely related to vision. 3D reconstruction based on view geometry models is a technique that reconstructs 3D scenes using image or video data from multiple perspectives. It relies on the geometric relationships and features of images to match and analyze image or video data from different perspectives, thereby recovering the 3D structure of the scene. Its unique input format makes it particularly challenging; using only a single-view image as input results in the loss of much geometric information during reconstruction. While 3D terrain reconstruction of a region based on sequence images acquired by UAVs can quickly generate a 3D terrain model of the area, this model, being a discrete point cloud model, suffers from poor computational efficiency, high noise, sparseness, and uneven distribution, hindering subsequent terrain analysis and simulation.

[0004] With the continuous advancement of research in fields such as sensors, electronics, optics, and computer science, numerous new technologies have emerged, providing strong support for the acquisition and high-speed processing of large-scale, high temporal and spatial resolution data. LiDAR is a non-contact, active Earth observation technology with a high degree of automation, minimal weather influence, and high data accuracy. Due to the inherent characteristics of lidar, such as the sparsity of point clouds and the lack of texture information, multi-source data fusion is often used for 3D reconstruction. However, this approach faces the problem of inconsistencies between different data sources.

[0005] How to build a real-time, accurate, and robust 3D reconstruction system to quickly reconstruct large-scale real-world scenes is an urgent problem to be solved. Summary of the Invention

[0006] This application provides a method for high-altitude, large-scale real-world scene reconstruction based on laser and vision fusion. Its technical purpose is to improve the robustness and real-time performance of large-scale scene reconstruction, so as to be able to reconstruct high-quality 3D maps in a short period of time.

[0007] The above-mentioned technical objective of this application is achieved through the following technical solution:

[0008] A method for reconstructing large-scale realistic scenes at high altitudes based on laser and vision fusion includes:

[0009] Step S1: Acquire a visible light image using a visible light camera, perform data augmentation on the visible light image, extract the visual features of the augmented visible light image, and obtain a feature grayscale image.

[0010] Step S2: Acquire the original point cloud P using LiDAR. j Using a constant velocity model on the original point cloud P j Predicted pose T pre,t Calculations are performed based on the predicted pose T. pre,t For the original point cloud P j The distortion of the point cloud is removed to obtain the distortion-free point cloud.

[0011] Step S3: Project the feature grayscale image onto the point cloud Generate a colored point cloud containing visual features;

[0012] Step S4: Use adaptive attention downsampling to downsample the color point cloud. Perform the first downsampling on the color point cloud to obtain a local point cloud map, and perform the second downsampling on the color point cloud to obtain the backbone point cloud.

[0013] Step S5: Initialize the iterative optimization parameters based on the constant velocity model offset error and the spatial distribution characteristics of the point cloud; wherein, the iterative optimization parameters include the search distance and confidence weight; the spatial distribution characteristics of the point cloud include the spatial factor of the colored point cloud;

[0014] Step S6: Based on the search distance, search for the nearest neighbor of the backbone point in the backbone point cloud in the local point cloud map, and use the adaptive dual iterative optimization method to perform iterative optimization based on the backbone point and its nearest neighbor to estimate the robust pose of the current frame.

[0015] Step S7: Project the data-enhanced visible light image onto the distortion-corrected point cloud. Generate a radial point cloud, and construct a 3D map of the real scene based on the radial point cloud and continuous robust poses.

[0016] The beneficial effects of this application are as follows: The high-altitude large-scale real scene reconstruction method based on laser and vision fusion described in this application reconstructs the scene in real time by utilizing the complementary characteristics of LiDAR and visible light camera; it uses visible light camera to obtain texture information and visual features of the real scene; it uses LiDAR to obtain spatial structure information of the scene; it uses adaptive attention downsampling to obtain stable backbone points; and it uses an adaptive dual iterative optimization method with feature constraints to estimate robust pose.

[0017] This method, while meeting real-time requirements, can obtain robust pose and high-quality real-scene reconstruction maps based on spatial information from LiDAR and texture information from visible light cameras, showing promising application prospects in robotics and 3D reconstruction. Experiments demonstrate that the system proposed in this application has low computational cost, strong robustness, and can reconstruct high-quality 3D maps. Attached Figure Description

[0018] Figure 1 This is a flowchart of the high-altitude large-scale real scene reconstruction method based on laser and vision fusion described in the embodiments of this application;

[0019] Figure 2 This is a flowchart of the adaptive dual-iterational optimization method in the embodiments of this application. Detailed Implementation

[0020] The technical solution of this application will be described in detail below with reference to the accompanying drawings.

[0021] like Figure 1 As shown, the high-altitude large-scale realistic scene reconstruction method based on laser and vision fusion described in this application includes:

[0022] Step S1: Acquire a visible light image using a visible light camera, perform data augmentation on the visible light image, extract the visual features of the augmented visible light image, and obtain a feature grayscale image.

[0023] Specifically, data augmentation includes exposure correction and texture enhancement. In large-scale scene reconstruction tasks, images have different exposure levels as the scene and time change. Exposure correction can make sequential images have similar brightness and contrast, making feature extraction easier. Enhancing image consistency improves the quality of the reconstructed map. This patent uses histogram equalization for exposure repair.

[0024] Texture enhancement can make images clearer and more realistic, improve the quality and accuracy of scene reconstruction, and make it easier to extract visual features later. This patent uses a filtering-based method to enhance the texture of images, highlighting high-frequency texture information in the images while suppressing noise, thus achieving the effect of texture enhancement.

[0025] To be applicable to more scenarios, a combination of line and point features is used. First, the LSD algorithm is applied to extract line features from the augmented visible light image, represented by RGB (255, 0, 0) pixel values. Then, the ORB algorithm is applied to extract point features, represented by RGB (0, 0, 255) pixel values. Directly extracting line features from the augmented visible light image would result in many messy line segments. Therefore, the visible light image is first converted to grayscale. Then, based on the grayscale values, a local adaptive image segmentation algorithm is used for image segmentation. Finally, the LSD algorithm is used to extract line features from the segmented image. The local adaptive image segmentation algorithm is represented as follows:

[0026]

[0027] Where Src(x,y) represents the pixel value at position (x,y) in the grayscale image, seg(x,y) represents the pixel value at position (x,y) in the segmented image, blockMean(x,y) represents the average grayscale value of the pixels surrounding pixel (x,y), and c represents the segmentation threshold.

[0028] Step S2: Acquire the original point cloud P using LiDAR. j Using a constant velocity model on the original point cloud P j Predicted pose T pre,t Calculations are performed based on the predicted pose T. pre,t For the original point cloud P j The distortion of the point cloud is removed to obtain the distortion-free point cloud.

[0029] The constant velocity model assumes that the object's velocity remains constant within infinitesimally small time intervals during its motion. Based on this characteristic, the pose of the current frame can be predicted, thus enabling distortion correction of the point cloud. Therefore, step S2 specifically includes:

[0030] Step S21: Based on the pose T of the previous two frames pre，t-1 =(R t-1 , t t-1 ) and T pre，t-2 =(R t-2 , t t-2 And the time interval Δt, the translational velocity v at time t-1 t-1 and angular velocity Calculations are performed; then, based on the constant velocity model, the translational velocity and angular velocity of the current frame are obtained, expressed as: v t =v t-1 ,

[0031] Step S22: Based on the translational velocity and angular velocity of the current frame, adjust the original point cloud P... jThe predicted pose of each point at the corresponding time is calculated and represented as:

[0032]

[0033] Step S23: Calculate the translational velocity and angular velocity of the current frame based on the predicted pose, as follows:

[0034]

[0035]

[0036] Step S24: Since a point cloud frame is obtained within time Δt, in order to remove motion distortion, all points in the point cloud need to be projected onto the coordinate system of the current time. For the original point cloud P... j A point p i ∈P j , and p i The relative time is t i If ∈[0, Δt], then the distortion-free points are obtained based on the translational velocity and angular velocity of the current frame. Represented as:

[0037]

[0038] Among them, t i Represents the original point cloud P j Each point p in i The corresponding relative capture time.

[0039] Step S3: Project the feature grayscale image onto the point cloud Generate a colored point cloud containing visual features.

[0040] Specifically, the feature grayscale image is projected onto the point cloud. The projection formula on is expressed as:

[0041]

[0042] Where (u,v) represents the position of a point on the feature grayscale image, (x w ,y w ,z w ) represents the position of a point in the corresponding point cloud, f u ,f v u0 and v0 represent the intrinsic parameters of the visible light camera, R represents the rotation matrix of the lidar and camera extrinsic parameters, and t represents the translation matrix of the lidar and camera extrinsic parameters.

[0043] During the projection process, the colors of the line features and point features in the feature grayscale image are directly assigned to the corresponding points in the point cloud, and the point color is RGB(0,255,0).

[0044] Step S4: Use adaptive attention downsampling to downsample the color point cloud. Perform the first downsampling on the color point cloud to obtain a local point cloud map, and perform the second downsampling on the color point cloud to obtain the backbone point cloud.

[0045] For ICP-based lidar odometry, the number of backbone points determines the matching accuracy and speed. The expected number of backbone points N... exp As an optimization objective, the acquisition of voxel size for adaptive attention downsampling specifically includes the following steps:

[0046] Step S41: Spatial factor of the colored point cloud The calculations include: using the voxel size v of the local point cloud map. map The same voxel grid is used to divide the space of the color point cloud, and the initial distribution volume of the color point cloud is obtained by accumulating the volumes of all occupied voxels. Based on the initial distribution volume The average number of points ρ per voxel was calculated. j Then, based on the average number of points ρ per voxel... j Get the average occupied size of each point Represented as:

[0047]

[0048] Step S42: Use voxel dimensions of A secondary correction and partitioning process is performed on the colored point cloud. If the number of points in the 27 voxels surrounding the current voxel exceeds 9, then the current voxel is considered occupied. This removes noise and discrete points from the point cloud. The occupied volume V of the colored point cloud is obtained by accumulating the volumes of all occupied voxels. j .

[0049] Step S43: During the downsampling process, only one point is retained within each voxel, and the desired number of backbone points N is determined. exp Adaptive downsampling voxel size v for the current frame j The calculation is performed and expressed as follows:

[0050]

[0051] Step S44: Based on the adaptive downsampling voxel size v of the current frame j The voxel size obtained from the first downsampling is αv. j The voxel size for the second downsampling is v j Where α∈[0,1], v j This indicates the adaptive downsampling voxel size of the current frame.

[0052] Specifically, to make point cloud matching more accurate, a frame-map search method is used, with the first downsampling size being αv. j (α∈[0,1]), the obtained sampling points are used to store the map. The size of the second downsampling is v. j This is used to extract backbone points. To improve the discriminability of backbone points and overcome the point cloud degradation problem, attention downsampling was designed. This method considers points with visual point and line features to deserve more attention; therefore, the voxel size of the downsampled feature points is half that of ordinary points, in order to obtain more feature points. The point cloud composed of visual feature points and ordinary points is called backbone points because it better reflects the structural features of the point cloud space.

[0053] Additionally, the local point cloud map M local via voxel blocks Store point clouds, each voxel block It includes 5 parameters, represented as follows:

[0054]

[0055] Where j represents the voxel block number; v map The voxel block size representing the local point cloud map is v. map ×v map ×v map ; R represents the maximum number of storage points allowed within each voxel block, where F represents the feature label of the voxel block; max This indicates the storage range of the local point cloud map.

[0056] For ease of management, visual line feature points and ordinary points are managed separately using labels F. Furthermore, due to the instability of visual features, only features from the next 30 frames are retained, while ordinary points are managed using a storage range R. max Control is implemented. Ideally, the density of the local point cloud map is higher than the density of the backbone points. Therefore, the maximum storage quantity of voxel blocks is dynamically adjusted based on the current spatial characteristics of the point cloud. Represented as: This map management strategy enables the system to operate stably and efficiently in environments with varying scene scales.

[0057] Step S5: Initialize the iterative optimization parameters based on the constant velocity model offset error and the spatial distribution characteristics of the point cloud; wherein, the iterative optimization parameters include the search distance and confidence weight; the spatial distribution characteristics of the point cloud include the spatial factor of the color point cloud, specifically the average number of points ρ per voxel. j And the occupied volume V of the colored point cloud j .

[0058] In the point-to-point matching process, the search distance τj The confidence weight ρ(e) affects the speed and registration accuracy of the iteration process. For the search distance, the larger the error of the constant-velocity model, the less accurate the initial pose, thus requiring a larger search range. Simultaneously, the larger the scene scale, the sparser the point cloud, and the greater the interval between points, thus requiring a larger search range. Therefore, step S5 includes:

[0059] Step S51: To ensure a uniform measurement of error, an adaptive downsampling voxel size v is added. j The average offset error of each frame yields the constant velocity model offset error σ. j , represented as:

[0060]

[0061] Where N represents the number of frames; δ(ΔT) represents the offset of the constant velocity model, and δ(ΔT)=δ(ΔR)+δ(Δt), where δ(ΔR) represents the rotation offset distance and δ(Δt) represents the translation offset distance.

[0062]

[0063] Step S52: Based on the constant velocity model offset error σ j For the search distance τ j The calculation is performed and expressed as follows:

[0064] τ j =σ j v j ;

[0065] Step S53: Based on the constant velocity model offset error σ j The confidence weight ρ(e) is obtained and expressed as:

[0066]

[0067] Where e represents the point-to-point residual; κ j The scale parameter is represented as:

[0068]

[0069] Where, αv j Indicates the voxel size of the first downsampling, v map ρ represents the voxel size of a local point cloud map. j This represents the average number of points distributed per voxel.

[0070] Step S6: Search for the nearest neighbor of the backbone point in the local point cloud map based on the search distance. Use an adaptive dual-iteration optimization method to perform iterative optimization based on the backbone point and its nearest neighbor to estimate the robust pose of the current frame.

[0071] like Figure 2 As shown, for the backbone points Visual feature points can be categorized based on color. and ordinary To achieve more robust and flexible iterative optimization, an adaptive dual iterative optimization method (AD-ICP) is proposed. First, based on feature points... Initial pose estimation, then using ordinary points Accurate pose estimation, this two-step estimation can greatly reduce the degradation characteristics of point clouds, making the estimated pose more robust.

[0072] Then step S6 includes:

[0073] Step S61: Extract the backbone points from the backbone point cloud. Visual feature points are categorized based on color. and ordinary

[0074] Step S62: Based on the predicted pose T pre,t pose T of the previous frame pre,t-1 Visual feature points The transformation to the global coordinate system is represented as follows:

[0075] Step S63: Based on the search distance τ j Local point cloud map The algorithm searches for the nearest neighbor of a visual feature point, calculates the confidence weight of the visual feature point, calculates the residual between the visual feature point and its nearest neighbor based on the confidence weight, and calculates the pose change threshold based on the residual.

[0076] Step S64: Repeat step S63 until the minimum pose change threshold is obtained, thus achieving convergence and obtaining the corrected pose. According to the corrected pose The visually estimated pose is represented as:

[0077] Step S65: Based on the predicted pose T pre,t pose T of the previous frame pre,t-1 Make it ordinary The transformation to the global coordinate system is represented as follows:

[0078] Step S66: Based on the search distance τ jLocal point cloud map The algorithm searches for the nearest neighbor of a common point, calculates the confidence weight of the common point, calculates the residual between the common point and its nearest neighbor based on the confidence weight, and calculates the pose change threshold based on the residual.

[0079] Step S67: Repeat step S66 until the minimum pose change threshold is obtained, thus achieving convergence and obtaining the corrected pose. According to the corrected pose The final estimated pose Δ is obtained Ticp,j ,

[0080] Step S68: Obtain the final estimated pose based on the visually estimated pose and the normal estimated pose, represented as:

[0081] Step S69: Obtain the robust pose of the current frame based on the final estimated pose, represented as:

[0082] T j =ΔT V.icp,j T pre,t T pre,t-1 .

[0083] Step S7: Project the data-enhanced visible light image onto the distortion-corrected point cloud P j * Generate a radial point cloud, and construct a 3D map of the real scene based on the radial point cloud and continuous robust poses.

[0084] The above are exemplary embodiments of this application, and the scope of protection of this application is defined by the claims and their equivalents.

Claims

1. A method for reconstructing large-scale realistic scenes at high altitudes based on laser and vision fusion, characterized in that, include: Step S1: Acquire a visible light image using a visible light camera, perform data augmentation on the visible light image, extract the visual features of the augmented visible light image, and obtain a feature grayscale image. Step S2: Acquire the original point cloud P using LiDAR. j Using a constant velocity model on the original point cloud P j Predicted pose T pre,t Calculations are performed based on the predicted pose T. pre,t For the original point cloud P j The distortion of the point cloud is removed to obtain the distortion-free point cloud. Step S3: Project the feature grayscale image onto the point cloud Generate a colored point cloud containing visual features; Step S4: Use adaptive attention downsampling to downsample the color point cloud. Perform the first downsampling on the color point cloud to obtain a local point cloud map, and perform the second downsampling on the color point cloud to obtain the backbone point cloud. Step S5: Initialize the iterative optimization parameters based on the constant velocity model offset error and the spatial distribution characteristics of the point cloud; wherein, the iterative optimization parameters include the search distance and confidence weight; the spatial distribution characteristics of the point cloud include the spatial factor of the colored point cloud; Step S6: Based on the search distance, search for the nearest neighbor of the backbone point in the backbone point cloud in the local point cloud map, and use the adaptive dual iterative optimization method to perform iterative optimization based on the backbone point and its nearest neighbor to estimate the robust pose of the current frame. Step S7: Project the data-enhanced visible light image onto the distortion-corrected point cloud. Generate a radial point cloud, and construct a 3D map of the real scene based on the radial point cloud and continuous robust poses.

2. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 1, characterized in that, In step S1, the data enhancement of the visible light image includes exposure correction and texture enhancement.

3. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 2, characterized in that, In step S1, visual features of the data-enhanced visible light image are extracted, including point features and line features. Point feature extraction includes: extracting point features from the data-enhanced visible light image using the ORB algorithm; Line feature extraction includes: converting the data-enhanced visible light image into a grayscale image, then performing image segmentation using a local adaptive image segmentation algorithm based on the grayscale values ​​of the grayscale image, and finally extracting line features from the segmented image using the LSD algorithm; wherein, the local adaptive image segmentation algorithm is expressed as: Where src(x,y) represents the pixel value at position (x,y) in the grayscale image, seg(x,y) represents the pixel value at position (x,y) in the segmented image, blockMean(x,y) represents the average grayscale value of the pixels surrounding pixel (x,y), and c represents the segmentation threshold.

4. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 3, characterized in that, Step S2 includes: Step S21: Based on the pose T of the previous two frames pre,t-1 =(R t-1 ,t t-1 ) and T pre,t-2 =(R t-2 ,t t-2 And the time interval Δt, the translational velocity v at time t-1 t-1 and angular velocity Calculations are performed; then, based on the constant velocity model, the translational velocity and angular velocity of the current frame are obtained, expressed as: v t =v t-1 , Step S22: Based on the translational velocity and angular velocity of the current frame, adjust the original point cloud P... j The predicted pose of each point at the corresponding time is calculated and represented as: Step S23: Calculate the translational velocity and angular velocity of the current frame based on the predicted pose, as follows: Step S24: For the original point cloud P j A point p i ∈P j , and p i The relative time is t i If ∈[0,Δt], then the distortion-free points are obtained based on the translational velocity and angular velocity of the current frame. Represented as: Among them, t i Represents the original point cloud P j Each point p in i The corresponding relative capture time.

5. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 4, characterized in that, In step S3, the feature grayscale image is projected onto the point cloud. The projection formula on is expressed as: Where (u,v) represents the position of a point on the feature grayscale image, (x w ,y w ,z w ) represents the position of a point in the corresponding point cloud, f u ,f v u0 and v0 represent the intrinsic parameters of the visible light camera, R represents the rotation matrix of the lidar and camera extrinsic parameters, and t represents the translation matrix of the lidar and camera extrinsic parameters.

6. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 5, characterized in that, In step S4, the process of obtaining the voxel size for adaptive attention downsampling includes: Step S41: Spatial factor of the colored point cloud The calculations include: using the voxel size v of the local point cloud map. map The same voxel grid is used to divide the space of the color point cloud, and the initial distribution volume of the color point cloud is obtained by accumulating the volumes of all occupied voxels. Based on the initial distribution volume The average number of points ρ per voxel was calculated. j Then, based on the average number of points ρ per voxel... j Get the average occupied size of each point Represented as: Step S42: Use voxel dimensions of The colored point cloud is further refined and divided. If the number of points in the 27 voxels surrounding the current voxel exceeds 9, then the current voxel is considered occupied. The occupied volume V of the colored point cloud is obtained by accumulating the volumes of all occupied voxels. j ; Step S43: During the downsampling process, only one point is retained within each voxel, and the desired number of backbone points N is determined. exp Adaptive downsampling voxel size v for the current frame j The calculation is performed and expressed as follows: Where, N exp Indicates the number of key points; Step S44: Based on the adaptive downsampling voxel size v of the current frame j The voxel size obtained from the first downsampling is αv. j The voxel size for the second downsampling is v j Where α∈[0,1], v j This indicates the adaptive downsampling voxel size of the current frame.

7. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 6, characterized in that, In step S4, the local point cloud map M local via voxel blocks Store point clouds, each voxel block It includes 5 parameters, represented as follows: Where j represents the voxel block number; v map The voxel block size representing the local point cloud map is v. map ×v map ×v map ; This indicates the maximum number of storage points allowed within each voxel block, and F represents the feature label of the voxel block; R max This indicates the storage range of the local point cloud map.

8. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 7, characterized in that, Step S5 includes: Step S51: Adjust the constant velocity model offset error σ j The calculation is performed and expressed as follows: Where N represents the number of frames; δ(ΔT) represents the offset of the constant velocity model, and δ(ΔT)=δ(ΔR)+δ(Δt), where δ(ΔR) represents the rotation offset distance and δ(Δt) represents the translation offset distance; Step S52: Based on the constant velocity model offset error σ j For the search distance τ j The calculation is performed and expressed as follows: t j =s j v j ; Step S53: Based on the constant velocity model offset error σ j The confidence weight ρ(e) is obtained and expressed as: Where e represents the point-to-point residual; κ j The scale parameter is represented as: Where, αv j Indicates the voxel size of the first downsampling, v map ρ represents the voxel size of a local point cloud map. j This represents the average number of points distributed per voxel.

9. The method for reconstructing large-scale realistic scenes at high altitudes as described in claim 8, characterized in that, Step S6 includes: Step S61: Extract the backbone points from the backbone point cloud. Visual feature points are categorized based on color. and ordinary Step S62: Based on the predicted pose T pre,t pose T of the previous frame pre,t-1 Visual feature points The transformation to the global coordinate system is represented as follows: Step S63: Based on the search distance τ j Local point cloud map The algorithm searches for the nearest neighbor of a visual feature point, calculates the confidence weight of the visual feature point, calculates the residual between the visual feature point and its nearest neighbor based on the confidence weight, and calculates the pose change threshold based on the residual. Step S64: Repeat step S63 until the minimum pose change threshold is obtained, thus achieving convergence and obtaining the corrected pose. According to the corrected pose Obtain visually estimated pose ΔT V.j , is represented as: Step S65: Based on the predicted pose T pre,t pose T of the previous frame pre,t-1 Make it ordinary The transformation to the global coordinate system is represented as follows: Step S66: Based on the search distance τ j Local point cloud map The algorithm searches for the nearest neighbor of a common point, calculates the confidence weight of the common point, calculates the residual between the common point and its nearest neighbor based on the confidence weight, and calculates the pose change threshold based on the residual. Step S67: Repeat step S66 until the minimum pose change threshold is obtained, thus achieving convergence and obtaining the corrected pose. According to the corrected pose Obtain the ordinary estimated pose ΔT icp,j , is represented as: Step S68: Obtain the final estimated pose based on the visually estimated pose and the normal estimated pose, represented as: Step S69: Obtain the robust pose of the current frame based on the final estimated pose, represented as: T j =ΔT V.icp,j T pre,t T pre,t-1 。

Images