Simultaneous Localization and Mapping Method for Complex Underwater Visual Conditions

Abstract

The present invention discloses a simultaneous localization and mapping method for complex underwater visual conditions. The method of the present invention first obtains image data by an underwater robot; determines whether there are light spots in the image, and if there are light spots, processes the light spot area; then determines whether there are shadows in the image, and if there are, first distinguishes the shadow and non-shadow areas, then optimizes the segmentation effect of the shadow area, and provides a compensation image for the shadow area according to the images of adjacent frames; extracts and matches feature points from the image frames after processing the light spot and shadow areas to obtain a local environmental map; uses M-estimation to process outliers or noise to enhance the robustness of the system, and further applies global optimization to optimize the entire system to obtain a globally consistent underwater robot trajectory and environmental map. The present invention effectively improves the performance of backend optimization, makes the entire system have good robustness, and can ensure that the underwater robot efficiently completes the tasks of simultaneous localization and mapping.

Description

Technical Field

[0001] The present invention relates to a method for simultaneous localization and mapping, and particularly to a method for simultaneous localization and mapping for complex underwater visual conditions. Background Art

[0002] Effective underwater simultaneous localization and mapping methods can significantly improve human understanding of the marine ecosystem, and promote scientific research and resource management such as marine biological observation, sunken ship detection, and marine mineral exploration. When an underwater robot is engaged in underwater exploration and rescue tasks, an accurate simultaneous localization and mapping method can improve its navigation performance and enhance data reliability. However, due to the refraction and scattering of light in the underwater environment, and the changes in water flow and light will affect the accuracy of environmental perception, it is challenging to develop an accurate underwater simultaneous localization and mapping method.

[0003] Currently, the visual Simultaneous Localization and Mapping (SLAM) algorithm is a relatively good solution for simultaneous localization and mapping, and many kinds have been developed, such as algorithms based on feature point methods like ORB-SLAM3. The SLAM algorithm based on the feature point method can work when the image noise is large and the camera moves fast. However, it can only be applied to underwater environments with relatively good water quality and lighting conditions. In actual waters, there are inevitably some complex visual conditions, such as the situation where the bottom of the water is full of light spots due to strong light, and at this time, the underwater robot may block the underwater objects photographed by the camera during operation, resulting in shadows in the visual image. These will all lead to the failure of feature point extraction in the front-end visual odometry process of the SLAM algorithm based on the feature point method, and ultimately lose some camera pose information and local environmental maps. Summary of the Invention

[0004] Object of the Invention: In complex underwater visual conditions, the front end of the visual SLAM algorithm based on the feature point method may not be able to effectively extract feature points, resulting in the failure of visual odometry and the reduction of algorithm performance and reliability. To solve the above problems, the present invention proposes a method for simultaneous localization and mapping for complex underwater visual conditions.

[0005] Technical Solution:

[0006] The present invention provides a method for simultaneous localization and mapping for complex underwater visual conditions, and the method includes the following steps:

[0007] S1. An underwater robot carries a camera to take images and obtain image data;

[0008] S2. Run the front - end visual odometry process to handle the problems of light spots and shadows in the image data obtained in step S1, and obtain the camera pose information and the local environment map;

[0009] S3. Perform back - end optimization to handle outliers or noise, enhance the robustness of the system, and obtain a globally consistent underwater robot trajectory and environment map.

[0010] Further, in step S1, after the underwater robot uses the equipped camera to take underwater images and obtain image data, it starts underwater simultaneous localization and mapping.

[0011] Further, in step S2, after obtaining the image data of step S1, first handle the problem of light spots or shadows that may exist in the image due to complex visual conditions, such as strong light or the underwater robot blocking the bottom objects. The existence of light spots or shadows will cause the front - end of the SLAM algorithm to fail to extract feature points, and finally part of the camera pose information will be lost. Then, after processing the image data set, start feature point extraction and matching. Finally, the camera pose information and the local environment map can be obtained through the front - end visual odometry.

[0012] Further, the method for handling the light spot problem in the image specifically includes: First, determine whether there may be light spots in the image: convert the image to a grayscale image, use the Gaussian blur algorithm to denoise the image, and then determine whether there are light spots according to the gray - scale threshold for the processed image. The judgment criterion is shown in formula (1):

[0013] (1),

[0014] where, represents the gray - scale value of any pixel point in the image, T represents the set gray - scale threshold. Detect each pixel point in the image in turn. If the situation where has a value of 1 appears, it means that there may be light spots; then, model the brightness change of the image through an affine shadow formation model and estimate the light - spot area: If there are light spots in the image, it is necessary to consider the influence of light changes on the image brightness and confirm which areas of the brightness change are caused by light spots rather than other factors (such as surface reflection or ambient light changes). Assume that in the case of no light - spot influence, the brightness change of the image is predicted through an affine shadow formation model based on reflectivity and ambient light, as shown in formula (2):

[0015] (2),

[0016] where, is the predicted value of the image brightness without light - spot influence, is the brightness value in the original image, is the reflectivity coefficient, is the offset, representing the influence of ambient light. Calculate the actual brightness value of the image and the predicted brightness value The difference between them is shown in Equation (3):

[0017] (3),

[0018] Estimate the spot area according to the brightness difference Set a threshold O. If exceeds O, then determine that this area is the spot area and proceed to the next step; finally, use an image filtering model based on saliency and gradient information to process the spot area: First, measure the saliency by calculating the local contrast of the neighborhood around each pixel in the spot area. The calculation of the local contrast is shown in Equation (4):

[0019] (4),

[0020] where, is the brightness value of the pixel point in the spot area, is the local brightness mean of the area around the pixel point and is the local standard deviation of the area around the pixel point Then calculate the gradients of the spot area image in the horizontal and vertical directions through the Sobel operator to obtain the gradient magnitude of the image, as shown in Equation (5):

[0021] (5),

[0022] Combine the saliency and the gradient information to obtain a comprehensive weight to guide the filtering, The calculation is shown in Equation (6):

[0023] (6),

[0024] where, and are the weighting coefficients; According to the calculated comprehensive weight, perform adaptive bilateral filtering to optimize the spot area. The specific formula for adaptive bilateral filtering is shown in Equation (7):

[0025] (7),

[0026] where, and is the Gaussian kernel function in the spatial domain and pixel value domain, controlling the influence of spatial distance and pixel value difference, is the brightness value after filtering the spot area, represents a pixel the positions of other pixels within the

[0027] Furthermore, the method for processing the problem of shadows in the image specifically includes: First, determine whether there are shadows in the image: Calculate the grayscale histogram of the image , where is the grayscale value, records the number of pixels at each grayscale value Set a threshold for the low grayscale value to distinguish the low grayscale area from other areas, and calculate the pixel ratio C of the low grayscale area in the image, as shown in Equation (8):

[0028] C (8),

[0029] where is the number of pixels whose grayscale value is in the range of , is the total number of pixels in the image. If C exceeds the threshold , it indicates that there is a shadow area; Then, initially distinguish the shadow and non-shadow areas through the likelihood ratio test method based on region growing: Assume that the probability distributions of the shadow area and non-shadow area are and , representing the probabilities of the image pixel under the shadow area and non-shadow area respectively, where is the shadow hypothesis, is the non-shadow hypothesis, and the likelihood ratio is defined as shown in Equation (9):

[0030] (9),

[0031] Compare the likelihood ratio with the set threshold to determine whether a pixel belongs to the shadow area. If the likelihood ratio of a pixel exceeds the threshold, it belongs to the shadow area; otherwise, it belongs to the non-shadow area. Region growing starts from the seed pixel and gradually expands to adjacent pixels. First, select the seed pixel area initially classified as a shadow, and then expand the area according to whether the likelihood ratio of the pixel meets the threshold condition. For each newly added pixel , calculate its likelihood ratio , and determine whether it belongs to the shadow area according to the threshold; then, further optimize the segmentation effect of the shadow area through the Markov random field method: regard the pixels in the image as random variables, and there is a dependency relationship between each pixel and its adjacent pixels. Given the label of the pixel (0 means non-shadow, 1 means shadow), the Markov energy function is defined as shown in Equation (10):

[0032] (10),

[0033] where the data term represents the classification cost of the pixel , and the calculation formula is as shown in Equation (11):

[0034] (11),

[0035] The smooth term represents the cost of label consistency between adjacent pixels. By defining a penalty term when the labels of adjacent pixels are inconsistent, adjacent pixels are encouraged to have the same label, and the smooth term is defined using Equation (12):

[0036] (12),

[0037] where is the penalty factor of the smooth term, which controls the influence degree of the smooth term on the total energy, ) is an indicator function. If , its value is 1, otherwise its value is 0. By minimizing the energy function , the optimal segmentation of the shadow area, that is, the optimal label configuration of each pixel, is obtained, which contains the information of whether each pixel belongs to the shadow area; finally, the compensated image of the shadow area is provided according to the images of adjacent frames: when the shadow area is identified and segmented, the shadow area is compensated through the image information of adjacent frames. Assume that the compensation information is obtained from the non-shadow area of the adjacent frame, such as the previous frame. For the shadow pixel in the current frame, its compensation value can be obtained by the weighted average method, as shown in Equation (13):

[0038] (13),

[0039] where is the pixel value after compensation, is the non-shadow pixel value corresponding to in the previous frame, is the shadow pixel value in the current frame, is a weight factor that controls the degree of compensation for the shadow area.

[0040] Further, the method for feature point extraction and matching is specifically as follows: For the image after processing the light spot and shadow problems, the Scale-Invariant Feature Transform (SIFT) algorithm is used to detect the feature points of each frame of the image and generate descriptors for each feature point, and then the matching algorithm based on the K-dimensional tree is used for feature point matching.

[0041] Further, obtaining the camera pose information and the local environmental map specifically includes the following steps: First, select key frames: According to the change in the number of matched image feature points between adjacent frames, select the frames with significant changes as key frames; then, use the Perspective-n-Point (PnP) algorithm combined with the pose optimization method based on Weighted Random Sample Consensus (RANSAC) to calculate the camera pose information: According to the matched feature points in the key frames, use the PnP algorithm to estimate the preliminary pose of the camera , calculate the reprojection error for each matched point , The calculation is as shown in Equation (14):

[0042] (14),

[0043] where, is the position of the th matched point in the two-dimensional image, is the position of the three-dimensional point corresponding to the th matched point in the world coordinate system, is the projection function that projects the three-dimensional point onto the two-dimensional image plane. Each matched point is assigned a weight factor according to the matching quality of the feature descriptor, which is used to measure the reliability of each matched point. The calculation is as shown in Equation (15):

[0044] (15),

[0045] where, represents the distance between the matched point and its descriptor. Calculate the weighted error for each matched point, and judge the inliers and outliers according to the weighted error. Set a threshold D. If is less than the threshold, it is considered that the Matching points are inliers, otherwise they are outliers. Each iteration calculates the inliers set based on the current pose and re-evaluates the inliers and outliers based on the weighted error. Through multiple iterations, the camera pose is gradually updated until the optimal pose is obtained. Finally, the local map is updated: the obtained pose information and matching feature points are updated to the local map of the environment to maintain the consistency and accuracy of the local map.

[0046] Furthermore, in step S3, back-end optimization is used to further process the camera pose information and the local map to reduce the overall error and improve the algorithm accuracy. The specific method steps include: first, using M estimation to process outliers or noise to enhance the robustness of the system; then using global optimization to optimize the entire system to obtain a globally consistent robot trajectory and environment map.

[0047] Furthermore, the M estimation method specifically comprises the following steps: first, select the first Matching points is the observed data, the initial pose , The position of the corresponding 3D point as an initial estimate; then, use the reprojection error As the observation error, ; Finally, select the Huber loss method in M ​​estimation for model estimation and design the Huber loss function for each observation As shown in formula (16):

[0048] (16)

[0049] in, is a set tolerance threshold. Set the loss threshold to , if the loss function of an observation is Exceed , then the observation is marked as an outlier and removed.

[0050] Furthermore, the global optimization is used to optimize the entire system, which specifically includes the following steps: first, construct an optimization problem and set the optimization variable as the posture ,Location , the objective function is , in is the designed Huber loss function; then, the nonlinear least squares optimization algorithm is selected for global optimization, and the updated variables are shown in formula (17):

[0051] (17)

[0052] in, , is the optimization variable, is the Jacobian matrix, is the residual vector, which is equal to the reprojection error is equal; is the adjustable adjustment factor; then, continuously adjust the pose and the position of the map points, that is, keep repeating the update of the optimization variable until the change of the objective function is less than the preset threshold N; finally, obtain the optimized variable, and draw the trajectory of the underwater robot and the global map.

[0053] The beneficial effects of the present invention compared with the prior art are:

[0054] The present invention provides a method for simultaneous localization and mapping for complex underwater visual conditions. When an underwater robot performs underwater operations, it needs to perform simultaneous localization and mapping, but the following situations may occur: due to strong light, there are a large number of light spots on the bottom, and the underwater robot blocks the objects on the bottom when moving forward, forming shadows on the captured images, resulting in the failure of the visual odometer at the front end of the visual SLAM algorithm based on the feature point method. The present invention proposes a solution to the above situation, enabling the underwater robot to obtain a stable pose estimation under complex underwater visual conditions. At the same time, the present invention uses the M-estimation and global optimization method for backend optimization, enabling the underwater robot to obtain a globally consistent trajectory and map, improving the performance and accuracy of the SLAM algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 is a schematic diagram of complex underwater visual conditions;

[0056] Figure 2 is the overall block diagram of the present invention;

[0057] Figure 3 is a schematic diagram of the processing method for encountering light spots;

[0058] Figure 4 is a schematic diagram of the processing method for encountering shadow problems;

[0059] Figure 5 is a schematic diagram of the backend optimization method;

[0060] Figure 6 is the effect diagram of underwater feature extraction, Figure 6 in which (a) is the effect diagram of underwater feature extraction in the image frame with light spots and shadow areas, and (b) is the effect diagram of underwater feature extraction in the image frame without light spots and shadow areas;

[0061] Figure 7 is a schematic diagram of the comparison between the trajectory generated by the method of the present invention and the true reference trajectory. DETAILED DESCRIPTION OF THE INVENTION

[0062] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

[0063] As Figure 1 shown, a simultaneous localization and mapping method for underwater complex visual conditions in this example is applicable to situations where, when the light is strong, there may be light spots filling the bottom of the water, and when the underwater robot moves forward, it may block the objects on the bottom of the water, resulting in shadows in the images captured by the camera. The figure shows an example where both light spots and shadows exist in the captured image.

[0064] As Figure 2 shown, a simultaneous localization and mapping method for underwater complex visual conditions in this embodiment includes the following steps:

[0065] S1. The camera carried by the underwater robot starts to capture images, obtains image data, and performs underwater simultaneous localization and mapping.

[0066] S2. Determine whether there may be light spots in the image obtained in step S1. If there may be light spots, use an affine shadow formation model to model the brightness change of the image and estimate the light spot area, and then use an image filtering model based on saliency and gradient information to process the light spot area. As Figure 3 shown, it specifically includes the following sub-steps:

[0067] S21. Determine whether there may be light spots in the image: Convert the image to a grayscale image, use the Gaussian blur algorithm to denoise the image, and then determine whether there are light spots according to the grayscale threshold of the processed image. The judgment criterion is shown in Equation (1):

[0068] (1),

[0069] where, represents the grayscale value of any pixel point in the image, T represents the set grayscale threshold, set to 200. Detect each pixel point in the image in turn. If the situation where has a value of 1 appears, it means that there may be light spots, and perform the operation of step S22; otherwise, transfer to step S3;

[0070] S22. Use an affine shadow formation model to model the brightness change of the image and estimate the light spot area: If there are light spots in the image, it is necessary to consider the influence of light changes on the image brightness and confirm which areas of the brightness change are caused by light spots rather than other factors (such as surface reflection or ambient light changes). Assume that in the absence of the influence of light spots, the brightness change of the image is predicted by an affine shadow formation model based on reflectivity and ambient light, as shown in Equation (2):

[0071] (2),

[0072] Among them, is the predicted value of the image brightness without the influence of light spots, is the brightness value in the original image, is the reflectivity coefficient, is the offset, representing the influence of ambient light. Calculate the actual brightness value of the image and the predicted brightness value The difference between them is shown in Equation (3):

[0073] (3),

[0074] According to the brightness difference To estimate the light spot area, set a threshold O. In this embodiment, O is taken as 50. If Exceeds O, then determine that this area is the light spot area and perform the next step of processing;

[0075] S23. Use an image filtering model based on saliency and gradient information to process the light spot area: First, measure the saliency by calculating the local contrast of the neighborhood around each pixel in the light spot area. The local contrast calculation is shown in Equation (4):

[0076] (4),

[0077] Among them, is the brightness value of the pixel point in the light spot area, is the local brightness mean value of the area around the pixel point , is the pixel point The local standard deviation of the surrounding area; then calculate the gradients of the light spot area image in the horizontal and vertical directions through the Sobel operator to obtain the gradient magnitude of the image, as shown in Equation (5):

[0078] (5),

[0079] Combine the saliency and the gradient information to obtain a comprehensive weight to guide the filtering, The calculation is shown in Equation (6):

[0080] (6),

[0081] Among them, and is the weighting coefficient; according to the calculated comprehensive weight, perform adaptive bilateral filtering to optimize the light spot area. The specific formula for adaptive bilateral filtering is shown in Equation (7):

[0082] (7),

[0083] wherein, and are Gaussian kernel functions in the spatial domain and pixel value domain, controlling the influence of spatial distance and pixel value difference, is the brightness value of the light spot area after filtering, represents pixel the positions of other pixels within the neighborhood.

[0084] S3. Determine whether there is a shadow in the image processed in step S2. If so, first preliminarily distinguish the shadow and non-shadow areas through the likelihood ratio test method based on region growing, and then further optimize the segmentation effect of the shadow area through the Markov random field method, and provide a compensation image for the shadow area according to the images of adjacent frames, as Figure 4 shown, specifically including the following sub-steps:

[0085] S31. Determine whether there is a shadow in the image processed in step S2: Calculate the grayscale histogram , wherein is the grayscale value, records the number of pixels at each grayscale value Set a threshold of the low grayscale value to distinguish the low grayscale area and other areas. In this embodiment take 40, and calculate the pixel ratio C of the low grayscale area in the image, as shown in Equation (8):

[0086] C (8),

[0087] wherein, is the number of pixels whose grayscale value is in the interval, is the total number of pixels in the image. If C exceeds the threshold , set to 0.2, it means there is a shadow area, and perform the operation of step S32, otherwise transfer to step S4;

[0088] S32. Preliminarily distinguish the shadow and non-shadow areas through the likelihood ratio test method based on region growing: Assume that the probability distributions of the shadow area and the non-shadow area are and , respectively representing the probabilities of the image pixel under the shadow area and the non-shadow area, wherein is the shadow hypothesis, is the non - shadow hypothesis, and the likelihood ratio is defined as shown in Equation (9):

[0089] (9),

[0090] Compare the likelihood ratio with the set threshold to determine whether the pixel belongs to the shadow area. In this embodiment is set to 0.5. If the likelihood ratio of a pixel exceeds the threshold, it belongs to the shadow area; otherwise, it belongs to the non - shadow area. Region growing starts from the seed pixels and gradually expands to adjacent pixels. First, select the seed pixel area initially classified as shadow, and then expand the area according to whether the likelihood ratio of the pixels meets the threshold condition. For each newly added pixel , calculate its likelihood ratio , and determine whether it belongs to the shadow area according to the threshold;

[0091] S33. Further optimize the segmentation effect of the shadow area by the Markov random field method: Consider the pixels in the image as random variables, and there is a dependence relationship between each pixel and its adjacent pixels. Given the label of pixel (0 represents non - shadow, 1 represents shadow), the Markov energy function is defined as shown in Equation (10):

[0092] (10),

[0093] where the data term represents the classification cost of pixel , and the calculation formula is shown in Equation (11):

[0094] (11),

[0095] The smoothness term represents the cost of label consistency between adjacent pixels. By defining a penalty term when the labels of adjacent pixels are inconsistent, it encourages adjacent pixels to have the same label. The smoothness term is defined using Equation (12):

[0096] (12),

[0097] where is the penalty factor of the smoothness term, set to 0.3, which controls the influence degree of the smoothness term on the total energy, ) is the indicator function. If , its value is 1; otherwise, its value is 0. By minimizing the energy function , the optimal shadow region segmentation, i.e., the optimal label configuration of each pixel, is obtained, which contains the information of whether each pixel belongs to the shadow region;

[0098] S34. Provide a compensated image of the shadow region based on the images of adjacent frames: After the shadow region is recognized and segmented, the shadow region is compensated by the image information of adjacent frames. Assume that compensation information is obtained from the non-shadow region of an adjacent frame, such as the previous frame. For the shadow pixels in the current frame , its compensation value can be obtained by the method of weighted average, as shown in Equation (13):

[0099] (13),

[0100] where, is the compensated pixel value, is the non-shadow pixel value corresponding to in the previous frame, is the shadow pixel value in the current frame, is a weight factor, set to 0.6, which controls the compensation degree of the shadow region.

[0101] S4. Extract and match feature points from the image frame after processing the light spot and shadow region, select key frames according to the matched feature points, and calculate the camera pose information by using the PnP algorithm combined with the pose optimization method based on weighted RANSAC. At the same time, obtain the local map of the environment, which specifically includes the following sub-steps:

[0102] S41. Feature point extraction and matching: First, use the scale-invariant feature transform algorithm to detect the feature points of each frame of the image processed in step S3 and generate the descriptor of each feature point, and then use the matching algorithm based on the K-dimensional tree to match the feature points;

[0103] S42. Select key frames: According to the change in the number of matched image feature points between adjacent frames, select the frames with significant changes as key frames;

[0104] S43. Calculate the camera pose information by using the PnP algorithm combined with the pose optimization method based on weighted RANSAC: First, estimate the initial pose of the camera by using the PnP algorithm according to the matched feature points in the key frame , and then calculate the reprojection error , The calculation is as shown in Equation (14):

[0105] (14),

[0106] where, is the The position of a matching point in the two-dimensional image, is the position of the corresponding three-dimensional point of the th matching point in the world coordinate system, is the projection function that projects the three-dimensional point onto the two-dimensional image plane. Each matching point is assigned a weight factor according to the matching quality of the feature descriptor to measure the reliability of each matching point. The calculation is as shown in Equation (15):

[0107] (15),

[0108] where represents the distance between the matching point and its descriptor. Calculate the weighted error of each matching point. According to the weighted error, inliers and outliers are judged. In this embodiment, a threshold D is set to 2. If is less than the threshold, it is considered that the th matching point is an inlier, otherwise it is an outlier. In each iteration, the inlier set is calculated based on the current pose and the inliers and outliers are re-evaluated according to the weighted error. Through multiple iterations, the camera pose is gradually updated until the optimal pose is obtained;

[0109] S44. Update the local map: Update the obtained pose information and matching feature points to the local environmental map to maintain the coherence and accuracy of the local map.

[0110] S5. Use M-estimation to handle outliers or noise to enhance the robustness of the system, and further apply global optimization to optimize the entire system to obtain a globally consistent underwater robot trajectory and environmental map, as Figure 5 shown, which specifically includes the following sub-steps:

[0111] S51. Use M-estimation to handle outliers or noise: Select the latest th matching point in step S44 as the observation data, and the positions and of the corresponding three-dimensional points of the initial pose as the initial estimate. Use the reprojection error as the observation error, select the Huber loss method in M-estimation for model estimation, and design the Huber loss function of each observation as shown in Equation (16):

[0112] (16),

[0113] where is a set tolerance threshold, which is set to 2 in this embodiment. The set loss threshold is , and in this embodiment is 3. If the loss function of a certain observation value exceeds , then mark this observation as an outlier and remove it;

[0114] S52. Apply global optimization to further optimize the entire system: First, construct the optimization problem, and set the optimization variables as pose and position . The objective function is , where is the Huber loss function designed in step S51. Select the nonlinear least squares optimization algorithm to perform global optimization, and update the variables as shown in Equation (17):

[0115] (17),

[0116] where , are the optimization variables, is the Jacobian matrix, is the residual vector, which is equal to in step S51, is a settable adjustment factor, which is set to 1e-3 here. Continuously adjust the pose and the position of the map points, that is, keep repeating to update the optimization variables until the change of the objective function is less than the preset threshold N, which is set to 1e-4 in this embodiment;

[0117] S53. Obtain the optimized variables in step S52, and draw the trajectory of the underwater robot and the global map.

[0118] The experimental simulation environment of the simultaneous localization and mapping method for complex underwater visual conditions is: GPU NVIDIA RTX4050, CPU I5-13450HX, Ubuntu 22.04.

[0119] To verify the effectiveness of the method of the present invention, the method of the present invention is compared with ORB-SLAM3, and the effects of feature extraction and matching and the absolute pose error APE are compared. The experimental results are shown in the following table. It can be seen that the feature extraction and matching effect of the method of the present invention is better than that of ORB-SLAM3, and the APE value is also smaller than that of the ORB-SLAM3 method.

[0120] Table 1 Comparison between ORB-SLAM3 and the method of the present invention

[0121] ORB-SLAM3 The method of the present invention Extraction number 466 475 Number of matching pairs 266 276 Matching time / s 0.0053 0.0026 Matching rate % 57.08 58.60 APE (m) 1.22 0.18

[0122] It can be seen that the simultaneous localization and mapping method for complex underwater vision conditions of the present invention can significantly improve the accuracy of AUV localization and mapping in the underwater environment. Specific experimental figures can be referred to Figure 6 and Figure 7 , where Figure 6 are the effect diagrams of underwater feature extraction in image frames with and without light spots and shadow areas respectively, Figure 7 is a comparison schematic diagram of the trajectory generated by the method of the present invention and the true reference trajectory.

Claims

1. A method for simultaneous positioning and real-time imaging for complex underwater visual conditions, characterized in that: The method comprises the following steps: S1. The underwater robot is equipped with a camera to take images and obtain image data; S2. Determine whether there is a light spot in the image obtained in step S1. If there is a light spot, use an affine shadow formation model to model the brightness change of the image and estimate the light spot area, and then use an image filtering model based on saliency and gradient information to process the light spot area; S3. Determine whether there is a shadow in the image processed by step S2. If there is a shadow, first preliminarily distinguish the shadow and non-shadow areas by a likelihood ratio test method based on region growth, then further optimize the segmentation effect of the shadow area by a Markov random field method, and provide a compensation image of the shadow area according to the image of the adjacent frame; S4. Extract and match feature points of the image frames after processing the light spot and shadow area, select key frames according to the matched feature points, use the PnP algorithm combined with the weighted RANSAC-based pose optimization method to calculate the camera pose information, and obtain a local map of the environment; S5. Use M-estimation to deal with outliers or noise and enhance the robustness of the system. Further apply global optimization to optimize the entire system and obtain globally consistent underwater robot trajectories and environmental maps.

2. The method for simultaneous positioning and real-time imaging for underwater complex visual conditions according to claim 1, characterized in that: The step S2 specifically includes the following sub-steps: S21. Determine whether there is a light spot in the image: convert the image into a grayscale image, use the Gaussian blur algorithm to denoise the image, and then determine whether there is a light spot based on the grayscale threshold of the processed image. The judgment criterion is shown in formula (1): Wherein, g(x,y) represents the gray value of any pixel point (x,y) in the image, T represents the set gray threshold, and each pixel point in the image is detected in turn. If the value of f(x,y) is 1, it means that there is a light spot, and the operation of step S22 is performed, otherwise it goes to step S3; S22. Model the brightness change of the image and estimate the spot area through the affine shadow formation model: If there is a spot in the image, it is necessary to consider the impact of the illumination change on the image brightness; assuming that there is no spot effect, the brightness change of the image is predicted by an affine shadow formation model based on reflectivity and ambient light, as shown in formula (2): I1(x,y)=α·I0(x,y)+β (2) Where I1(x, y) is the image brightness prediction value without the influence of light spots, I0(x, y) is the brightness value in the original image, α is the reflectivity coefficient, and β is the offset, which indicates the influence of ambient light. The difference between the actual image brightness value I2(x, y) and the brightness prediction value I1(x, y) is calculated as shown in formula (3): ΔI(x,y)=I2(x,y)-I1(x,y) (3) The spot area is estimated based on the brightness difference ΔI(x,y), and a threshold O is set. If ΔI(x,y) exceeds O, the area is determined to be a spot area and the next step is performed. S23. The image filtering model based on saliency and gradient information is used to process the spot area: First, the saliency is measured by calculating the local contrast of the neighborhood around each pixel in the spot area. The local contrast calculation is shown in formula (4): Among them, I(x,y) is the brightness value of the pixel point (x,y) in the spot area, μ(x,y) is the local brightness mean of the area around the pixel point (x,y), and σ(x,y) is the local standard deviation of the area around the pixel point (x,y). Then, the Sobel operator is used to calculate the gradient of the spot area image in the horizontal and vertical directions to obtain the gradient amplitude F(x,y) of the image, as shown in formula (5): Combining the saliency S(x,y) and the gradient information F(x,y), we get a comprehensive weight W(x,y) to guide the filtering. The calculation of W(x,y) is shown in formula (6): W(x,y)=γ·S(x,y)+η·F(x,y) (6) Among them, γ and η are weighting coefficients; according to the calculated comprehensive weight, adaptive bilateral filtering is performed to optimize the spot area. The specific formula of adaptive bilateral filtering is shown in formula (7): in, and It is the Gaussian kernel function of the spatial domain and the pixel value domain, which controls the influence of spatial distance and pixel value difference. I′(x,y) is the brightness value of the spot area after filtering, and (i,j) represents the positions of other pixels in the neighborhood of pixel (x,y).

3. The method for simultaneous positioning and real-time imaging for underwater complex visual conditions according to claim 1, characterized in that: The step S3 specifically comprises the following sub-steps: S31. Determine whether there is a shadow in the image processed by step S2: calculate the grayscale histogram H(g) of the image, where g is the grayscale value, H(g) records the number of pixels at each grayscale value g, set a low grayscale value threshold g0 to distinguish low grayscale areas from other areas, and calculate the pixel ratio C of the low grayscale area in the image, as shown in formula (8): in, is the number of pixels whose grayscale value is in the interval [0,g0], is the total number of pixels in the image. If C exceeds the threshold C0, it means that there is a shadow area and the operation of step S32 is performed. Otherwise, it goes to step S4. S32. Preliminary distinction between shadow and non-shadow areas is made by the likelihood ratio test method based on region growing: Assume that the probability distribution of shadow and non-shadow areas is P((x, y)|Q1) and P((x, y)|Q0), which represent the probability of image pixel (x, y) being in the shadow and non-shadow areas respectively, where Q1 is the shadow hypothesis and Q0 is the non-shadow hypothesis. The likelihood ratio Λ(x, y) is defined as shown in formula (9): Compare the likelihood ratio Λ(x,y) with the set threshold Λ0 to determine whether the pixel belongs to the shadow area. If the likelihood ratio of a pixel exceeds the threshold, it belongs to the shadow area, otherwise it belongs to the non-shadow area; the region growth starts from the seed pixel and gradually expands to the adjacent pixels. First, the seed pixel region that is initially classified as the shadow is selected, and then the region is expanded according to whether the likelihood ratio of the pixel meets the threshold condition; for each newly added pixel (x′,y′), calculate its likelihood ratio Λ(x′,y′), and determine whether it belongs to the shadow area according to the threshold; S33. The segmentation effect of the shadow area is further optimized by the Markov random field method: the pixels in the image are regarded as random variables, each pixel has a dependency relationship with its neighboring pixels, and the label a(x,y)∈{0,1} of a given pixel (x,y), 0 represents non-shadow and 1 represents shadow. The Markov energy function E is defined as shown in formula (10): E=∑ (x,y) U(x,y,a(x,y))+∑ <(x,y),(x′,y′)> V(x,u,d′,y′,a(x,y),a(x′,y′)) (10) Among them, the data item U(x,y,a(x,y)) represents the classification cost of pixel (x,y), and the calculation formula is shown in formula (11): The smoothing term V(x,y,x′,y′,a(x,y),a(x′,y′)) represents the cost of label consistency between adjacent pixels. By defining a penalty term when the labels of adjacent pixels are inconsistent, adjacent pixels are encouraged to have the same label. The smoothing term is defined using formula (12): V(x,y,x′,y′,a(x,y),a(x′,y′))=ν·M(a(x,y)≠a(x′,y′)) (12) Among them, ν is the penalty factor of the smoothness term, which controls the influence of the smoothness term on the total energy. Μ(a(x,y)≠a(x′,y′)) is the indicator function. If a(x,y)≠a(x′,y′), its value is 1, otherwise its value is 0. By minimizing the energy function E, the optimal shadow area segmentation, that is, the optimal label configuration of each pixel, is obtained, which contains the information of whether each pixel belongs to the shadow area; S34. Providing a compensated image of the shadow area according to the image of the adjacent frame: After the shadow area is identified and segmented, the shadow area is compensated by the image information of the adjacent frame. Assuming that the compensation information is obtained from the non-shadow area of ​​the adjacent frame such as the previous frame, for the shadow pixel (x, y) in the current frame, its compensation value B(x, y) is obtained by weighted averaging, as shown in formula (13): B(x,y)=ε·B1(x,y)+(1-ε)B2(x,y) (13) Among them, B(x,y) is the compensated pixel value, B1(x,y) is the non-shadow pixel value corresponding to (x,y) in the previous frame, B2(x,y) is the shadow pixel value in the current frame, and ε is a weight factor that controls the degree of compensation of the shadow area.

4. The method for synchronous positioning and real-time imaging for underwater complex visual conditions according to claim 1, characterized in that: The step S4 specifically comprises the following sub-steps: S41. Feature point extraction and matching: First, the scale-invariant feature conversion algorithm is used to detect the feature points of each frame image processed by step S3, and a descriptor of each feature point is generated, and then the feature points are matched using a K-dimensional tree-based matching algorithm; S42. Select key frames: select frames with significant changes as key frames according to the change in the number of image feature point matches between adjacent frames; S43. The PnP algorithm is combined with the weighted RANSAC-based pose optimization method to calculate the camera pose information: First, the PnP algorithm is used to estimate the initial pose Z of the camera based on the matched feature points in the key frame, and then the reprojection error e is calculated for each matching point. l , e l The calculation is shown in formula (14): e l =‖d l -π(Z·X l )‖ (14) Among them, d l is the position of the lth matching point in the two-dimensional image, X l is the position of the 3D point corresponding to the lth matching point in the world coordinate system, and π is the projection function; The three-dimensional point X l Projected onto the two-dimensional image plane, each matching point is assigned a weight factor k according to the matching quality of the feature descriptor. l , used to measure the reliability of each matching point, k l The calculation is shown in formula (15): Among them, A(d l ) represents matching point d l and the distance between its descriptors; Calculate the weighted error for each matching point The weighted error is used to determine the internal and external points. If If the value of the matching point is less than the threshold D, the lth matching point is considered to be an inlier, otherwise it is an outlier. Each iteration calculates the inlier set based on the current pose and re-evaluates the inliers and outliers according to the weighted error. Through multiple iterations, the camera pose is gradually updated until the optimal pose is obtained. S44. Update local map: update the obtained posture information and matching feature points to the local map of the environment to maintain the consistency and accuracy of the local map.

5. The method for simultaneous positioning and real-time imaging for underwater complex visual conditions according to claim 4, characterized in that: The step S5 specifically comprises the following sub-steps: S51. Use M estimation to process outliers or noise: Select the latest l′th matching point d in step S44 l′ As observation data, the initial pose Z ′ d l′ The corresponding 3D point position X l′ As an initial estimate, use the reprojection error e l′ As the observation error, e l′ =‖d l′ -π(Z′·X l′ )‖, select the Huber loss method in M ​​estimation for model estimation, and design the Huber loss function L(e l′ ) is shown in formula (16): Among them, δ is a set tolerance threshold, and the loss threshold is set to L0. If the loss function L(e l′ ) exceeds L0, the observation is marked as an outlier and removed; S52. Apply global optimization to further optimize the entire system: First, construct the optimization problem and set the optimization variables as posture Z′, position X l′ , the objective function is Where L(e l′ ) is the Huber loss function designed in step S51. The nonlinear least squares optimization algorithm is selected for global optimization. The updated variables are shown in formula (17): q t+1 =q t -(J T J+ΩI) -1 J T r (17) Among them, q t ,q t+1 is the optimization variable, J is the Jacobian matrix, and r is the residual vector, which is the same as e in step S51. l′ equal, Ω is a configurable adjustment factor, which continuously adjusts the pose and map point position, that is, repeatedly updates the optimization variables until the change of the objective function is less than the preset threshold N; S53. Obtain the variables optimized in step S52, and draw the underwater robot trajectory and the global map.

Images