Underwater visual SLAM method based on constrained camera motion and distortion correction

Abstract

The invention discloses an underwater visual SLAM method based on constrained camera motion and distortion correction. By utilizing a method of constraining the shooting of adjacent frame images and an overall camera motion path, an underwater speed optimization equation is created, and the underwater camera motion rate is locally and globally optimized. The invention solves the problem of bubbles caused by excessive deviation of the underwater camera motion rate, ensures the quality of the shot image, and is beneficial to SLAM mapping. When creating a bag of words, underwater distortion correction is performed on the shot image and its feature points by fusing radial distortion, tangential distortion and scattering coefficient, which can effectively solve the problem of feature point mismatching during loop detection, effectively eliminate underwater accumulated errors, and improve the true loop rate of loop detection.

Description

Technical Field

[0001] The present invention relates to the field of synchronous positioning and map construction, and in particular to an underwater visual SLAM method based on constrained camera motion and distortion correction. Background Art

[0002] In recent years, as robotics technology has attracted more and more attention, SLAM key technologies have developed rapidly, but there are still some unresolved problems.

[0003] At present, the existing visual SLAM methods can be divided into two categories: direct method and feature point method. Among them, the direct method is represented by the sparse direct method visual odometer (Direct Sparse Odometry, abbreviated as "DSO"), and the feature point method is represented by the monocular SLAM system based on sparse feature points (Oriented FAST and Rotated BRIEF-SLAM, abbreviated as "ORB-SLAM"). Although the public data sets of DSO and ORB-SLAM on land have achieved good results, the situation of testing in underwater environments is not ideal.

[0004] The existing visual real-time scene construction methods will cause the following problems when used in underwater environments:

[0005] (1) When the visual real-time scene construction system is used underwater, if the camera does not have other motion constraints, it will cause certain environmental factors such as bubbles to be generated underwater, which will cause the camera shooting quality to deteriorate, thus affecting the quality of SLAM mapping;

[0006] (2) In order to improve the shooting quality in underwater environments, the existing method is to improve it through air pre-calibration and underwater calibration. However, this method cannot directly process feature points, which will lead to mismatching of feature points, affect the true accuracy of loop detection, and affect the overall mapping.

[0007] In response to the above problems, in 2017, Yang Pingle and others from the Industrial Technology Research Institute of Jiangsu University of Science and Technology proposed an image processing method in an underwater visual SLAM system (authorization announcement number: CN104574387B). By establishing an underwater imaging model and dealing with the impact of underwater environmental factors on camera imaging, the mismatching and matching efficiency problems in data association are solved, and the feature matching problems caused by light refraction, scattering and absorption phenomena can be handled well. However, image distortion and restrictions on camera motion are not considered, which may cause feature matching errors and affect the mapping effect. In 2019, Wu Xiaojun and others from the Shenzhen Graduate School of Harbin Institute of Technology proposed a vision-based underwater measurement method (announcement number: CN105698767B), which uses the method of pre-calibration in air and underwater camera calibration to achieve accurate underwater measurement of the camera outside the water surface, but does not solve the distortion of feature points when the SLAM camera works underwater. In 2021, Zhang Fei and others from Jiangsu University of Science and Technology proposed a method for underwater autonomous navigation and positioning (authorization announcement number: CN106403953B). They used a random finite set method to model the SLAM problem, which can more accurately describe map feature information, map feature observation information, clutter and other factors. In terms of map feature estimation, it improves the estimation accuracy of the traditional method on the number and position of map features, and only improves the estimation calculation speed of the camera pose, but does not consider the important impact of the complex underwater environment on camera motion. In addition, it does not consider the distortion of images and their feature points that may occur underwater.

[0008] In summary, to realize the feasibility of underwater real-time scene construction, it is necessary to optimize the motion of the underwater camera and perform distortion correction on the image and its feature points. Summary of the invention

[0009] The purpose of the present invention is to overcome the defects of the prior art and provide an underwater visual SLAM method based on constrained camera motion and distortion correction. By creating a camera motion equation with constrained local adjacent frames and overall underwater speed, the underwater camera is locally and globally optimized. The optimization equation is used as an auxiliary condition to solve the influence of the underwater environment on mapping, which can effectively improve the quality of the real-time scene construction of the monocular camera vision in the underwater environment.

[0010] The object of the present invention is achieved in this way: an underwater visual SLAM method based on constrained camera motion and distortion correction comprises the following steps:

[0011] Step 1) sensor information acquisition;

[0012] Step 2) Front-end visual odometer based on feature point method of monocular camera;

[0013] Step 3) Backend optimization based on constrained camera motion and image distortion correction;

[0014] Step 3-1) Establish and solve the error cost function and increment equation;

[0015] Step 3-2) using the method of constraining the number of adjacent frames and the overall camera motion path, creating a camera motion equation that constrains the number of local adjacent frames and the overall underwater speed optimization;

[0016] Step 3-3) When creating the SLAM word bag, the captured image and its feature points are distorted by fusing radial distortion, tangential distortion and scattering coefficient;

[0017] Step 4) loop detection based on feature element similarity;

[0018] Step 5) SLAM mapping based on ORB feature matching.

[0019] In order to further optimize the camera motion, the step 3-1) specifically includes: using the relative relationship between the position information and relative motion information of the two cameras, constructing a least squares problem for optimization and solving, and estimating the camera motion change after optimization, and providing feedback on the camera motion;

[0020] First, the relative pose transformation is obtained based on the poses of the two frames of cameras. where η i ' j is the calculated relative displacement of the camera when taking two frames of images, where η i is the camera pose information of frame i, η i -1 is the inverse matrix of the camera pose information of frame i, η j is the camera pose information of frame j; in addition, the actual relative displacement η of the camera between the two frames of images is ij , the optimized objective function o is calculated based on the epipolar geometry method of the two frames i and j. ij for:

[0021] min o ij =min(η ij -η' ij ) (1)

[0022] Among them, ij is the objective function, η ij To capture the actual relative displacement of the camera between two frames of images;

[0023] Solving the objective function yields:

[0024]

[0025] in, is the Taylor expansion of the objective function, is the partial derivative of the objective function, δη i and δη j is a small perturbation of the camera pose between the two frames.

[0026] In order to eliminate the negative impact of the underwater environment on map construction, the path distance of adjacent frames is used to calculate the local and overall motion rates at the beginning to maintain the optimal motion rate. The step 3-2) specifically includes: proposing a method to limit the local and overall motion of the camera, denoted as C k,k+1,k+2,k+3,k+4 is the camera position at a certain frame number, C k,k+1 , C k+1,k+2 , C k+2,k+3 is the displacement path of the camera between adjacent frames, T is the time between each frame, and the speed optimization formula is used to limit the local and overall rates for:

[0027]

[0028] Among them, k is a certain position point in the camera motion process, T is the time for the camera to capture consecutive frames, and n is the total number of captured images - 1.

[0029] In order to integrate the distortion error that may be caused by the underwater environment into the original loop detection, effectively solve the similarity calculation of the loop detection, eliminate the cumulative error, improve the accuracy and recall rate of the loop detection, and thus improve the accuracy of map construction, the step 3-3) specifically includes: solving the radial distortion and tangential distortion of the camera, so as to solve the distortion of the spatial coordinates and pixel coordinates of the feature points caused by the underwater environment;

[0030] The camera's coordinate system point Q[X,Y,Z] is corrected by four distortion coefficients h 1 ,h 2 , l 1 , l 2 and the scattering coefficient k υ , get the correct pixel coordinates [x cd ,y cd ], that is, through the correction of radial distortion and tangential distortion:

[0031]

[0032] Among them, the overall distortion correction coordinate is [x cd ,y cd ], the distorted coordinates are [x,y], h 1 and h 2 is the radial distortion coefficient of the central area and the edge area, l 1 and l 2 is the tangential distortion coefficient in the x and y axes, b2 The value is the square sum of the horizontal and vertical coordinates of the corrected position, k υ is the scattering coefficient, d x d y They are the offsets in the x and y axis directions respectively;

[0033] The image and feature points after distortion correction, that is, the probability that all loops extracted by the algorithm are real loops:

[0034]

[0035] Among them, P is the loop detection accuracy, ∑m tp is the total number of events where the algorithm is looped and the fact is also looped, ∑n fp The algorithm is a loop but the fact is not the whole event of the loop.

[0036] The present invention adopts the above technical solution, and compared with the prior art, the beneficial effects are as follows: 1) In view of the problem that the environmental changes such as bubbles caused by the movement of underwater SLAM cameras affect the mapping, the present invention proposes a method for creating a camera motion equation that constrains the number of local adjacent frames and the overall underwater speed optimization. Through the incremental equation and the underwater speed optimization equation, the camera motion is optimized in motion, which can effectively limit and avoid the influence of the underwater environment on the camera mapping, and the optimization of the camera motion assists in improving the quality of the map construction. 2) In view of the problem that the underwater distortion of feature points and their images caused by underwater camera shooting affects the loop detection and judgment, the present invention proposes a method for underwater distortion correction of the captured images and establishing corresponding word bags. By introducing tangential, radial distortion coefficients and scattering coefficients, the feature points and images are distorted to prevent the problem of feature point mismatching, and by calculating the feature similarity of the word bag after distortion correction, it is determined whether there is a loop in the word bag, which can effectively eliminate the underwater cumulative error and complete the mapping with high quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 Flowchart of the present invention.

[0038] Figure 2 Schematic diagram of camera motion with back-end optimization of the present invention. DETAILED DESCRIPTION

[0039] like Figure 1 An underwater visual SLAM method based on constrained camera motion and distortion correction is shown, comprising the following steps:

[0040] Step 1) sensor information acquisition;

[0041] The camera captures visible light or electromagnetic spectrum and records it as a picture, which is represented as a digital image, that is, a two-dimensional matrix represented by a finite number of values, namely pixels. Sensor information reading in SLAM mainly involves the reading and preprocessing of camera image information.

[0042] Step 2) Front-end visual odometer based on feature point method of monocular camera;

[0043] The front-end visual odometer based on feature points is a more mainstream method of visual odometer. It has stable motion and is insensitive to lighting and dynamic objects. It is a more mature solution. Its work is mainly divided into two steps:

[0044] First, feature points are extracted from the image. The feature points remain unchanged after a small change in the camera perspective, allowing the computer to find the feature points again in subsequent images and then generate certain associations, so that the corresponding camera motion between the two frames can be found.

[0045] Based on ORB feature point matching and extraction, the main direction of the feature point is calculated in the previous feature extraction, that is, the extraction of corner points (key points), which adds the rotation invariance feature to the subsequent descriptor; and the descriptor describes the image area around the feature point extracted in the previous step. In ORB feature extraction, the main direction of the corner feature point is calculated to add the rotation invariance feature to the descriptor, which is mainly achieved through the grayscale centroid method. That is, the total grayscale value and its centroid are obtained, and the main direction offset rotation angle of the feature point is obtained:

[0046] θ=arctan -1 (u 01 ,u 10 ) (6)

[0047] Among them, θ is the main direction offset rotation angle of the feature point, u 01 is the gray value in the x direction, u 10 is the grayscale value in the y direction.

[0048] The matching of feature points corresponds to the real-time requirements in SLAM, and a large number of feature points can be matched based on the fast approximate nearest neighbor algorithm.

[0049] Second, based on matching the same feature points, the epipolar geometry constraint can be used to estimate the camera pose and the spatial position of the feature points. Assume that the matched feature points are located in the i-th frame and the j-th frame, that is, the feature point p i and p j It is the projection of P on the two frames. Assuming that the camera is from O i Movement to O j , that is, the motion and camera pose information η between the two frames needs to be solved.

[0050] Step 3) Backend optimization based on constrained camera motion and image distortion correction; Figure 2 As shown, based on the back-end optimization of constrained camera motion and image distortion correction, the error cost function and incremental equation are established and solved. This method utilizes the relative relationship between the position information and relative motion information of the two cameras, constructs a least squares problem for optimization and solution, and estimates the camera motion changes after optimization, and feeds back the camera motion to further optimize the camera motion. Figure 2 The middle triangle is a schematic diagram of the camera pose. The improved pose graph method limits and optimizes the number of frames and overall speed of the camera.

[0051] Step 3-1) Establish and solve the error cost function and increment equation;

[0052] The relative relationship between the position information and relative motion information of the two cameras is used to construct a least squares problem for optimization and solution. After optimization, the camera motion change is estimated and the camera motion is fed back.

[0053] First, the relative pose transformation is obtained based on the poses of the two frames of cameras. where η' ij is the calculated relative displacement of the camera when taking two frames of images, where η i is the camera pose information of frame i, η i -1 is the inverse matrix of the camera pose information of frame i, η j is the camera pose information of frame j; in addition, the actual relative displacement η of the camera between the two frames of images is ij , the optimized objective function o is calculated based on the epipolar geometry method of the two frames i and j. ij for:

[0054] min o ij =min(η ij -η' ij ) (7)

[0055] Among them, ij is the objective function, η ij To capture the actual relative displacement of the camera between two frames of images;

[0056] Solving the objective function yields:

[0057]

[0058] in, is the Taylor expansion of the objective function, is the partial derivative of the objective function, δη i and δη j is a small perturbation of the camera pose between the two frames.

[0059] We can get η i and η j The partial derivative of the corresponding objective function can be used to solve the incremental equation to solve the displacement optimization problem of the camera affected by the underwater environment.

[0060] Step 3-2) using the method of constraining the number of adjacent frames and the overall camera motion path, creating a camera motion equation that constrains the number of local adjacent frames and the overall underwater speed optimization;

[0061] When underwater objects move, bubbles will inevitably be generated, which will affect the camera's shooting quality and interfere with the construction of the map. Therefore, after solving the incremental equation, the speed between each frame number needs to be adjusted to achieve global optimization of the camera movement.

[0062] A method to limit the local overall motion of the camera is proposed, denoted as C k,k+1,k+2,k+3,k+4 is the camera position at a certain frame number, C k,k+1 , C k+1,k+2 , C k+2,k+3 is the displacement path of the camera between adjacent frames, T is the time between each frame, and the speed optimization formula is used to limit the local and overall rates for:

[0063]

[0064] Among them, k is a certain position point in the camera motion process, T is the time for the camera to capture consecutive frames, and n is the total number of captured images - 1.

[0065] After the map construction begins, the back-end optimization based on the improved pose graph method not only uses the pose information between adjacent frames and the image transformation between adjacent frames to solve the objective function to optimize the camera motion, but also uses the path distance between adjacent frames to calculate the local and overall motion rates at the beginning to maintain the optimal motion rate to eliminate the negative impact of the underwater environment on map construction.

[0066] Step 3-3) When creating the SLAM word bag, the captured image and its feature points are distorted by fusing radial distortion, tangential distortion and scattering coefficient;

[0067] Based on the back-end optimization of constrained camera motion and image distortion correction, the captured images are corrected for underwater distortion and the corresponding word bags are established. That is, the distortion errors that may be caused by the underwater environment are integrated into the original loop detection. This can effectively solve the similarity calculation of loop detection, eliminate cumulative errors, improve the accuracy and recall rate of loop detection, and thus improve the accuracy of map construction.

[0068] The method to solve underwater image distortion is to solve the radial distortion and tangential distortion of the camera, so as to solve the distortion of spatial coordinates and pixel coordinates of feature points caused by the underwater environment.

[0069] Assuming that the spatial coordinates of any point Q on the plane are [X, Y, Z], and the pixel coordinates of point Q are [m, n], the corresponding homogeneous coordinates are expressed as and in The feature points in the plane pass through the optical center to reach the imaging plane. Since the extracted feature points are all in the same plane, let Z = 0, then the spatial point and image points It can be expressed as an equation:

[0070]

[0071] Where s is a non-zero scale factor, r 1 and r 2 is the non-vertical factor of the x-axis and y-axis of the pixel plane coordinate axis, and e is the translation vector.

[0072] For the physical characteristics of radial distortion, the closer the feature point is to the edge, the greater the distortion is. The coordinates after radial distortion correction can be expressed as:

[0073]

[0074] Among them, the radial distortion correction coordinates are The distorted coordinates are [x, y], h 1 and h 2 is the radial distortion coefficient, h 1 Mainly corrects the central area of ​​the image with less distortion, h 2 Mainly corrects the edge area with large distortion, b 2 The value is the sum of the squares of the horizontal and vertical coordinates of the corrected position.

[0075] For tangential distortion, the tangential distortion parameters l in the x and y axes can be used 1 , l 2 To make corrections:

[0076]

[0077] Among them, the tangential distortion correction coordinates are The distorted coordinates are [x, y], l 1 and l 2 are the tangential distortion coefficients in the x and y axes.

[0078] In addition to the radial and tangential distortions that affect the image and feature point coordinates, the scattering coefficient k of the underwater environment υIt also needs to be taken into account in the image distortion correction process.

[0079] The camera's coordinate system point Q[X,Y,Z] is corrected by four distortion coefficients h 1 ,h 2 , l 1 , l 2 and the scattering coefficient k υ , get the correct pixel coordinates [x cd ,y cd ], that is, through the correction of radial distortion and tangential distortion:

[0080]

[0081] Among them, the overall distortion correction coordinate is [x cd ,y cd ], the distorted coordinates are [x,y], h 1 and h 2 is the radial distortion coefficient of the central area and the edge area, l 1 and l 2 is the tangential distortion coefficient in the x and y axes, b 2 The value is the square sum of the horizontal and vertical coordinates of the corrected position, k υ is the scattering coefficient, d x d y They are the offsets in the x and y axis directions respectively;

[0082] The distortion-corrected images and feature points can effectively improve the accuracy of loop detection, that is, the probability that all loops extracted by the algorithm are real loops:

[0083]

[0084] Among them, P is the loop detection accuracy, ∑m tp is the total number of events where the algorithm is looped and the fact is also looped, ∑n fp The algorithm is a loop but the fact is not the whole event of the loop.

[0085] Step 4) loop detection based on feature element similarity;

[0086] A new bag-of-words model is established using the distortion-corrected image and its feature points. When a new bag of words is added, the similarity between its feature points and the previous frame bag of words is calculated to determine whether the camera has returned to a place it has passed before. This can eliminate accumulated errors and improve the quality of map construction.

[0087] Step 5) SLAM mapping based on ORB feature matching;

[0088] When the front-end odometer, back-end optimization and loop detection in the SLAM model complete the location determination of all landmarks, that is, the set of all landmarks is obtained, the map can be built. In visual SLAM, mapping is used for service and positioning, but at the application level, mapping also has many other requirements, such as positioning, navigation, obstacle avoidance, reconstruction and interaction. Since SLAM is a low-level technology, it generally provides necessary information to upper-level applications according to different requirements.

[0089] The invention improves the classic ORB-SLAM, and optimizes the underwater camera locally and globally by creating a camera motion equation that constrains the number of local adjacent frames and the overall underwater speed. The optimization equation is used as an auxiliary condition to solve the influence of the underwater environment on map construction. Underwater distortion correction is performed on the captured images and their feature points, and corresponding word bags are established, which can effectively solve the problem of feature point mismatching during loop detection, effectively eliminate underwater accumulated errors, and improve the true loop rate of loop detection.

[0090] The present invention is not limited to the above-mentioned embodiments. On the basis of the technical solution disclosed in the present invention, technicians in this field can make some substitutions and deformations to some technical features therein according to the disclosed technical content without creative labor, and these substitutions and deformations are all within the protection scope of the present invention.

Claims

1. An underwater visual SLAM method based on constrained camera motion and distortion correction, characterized in that: The following steps are involved: Step 1) sensor information acquisition; Step 2) Front-end visual odometer based on feature point method of monocular camera; Step 3) Backend optimization based on constrained camera motion and image distortion correction; Step 3-1) Establish and solve the error cost function and increment equation; The step 3-1) specifically includes: using the relative relationship between the position information and relative motion information of the two cameras, constructing a least squares problem for optimization and solving, estimating the motion change of the camera after optimization, and providing feedback on the camera motion; First, the relative pose transformation is obtained based on the poses of the two frames of cameras. where η i ' j is the calculated relative displacement of the camera when taking two frames of images, where η i is the camera pose information of frame i, η i -1 is the inverse matrix of the camera pose information of frame i, η j is the camera pose information of frame j; in addition, the actual relative displacement η of the camera between the two frames of images is ij , the optimized objective function o is calculated based on the epipolar geometry method of the two frames i and j. ij for: mino ij =min(η ij -or i ' j ) (1) Among them, ij is the objective function, η ij To capture the actual relative displacement of the camera between two frames of images; Solving the objective function yields: in, is the Taylor expansion of the objective function, is the partial derivative of the objective function, δη i and δη j is a small perturbation of the camera pose in two frames; Step 3-2) using the method of constraining the number of adjacent frames and the overall camera motion path, creating a camera motion equation that constrains the number of local adjacent frames and the overall underwater speed optimization; The step 3-2) specifically includes: proposing a method for limiting the local overall motion of the camera, denoted as C k,k+1,k+2,k+3,k+4 is the camera position at a certain frame number, C k,k+1 , C k+1,k+2 , C k+2,k+3 is the displacement path of the camera between adjacent frames, T is the time between each frame, and the speed optimization formula is used to limit the local and overall rates for: Among them, k is a certain position point in the camera motion process, T is the time for the camera to take consecutive frames of pictures, and n is the total number of images taken - 1; Step 3-3) When creating the SLAM word bag, the captured image and its feature points are distorted by fusing radial distortion, tangential distortion and scattering coefficient; The step 3-3) specifically includes: solving the radial distortion and tangential distortion of the camera, thereby solving the distortion of the spatial coordinates and pixel coordinates of the feature points caused by the underwater environment; The camera's coordinate system point Q[X,Y,Z] is calculated by four distortion correction coefficients h1, h2, l1, l2 and the scattering coefficient k υ , get the correct pixel coordinates [x cd ,y cd ], that is, through the correction of radial distortion and tangential distortion: Among them, the overall distortion correction coordinate is [x cd ,y cd ], the distortion coordinates are [x, y], h1 and h2 are the radial distortion coefficients of the central area and the edge area, l1 and l2 are the tangential distortion coefficients in the x and y axis directions, b 2 The value is the square sum of the horizontal and vertical coordinates of the corrected position, k υ is the scattering coefficient, d x d y They are the offsets in the x and y axis directions respectively; The image and feature points after distortion correction, that is, the probability that all loops extracted by the algorithm are real loops: Among them, P is the loop detection accuracy, ∑m tp is the total number of events where the algorithm is looped and the fact is also looped, ∑n fp All events where the algorithm is a loop but the fact is not a loop; Step 4) loop detection based on feature element similarity; Step 5) SLAM mapping based on ORB feature matching.

Images