A method and apparatus for underwater refraction-compensated monocular and binocular cooperative localization and mapping

Abstract

This invention discloses an underwater refraction-compensated monocular and binocular collaborative localization and mapping method and apparatus. By extracting checkerboard corner points, a two-dimensional vector correction model based on Snell's law is applied to compensate for the underwater optical path, accurately calibrating the intrinsic and extrinsic parameters of the binocular camera. This method effectively restores the true underwater optical path and significantly reduces calibration errors caused by medium refraction. Epipolar correction is then performed on the calibrated left and right images to ensure geometric consistency in stereo matching. Finally, the monocular and binocular poses are fused and a seven-degree-of-freedom similarity transformation is applied for global alignment. This not only effectively solves the uncertainty of monocular scale but also, by introducing the true binocular scale, achieves high-precision and robust 3D mapping. By integrating key steps such as camera pre-calibration, refraction compensation, image correction, monocular tracking, binocular depth measurement, and scale alignment into a unified process, the accuracy of underwater visual localization and mapping is improved.

Description

Technical Field

[0001] This invention relates to the field of synchronous positioning and mapping technology, and in particular to a method and apparatus for underwater refraction-compensated monocular and binocular cooperative positioning and mapping. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology for intelligent autonomous systems to achieve autonomous navigation and environmental perception in unknown environments. Underwater, visual SLAM has become the mainstream passive optical imaging navigation and mapping solution due to its cost and information acquisition advantages. Visual SLAM can be mainly divided into monocular SLAM and binocular SLAM based on the configuration of the cameras used. Monocular SLAM systems typically use a single camera to acquire image sequences. By analyzing visual changes between consecutive image frames (such as the displacement of feature points), the system can estimate the camera's trajectory and gradually reconstruct the three-dimensional structure of the environment. Binocular SLAM systems use two cameras with a fixed baseline to simultaneously acquire left and right views of the scene. Utilizing the principle of stereo vision, by matching corresponding points in the left and right images and calculating their disparity, depth information of objects in the scene can be directly obtained. Some existing visual SLAM frameworks, such as ORB-SLAM, have been developed to support both monocular and binocular modes and are being tested in underwater environments, providing a certain technical foundation for underwater visual navigation and mapping.

[0003] Although existing underwater visual SLAM technology has been applied in some scenarios, it still has significant limitations in terms of accuracy and environmental adaptability. On the one hand, in terms of camera calibration, most methods ignore or fail to accurately handle the complex refraction effects of underwater light propagating through multiple media (such as water, camera housing glass, and air). Traditional calibration models based on the air environment often lead to parameter inaccuracies when directly applied underwater. Even if some methods attempt to compensate for refraction, due to model simplification (such as considering only water and air media) or computational complexity, it is difficult to effectively correct imaging distortion, resulting in significant calibration errors in camera parameter estimation, thereby weakening the overall system's localization robustness and mapping accuracy. On the other hand, in terms of SLAM algorithms, monocular systems are prone to cumulative scale drift during long-term underwater operation, resulting in a lack of global map consistency; although binocular systems have scale perception capabilities, in the underwater environment, due to the sparse and difficult matching of keyframe features, loop closure detection is difficult to trigger effectively, affecting both system robustness and output stability. These limitations collectively lead to problems such as low positioning accuracy, map distortion, and even system failure in complex underwater environments, restricting the breadth and depth of their application. Summary of the Invention

[0004] This invention provides a method and apparatus for underwater refraction-compensated monocular and binocular cooperative positioning and mapping, in order to reduce calibration errors in the underwater environment, avoid scale drift, and improve the accuracy of cooperative positioning and mapping.

[0005] To address the aforementioned technical problems, embodiments of the present invention provide an underwater refraction-compensated monocular and binocular cooperative localization and mapping method, comprising:

[0006] Acquire images of a chessboard calibration board, extract the corner pixel coordinates of the chessboard calibration board images; and obtain the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the images.

[0007] The compensation parameters of the binocular camera are obtained based on the initial intrinsic and extrinsic parameters and Snell's law. A global optimization model is constructed based on the compensation parameters, and the global optimization model is optimized based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters. The compensation parameters are the intrinsic and extrinsic parameters after refraction compensation.

[0008] Based on the target intrinsic and extrinsic parameters, the relative rotation matrix and translation vector of the binocular camera are decomposed and aligned to construct the correction matrix of the left and right images. The original image is then corrected based on the correction matrix to obtain the monocular image sequence and the binocular image sequence.

[0009] ORB features are extracted and tracked from the monocular image sequence, and the ORB features are normalized and translated based on a preset depth median to obtain the monocular pose at a relative scale.

[0010] Based on epipolar constraints, stereo matching is performed on the ORB features of the binocular image sequence to obtain disparity. Triangulation is then performed based on the calibrated baseline and focal length to obtain the three-dimensional coordinates of the feature points. Finally, the binocular pose at absolute scale is obtained based on the three-dimensional coordinates.

[0011] A seven-DOF similarity transformation matrix is ​​constructed for the monocular and binocular poses using the least squares method. Based on the binocular pose, the monocular pose is aligned and optimized using the similarity transformation matrix to output a target 3D map.

[0012] This invention extracts checkerboard corner points and applies a two-stage vector correction model based on Snell's law to compensate for underwater optical paths, accurately calibrating the intrinsic and extrinsic parameters of a binocular camera. This method effectively restores the true underwater optical path and significantly reduces calibration errors caused by medium refraction. Epipolar correction is then applied to the calibrated left and right images to ensure geometric consistency in stereo matching. Finally, the monocular and binocular poses are fused and a seven-DOF similarity transformation is applied for global alignment. This not only effectively solves the uncertainty of monocular scale but also, by introducing the true binocular scale, achieves high-precision and robust 3D mapping. By integrating key steps such as camera pre-calibration, refraction compensation, image correction, monocular tracking, binocular depth measurement, and scale alignment into a unified process, the accuracy of underwater visual positioning and mapping is improved.

[0013] Furthermore, the process involves acquiring a chessboard calibration board image, extracting the corner pixel coordinates of the chessboard calibration board image, and obtaining the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the image, including:

[0014] Acquire a chessboard calibration board image, extract initial corner pixel coordinates based on the chessboard calibration board image, perform anomaly correction on the initial corner pixel coordinates, and obtain the corner pixel coordinates;

[0015] The initial intrinsic and extrinsic parameters are obtained based on the corner pixel coordinates and the direct linear transformation method. The initial intrinsic and extrinsic parameters include an initial intrinsic parameter matrix and an initial extrinsic parameter matrix.

[0016] Furthermore, the process of obtaining compensation parameters for the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, constructing a global optimization model based on the compensation parameters, and optimizing the global optimization model based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters includes:

[0017] Based on the target intrinsic and extrinsic parameters, the corner pixel coordinates are normalized and transformed to obtain a first direction vector; the first direction vector is the unit ray direction vector in the air medium.

[0018] Based on Snell's law, the direction of light rays in the first direction vector is vector-corrected during the two refraction processes from air to glass and from glass to water to obtain compensation parameters and ideal pixel coordinates of corner points;

[0019] Based on the compensation parameters, preset radial distortion coefficients, and tangential distortion coefficients, a global optimization model is constructed. With the goal of minimizing the reprojection error, for each corner point's ideal pixel coordinates, the Levenberg-Marquardt algorithm is used to perform joint nonlinear optimization processing on the global optimization model to obtain the target's intrinsic and extrinsic parameters and target distortion parameters.

[0020] This invention significantly improves camera calibration accuracy by introducing radial and tangential distortion models after obtaining initial camera intrinsic and extrinsic parameters through direct linear transformation (DLT) and performing Levenberg-Marquardt joint nonlinear optimization with the goal of minimizing reprojection error. It effectively removes the influence of inherent lens distortion on imaging geometry, making subsequent ray normalization, refraction compensation, and image correction based on calibration parameters more reliable. This lays an accurate geometric foundation for precise mapping of the entire binocular system in complex underwater environments.

[0021] Furthermore, the binocular camera includes a left camera and a right camera; the step of decomposing and aligning the relative rotation matrix and translation vector of the binocular camera based on the target intrinsic and extrinsic parameters to construct a correction matrix for the left and right images, and correcting the original image based on the correction matrix to obtain a monocular image sequence and a binocular image sequence includes:

[0022] Based on the target intrinsic and extrinsic parameters and the Rodriguez formula, the rotation matrix of the left camera relative to the right camera is decomposed into a first rotation matrix and a second rotation matrix;

[0023] The rotation transformation matrix is ​​constructed based on the Rodriguez formula and the translation vector of the left camera relative to the right camera;

[0024] The correction matrices for the left and right images are constructed based on the first rotation matrix, the second rotation matrix, and the rotation transformation matrix.

[0025] This invention decomposes the binocular relative rotation matrix and translation vector obtained after refraction compensation to generate a correction matrix, aligning the epipolar lines of the left and right images to the same horizontal line and eliminating tilt and distortion between image pairs. This matrix, on the one hand, compresses the search space for stereo matching, improving the speed and accuracy of ORB feature matching; on the other hand, it ensures a pixel-level one-to-one correspondence during the matching process, providing a reliable geometric guarantee for precise triangulation and binocular pose estimation, thereby significantly improving stereo matching efficiency and depth measurement accuracy.

[0026] Furthermore, the step of constructing a seven-DOF similarity transformation matrix for the monocular and binocular poses based on the least squares method, using the binocular pose as a reference, and performing alignment optimization on the monocular pose based on the similarity transformation matrix to output a target 3D map includes:

[0027] The monocular and binocular poses are aligned and scaled based on timestamps to generate pose correspondences.

[0028] The rotational, translational, and scaling degrees of freedom parameters are estimated using the least squares method, and a seven-degree-of-freedom similarity transformation matrix is ​​constructed based on these parameters.

[0029] The similarity transformation matrix is ​​applied to the monocular pose, and global alignment is performed using the binocular pose as the scale and orientation reference to obtain the target monocular trajectory.

[0030] Global beam adjustment is performed based on the target monocular trajectory to output a 3D map of the target.

[0031] This invention, based on preliminary monocular and binocular pose estimation, constructs a seven-DOF similarity transformation by estimating rotation, translation, and scale parameters using time-stamp-based frame pair association and least squares method, and applies it to the entire monocular trajectory, achieving global alignment using the binocular trajectory as the scale and orientation reference. Subsequently, global bundle adjustment is further performed to jointly optimize camera pose and 3D map points. This not only eliminates scale drift in the monocular view but also suppresses accumulated errors through global optimization, ultimately outputting a 3D underwater map with realistic physical scale, high consistency, and high accuracy, greatly enhancing the overall reliability and map quality of the system.

[0032] In a second aspect, the present invention provides an underwater refraction-compensated monocular and binocular cooperative positioning and mapping device, comprising: an intrinsic and extrinsic parameter acquisition module, an optimization module, a correction module, a monocular pose estimation module, a binocular pose estimation module, and a mapping module;

[0033] The intrinsic and extrinsic parameter acquisition module is used to acquire chessboard calibration board images, extract the corner pixel coordinates of the chessboard calibration board images, and acquire the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the images.

[0034] The optimization module is used to obtain the compensation parameters of the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, construct a global optimization model based on the compensation parameters, and optimize the global optimization model based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters; the compensation parameters are intrinsic and extrinsic parameters after refraction compensation.

[0035] The correction module is used to decompose and align the relative rotation matrix and translation vector of the binocular camera based on the intrinsic and extrinsic parameters of the target, construct the correction matrix of the left and right images, correct the original image based on the correction matrix, and obtain the monocular image sequence and the binocular image sequence.

[0036] The monocular pose estimation module is used to extract and track ORB features from the monocular image sequence, and to normalize and translate the ORB features into a translation vector based on a preset depth median to obtain the monocular pose at a relative scale.

[0037] The binocular pose estimation module is used to perform stereo matching on the ORB features of the binocular image sequence based on epipolar constraints to obtain disparity, and to perform triangulation based on the calibrated baseline and focal length to obtain the three-dimensional coordinates of the feature points, and to obtain the binocular pose at the absolute scale based on the three-dimensional coordinates.

[0038] The mapping module is used to construct a seven-degree-of-freedom similarity transformation matrix for the monocular pose and binocular pose based on the least squares method, and to perform alignment optimization on the monocular pose based on the similarity transformation matrix using the binocular pose as a reference, thereby outputting a target 3D map.

[0039] Furthermore, the intrinsic and extrinsic parameter acquisition module is used to acquire a chessboard calibration board image, extract the corner pixel coordinates of the chessboard calibration board image, and obtain the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the image, including:

[0040] Acquire a chessboard calibration board image, extract initial corner pixel coordinates based on the chessboard calibration board image, perform anomaly correction on the initial corner pixel coordinates, and obtain the corner pixel coordinates;

[0041] The initial intrinsic and extrinsic parameters are obtained based on the corner pixel coordinates and the direct linear transformation method. The initial intrinsic and extrinsic parameters include an initial intrinsic parameter matrix and an initial extrinsic parameter matrix.

[0042] Furthermore, the optimization module is used to obtain compensation parameters for the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, construct a global optimization model based on the compensation parameters, and optimize the global optimization model based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters, including:

[0043] Based on the target intrinsic and extrinsic parameters, the corner pixel coordinates are normalized and transformed to obtain a first direction vector; the first direction vector is the unit ray direction vector in the air medium.

[0044] Based on Snell's law, the direction of light rays in the first direction vector is vector-corrected during the two refraction processes from air to glass and from glass to water to obtain compensation parameters and ideal pixel coordinates of corner points;

[0045] Based on the compensation parameters, preset radial distortion coefficients, and tangential distortion coefficients, a global optimization model is constructed. With the goal of minimizing the reprojection error, for each corner point's ideal pixel coordinates, the Levenberg-Marquardt algorithm is used to perform joint nonlinear optimization processing on the global optimization model to obtain the target's intrinsic and extrinsic parameters and target distortion parameters.

[0046] Furthermore, the binocular camera includes a left camera and a right camera; the correction module is used to decompose and align the relative rotation matrix and translation vector of the binocular camera based on the target intrinsic and extrinsic parameters, construct correction matrices for the left and right images, correct the original image based on the correction matrices, and obtain monocular image sequences and binocular image sequences, including:

[0047] Based on the target intrinsic and extrinsic parameters and the Rodriguez formula, the rotation matrix of the left camera relative to the right camera is decomposed into a first rotation matrix and a second rotation matrix;

[0048] The rotation transformation matrix is ​​constructed based on the Rodriguez formula and the translation vector of the left camera relative to the right camera;

[0049] The correction matrices for the left and right images are constructed based on the first rotation matrix, the second rotation matrix, and the rotation transformation matrix.

[0050] Furthermore, the mapping module is used to construct a seven-degree-of-freedom similarity transformation matrix for the monocular and binocular poses based on the least squares method, and to perform alignment optimization on the monocular pose based on the similarity transformation matrix using the binocular pose as a reference, outputting a target 3D map, including:

[0051] The monocular and binocular poses are aligned and scaled based on timestamps to generate pose correspondences.

[0052] The rotational, translational, and scaling degrees of freedom parameters are estimated using the least squares method, and a seven-degree-of-freedom similarity transformation matrix is ​​constructed based on these parameters.

[0053] The similarity transformation matrix is ​​applied to the monocular pose, and global alignment is performed using the binocular pose as the scale and orientation reference to obtain the target monocular trajectory.

[0054] Global beam adjustment is performed based on the target monocular trajectory to output a 3D map of the target. Attached Figure Description

[0055] Figure 1 This is a flowchart illustrating an underwater refraction-compensated monocular and binocular cooperative localization and mapping method provided in an embodiment of the present invention.

[0056] Figure 2 An original image for polar line correction provided in an embodiment of the present invention;

[0057] Figure 3 An epipolar correction result diagram provided in an embodiment of the present invention;

[0058] Figure 4A schematic diagram of a monocular and binocular cooperative localization and scale correction process provided for an embodiment of the present invention;

[0059] Figure 5 This is another flowchart illustrating an underwater refraction-compensated monocular and binocular cooperative localization and mapping method provided in an embodiment of the present invention. Detailed Implementation

[0060] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and are not intended to limit the scope of the invention.

[0061] The terms "first" and "second," etc., in the specification, claims, and drawings of this application are used to distinguish different objects, not to describe a specific order. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or apparatus that includes a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to such processes, methods, products, or apparatus.

[0062] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0063] Example 1

[0064] See Figure 1 , Figure 1 This is a flowchart illustrating an underwater refraction-compensated monocular and binocular cooperative localization and mapping method provided in an embodiment of the present invention. The embodiment of the present invention provides an underwater refraction-compensated monocular and binocular cooperative localization and mapping method, including steps 101 to 106, as detailed below:

[0065] Step 101: Acquire images of the chessboard calibration board and extract the corner pixel coordinates of the chessboard calibration board image; and obtain the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the image;

[0066] In this embodiment, the process of acquiring a chessboard calibration board image, extracting the corner pixel coordinates of the chessboard calibration board image, and obtaining the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the image includes:

[0067] Acquire a chessboard calibration board image, extract initial corner pixel coordinates based on the chessboard calibration board image, perform anomaly correction on the initial corner pixel coordinates, and obtain the corner pixel coordinates;

[0068] The initial intrinsic and extrinsic parameters are obtained based on the corner pixel coordinates and the direct linear transformation method. The initial intrinsic and extrinsic parameters include an initial intrinsic parameter matrix and an initial extrinsic parameter matrix.

[0069] In this embodiment, for the initial calibration of the binocular camera, images of the calibration board from multiple different perspectives are first acquired in an air environment. The corner pixel coordinates of each image are then extracted using a sub-pixel level corner detection algorithm. For corners with large detection errors due to lighting or occlusion, their sub-pixel positions can be manually removed or fine-tuned after extraction to ensure the accuracy of subsequent calculations. Next, based on the known grid spacing and geometric topology of the checkerboard, a system of linear equations between the world coordinates and pixel coordinates of the calibration board is constructed using the Direct Linear Transformation (DLT) method. Solving this system yields the camera's initial intrinsic parameter matrix K (including focal length f and principal point coordinates c) and initial extrinsic parameters.

[0070] Step 102: Determine the first direction vector based on the target intrinsic and extrinsic parameters and the chessboard calibration board image, and perform vector correction on the first direction vector based on Snell's law to obtain the compensation parameters of the binocular camera; the first direction vector is the unit ray direction vector in the air medium;

[0071] In this embodiment, the process of obtaining compensation parameters for the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, constructing a global optimization model based on the compensation parameters, and optimizing the global optimization model based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters includes:

[0072] Based on the target intrinsic and extrinsic parameters, the corner pixel coordinates are normalized and transformed to obtain a first direction vector; the first direction vector is the unit ray direction vector in the air medium.

[0073] Based on Snell's law, the direction of light rays in the first direction vector is vector-corrected during the two refraction processes from air to glass and from glass to water to obtain compensation parameters and ideal pixel coordinates of corner points;

[0074] Based on the compensation parameters, preset radial distortion coefficients, and tangential distortion coefficients, a global optimization model is constructed. With the goal of minimizing the reprojection error, for each corner point's ideal pixel coordinates, the Levenberg-Marquardt algorithm is used to perform joint nonlinear optimization processing on the global optimization model to obtain the target's intrinsic and extrinsic parameters and target distortion parameters.

[0075] In this embodiment, images of a checkerboard calibration board taken from multiple angles in an underwater environment are acquired, and the pixel coordinates (u,v) of the checkerboard corner points in the image are detected and extracted. The camera intrinsic parameter matrix K obtained in the previous step of calibration in an air environment is called, and the extracted pixel coordinates (u,v) are converted into normalized image plane coordinates, thereby calculating the unit direction vector d of the ray inside the camera (air medium). air That is, the first direction vector:

[0076]

[0077] In this embodiment, in the underwater refraction compensation model, for each incident ray, its unit direction vector is denoted as I, the refracted unit direction vector is denoted as T, and the normal unit vector at the interface is denoted as N. First, the relationship between the incident angle θ1 and the refraction angle θ2 is calculated according to Snell's law n1sinθ1=n2sinθ2, where the refractive indices of the media before and after refraction are n1 and n2, respectively.

[0078] In this embodiment, in order to obtain the refraction relationship in vector form, the direction vector I of the incident light is decomposed into a component I perpendicular to the interface. N and the component I parallel to the interface L :

[0079] cosθ1=cos(I,N)=|I||N|cos(I,N)=I·N (4)

[0080] I N =cosθ1N=(I·N)N (5)

[0081] I L =II N =I-(I·N)N (6)

[0082] sinθ1=|I L | (7)

[0083] According to Snell's law, we can obtain:

[0084]

[0085] Next, the direction vector T of the refracted ray is decomposed into components T parallel to the interface. L and the component T perpendicular to the interface N :

[0086]

[0087]

[0088] From parallel component T L and vertical component TN The direction vector T of the composite refracted ray:

[0089]

[0090] The specific formula for correcting the ray direction vector is as follows:

[0091]

[0092] This vector-form refraction formula can be directly applied to the light correction at both the air-glass and glass-water interfaces, accurately restoring the light path offset during underwater imaging, thereby significantly improving the measurement accuracy of binocular cameras in underwater environments.

[0093] In this embodiment, the direction vector of the light ray is accurately corrected based on the two refraction processes. According to the symmetry of Snell's law, under ideal conditions where absorption and scattering by the medium are ignored, the refraction path of the light ray is reversible. In an underwater environment, the light ray first enters the glass (the camera's waterproof housing) from the water, and then enters the camera sensor in the air. Therefore, it is necessary to derive and correct the change in the direction vector of the light ray during the two refraction processes. The unit direction vector of the refraction interface normal is N = (0,0,1). Combining the refraction model, the above correction formula is applied to each interface to obtain the direction vector d of the light ray in the water. water :

[0094] d glass =refract(d air ,N) (15)

[0095] d water =refract(d glass ,N) (16)

[0096] Wherein, the refractive index n of air water The refractive index n of the glass is approximately 1.00. water ≈1.52, the refractive index n of the water body water ≈1.33. Finally, by d... water Reconstructing the pixel coordinates of the corner points of the checkerboard pattern without refraction (u ′ ,v ′ ):

[0097]

[0098] In this embodiment, the initial intrinsic and extrinsic parameter matrices calibrated in an air environment, along with the corresponding preset radial and tangential distortion coefficients, are first used to perform refraction compensation on the pixel coordinates of the checkerboard corner points acquired underwater, in conjunction with Snell's law. First, the pixel coordinates of each corner point are transformed to a normalized image plane using the target intrinsic and extrinsic parameters to obtain the unit ray direction vector of that corner point in the air medium. Then, Snell's law is applied to perform vector correction for the light rays at the air-glass and glass-water interfaces, accurately restoring the propagation direction of the light rays in each medium, and the compensated ideal pixel coordinates are obtained by back-projection. Next, the system constructs a global nonlinear optimization model using the compensated ideal pixel coordinates of all corner points, the corresponding world 3D coordinates, and the preset distortion coefficients as input. With the objective of minimizing the reprojection error of all corner points, the Levenberg-Marquardt algorithm is invoked to perform joint iterative optimization of the intrinsic, extrinsic, and radial / tangential distortion coefficients in the model. This optimization process can simultaneously correct imaging deviations caused by medium refraction and lens distortion, and finally output highly accurate and robust target intrinsic parameter matrix, extrinsic parameter matrix and distortion parameters, providing an accurate and reliable geometric basis for subsequent underwater binocular vision measurement and 3D reconstruction.

[0099] In this embodiment, the ideal pixel coordinates (u) of the checkerboard corner points, calculated based on the above refraction model and compensated for the refraction effect, are used. ′ ,v ′ The process involves estimating the intrinsic and extrinsic parameters of the left and right cameras, and initializing the rotation matrix and translation vector of the right camera relative to the left camera. Then, a global optimization model is constructed, incorporating the intrinsic parameters and relative poses of the left and right cameras. The goal is to minimize the total reprojection error of the checkerboard corner points across all images. The Levenberg-Marquardt algorithm is used for nonlinear joint optimization to finally obtain the intrinsic and extrinsic parameters of the binocular cameras, compensated for underwater refraction effects.

[0100] In this embodiment, radial distortion coefficients k1 and k2 and tangential distortion coefficients p1 and p2 are introduced to perform nonlinear optimization on the initial intrinsic, extrinsic, and distortion parameters. With the goal of minimizing the reprojection error of corner points in all images, the Levenberg-Marquardt algorithm is used to iteratively adjust the parameters, and the contribution of each parameter to the error is evaluated in real time until the error converges to a preset threshold. This step ultimately outputs the target intrinsic parameters K, extrinsic parameters [R|t], and distortion parameters {k1,k2,p1,p2} with high-precision geometric correction capabilities, providing accurate and reliable basic parameters for subsequent underwater environment refraction compensation and dual-target calibration.

[0101] In this embodiment, after obtaining the initial camera intrinsic and extrinsic parameters using direct linear transformation (DLT), radial and tangential distortion models are introduced, and Levenberg-Marquardt joint nonlinear optimization is performed with the goal of minimizing reprojection error, significantly improving the camera calibration accuracy. This effectively removes the influence of inherent lens distortion on imaging geometry, making subsequent ray normalization, refraction compensation, and image correction based on calibration parameters more reliable, thus laying an accurate geometric foundation for precise mapping of the entire binocular system in complex underwater environments.

[0102] In this embodiment, the intrinsic and extrinsic parameters of the target are used to normalize the pixel coordinates of the two-dimensional corner points into a three-dimensional unit direction vector. Based on Snell's law, this vector is progressively corrected for the refraction process at the air-to-glass and glass-to-water interfaces, accurately reconstructing the propagation path of light in different media. By using the ideal pixel positions after refraction compensation again for joint optimization of camera parameters, not only is the image point offset caused by interface refraction compensated, but the adaptability and accuracy of the calibration parameters in the underwater environment are further improved, thus providing highly reliable geometric input for subsequent image correction and 3D reconstruction.

[0103] Step 103: Based on the target intrinsic and extrinsic parameters, decompose and align the relative rotation matrix and translation vector of the binocular camera to construct the correction matrix of the left and right images. Based on the correction matrix, correct the original image to obtain the monocular image sequence and the binocular image sequence.

[0104] In this embodiment, the binocular camera includes a left camera and a right camera; the step of decomposing and aligning the relative rotation matrix and translation vector of the binocular camera based on the target intrinsic and extrinsic parameters to construct a correction matrix for the left and right images, and correcting the original image based on the correction matrix to obtain a monocular image sequence and a binocular image sequence includes:

[0105] Based on the compensation parameters and the Rodriguez formula, the rotation matrix of the left camera relative to the right camera is decomposed into a first rotation matrix and a second rotation matrix.

[0106] The rotation transformation matrix is ​​constructed based on the Rodriguez formula and the translation vector of the left camera relative to the right camera;

[0107] The correction matrices for the left and right images are constructed based on the first rotation matrix, the second rotation matrix, and the rotation transformation matrix.

[0108] In this embodiment, based on the parameters obtained from the binocular camera calibration, the rotation matrix R of the left camera relative to the right camera is... LR Decomposed into two rotation matrices R left and R right :

[0109]

[0110] Where θ and m are rotation matrices R LR The corresponding rotation angle and rotation axis unit vector, M is an antisymmetric matrix constructed based on the rotation axis unit vector m, I is the identity matrix, R left and R right The rotation matrices for the left and right cameras are calculated using the Rodriguez formula. Each camera is rotated by half, making their optical axes parallel, but the baseline is still not parallel to the imaging plane. To align the imaging baseline with the epipolar direction, the translation vector T of the left camera relative to the right camera needs to be determined. LR Construct the rotation transformation matrix R align :

[0111] u = [1, 0, 0] T (twenty three)

[0112]

[0113] Where vector u is the target direction vector, and represents the desired translation vector T. LR Align with the horizontal direction of the imaging plane. It is T LR The angle between u and n is the angle of rotation required, n is the unit vector of the rotation axis, N is the antisymmetric matrix constructed based on the unit vector n of the rotation axis, and R align It is a rotation matrix constructed using the Rodriguez formula. Combined with R... left R right and R align The final correction matrix is ​​obtained as follows:

[0114]

[0115] in, and This is the inverse matrix of the intrinsic parameters of the left and right cameras. The final correction matrix is ​​then applied to reproject the left and right images:

[0116] I' L =R rect,L I L (29)

[0117] I' R =R rect,R I R (30)

[0118] Among them, I L and I R These are the original left and right images, I′ L and I′ RThis is the corrected image, in which the epipolar lines are horizontal and parallel. This correction step ensures the geometric consistency of the correspondence between the binocular images and is a key step in achieving high-precision underwater vision processing.

[0119] Please refer to Figure 2 and Figure 3 , Figure 2 This is an original image for polar line correction provided in an embodiment of the present invention. Figure 3 This is a polar correction result diagram provided in an embodiment of the present invention.

[0120] In this embodiment, firstly, using the refraction compensation parameters obtained in the previous steps, the overall rotation matrix of the right camera relative to the left camera is decomposed into two "half-rotation" matrices using the Rodriguez formula, corresponding to a half rotation for each of the left and right cameras, so that the optical axis directions of the two cameras tend to be consistent. Then, based on the translation vector between the left and right cameras, an alignment rotation matrix is ​​constructed using the Rodriguez formula to adjust the camera baseline to be aligned with the horizontal direction of the image plane. Next, the system combines the above two sets of rotation matrices with the intrinsic parameter matrices of their respective cameras to calculate the correction matrices for the left and right images. Finally, the correction matrices are applied to the original left and right images respectively, and resampling interpolation is performed to generate corrected monocular image sequences and binocular image sequences with horizontal and parallel epipolar lines. This step ensures the geometric consistency between the left and right image pairs, significantly improving the accuracy and efficiency of subsequent stereo matching and 3D reconstruction.

[0121] In this embodiment, a correction matrix is ​​generated by decomposing the binocular relative rotation matrix and translation vector obtained after refraction compensation. This aligns the epipolar lines of the left and right images to the same horizontal line and eliminates tilt and distortion between image pairs. This matrix compresses the search space for stereo matching, improving the speed and accuracy of ORB feature matching. Furthermore, it ensures a pixel-level one-to-one correspondence during the matching process, providing a reliable geometric guarantee for precise triangulation and binocular pose estimation, thereby significantly improving stereo matching efficiency and depth measurement accuracy.

[0122] Step 104: Extract and track ORB features from the monocular image sequence, and normalize the ORB features into a translation vector based on a preset depth median to obtain the monocular pose at a relative scale.

[0123] In this embodiment, after obtaining the corrected monocular image sequence, the OrientedFAST algorithm is sequentially applied to each frame for keypoint detection, and the Rotated BRIEF operator is used to generate descriptors for each keypoint. Subsequently, the system uses brute-force matching of descriptors and ratio testing to filter out high-confidence feature matching pairs between consecutive frames, and then uses the RANSAC method to remove erroneous matches to obtain a stable set of inliers. Next, the system queries the depth value of each inlier in the current frame in the 3D map or binocular depth measurement results, and calculates the median of all depth values ​​as the representative depth. Then, the translation vector estimated by matching previous and subsequent frames is divided by this median depth to achieve scale normalization of the translation vector. Through this normalization strategy, the system can maintain a consistent proportional relationship between the magnitude of the monocular translation vector and the scene depth, thereby significantly suppressing scale drift while ensuring high-frequency updates, generating a monocular pose sequence with a relatively consistent scale, and providing reliable relative motion information for subsequent loop closure correction and global optimization.

[0124] In this embodiment, after obtaining the corrected monocular image sequence, the OrientedFAST algorithm is sequentially applied to each frame for keypoint detection, and the Rotated BRIEF operator is used to generate descriptors for each keypoint. Subsequently, the system uses brute-force matching of descriptors and ratio testing to filter out high-confidence feature matching pairs between consecutive frames, and then uses the RANSAC method to remove erroneous matches to obtain a stable set of inliers. Next, the system queries the depth value of each inlier in the current frame in the 3D map or binocular depth measurement results, and calculates the median of all depth values ​​as the representative depth. Then, the translation vector estimated by matching previous and subsequent frames is divided by this median depth to achieve scale normalization of the translation vector. Through this normalization strategy, the system can maintain a consistent proportional relationship between the magnitude of the monocular translation vector and the scene depth, thereby significantly suppressing scale drift while ensuring high-frequency updates, generating a monocular pose sequence with a relatively consistent scale, and providing reliable relative motion information for subsequent loop closure correction and global optimization.

[0125] Step 105: Perform stereo matching on the ORB features of the binocular image sequence based on epipolar constraints to obtain disparity, and perform triangulation based on the calibrated baseline and focal length to obtain the three-dimensional coordinates of the feature points, and obtain the binocular pose at the absolute scale based on the three-dimensional coordinates.

[0126] In this embodiment, the binocular vision mode processing function is started in parallel in the background. For each pair of corrected left and right images, the system first opens a small search window in the same scan line of the right image for each ORB feature point extracted in the left image, based on the horizontal and parallel characteristics of the epipolar lines. It then uses the Hamming distance of the BRIEF descriptor to quickly match the best feature correspondence, thereby obtaining the pixel column coordinate difference (i.e., parallax) of the feature point in the left and right images. Subsequently, the system calls the previously calibrated camera baseline length and equivalent focal length to convert the parallax into a depth value with physical units at the meter or millimeter level, and further calculates the three-dimensional coordinates of the feature point in the camera coordinate system. Finally, based on all three-dimensional points in the current frame and the corresponding three-dimensional points in the reference frame or local map, the RANSAC-PnP algorithm is used to estimate the pose of the binocular camera. The pose is then filtered and verified using the number of inliers and the reprojection error threshold, outputting a stable and reliable binocular pose estimation result. This not only provides absolute scale information of the scene but also lays a solid three-dimensional foundation for subsequent scale correction and global optimization.

[0127] Step 106: Construct a seven-degree-of-freedom similarity transformation matrix for the monocular pose and binocular pose based on the least squares method. Using the binocular pose as a reference, perform alignment optimization on the monocular pose based on the similarity transformation matrix to output the target 3D map.

[0128] In this embodiment, the step of constructing a seven-DOF similarity transformation matrix for the monocular and binocular poses based on the least squares method, using the binocular pose as a reference, and performing alignment optimization on the monocular pose based on the similarity transformation matrix to output a target 3D map includes:

[0129] The monocular and binocular poses are aligned and scaled based on timestamps to generate pose correspondences.

[0130] The rotational, translational, and scaling degrees of freedom parameters are estimated using the least squares method, and a seven-degree-of-freedom similarity transformation matrix is ​​constructed based on these parameters.

[0131] The similarity transformation matrix is ​​applied to the monocular pose, and global alignment is performed using the binocular pose as the scale and orientation reference to obtain the target monocular trajectory.

[0132] Global beam adjustment is performed based on the target monocular trajectory to output a 3D map of the target.

[0133] In this embodiment, after loop closure detection triggers the scale correction mechanism, each frame in the stereo trajectory sequence is first traversed. By comparing timestamps, the closest temporally matched monocular trajectory frame is precisely matched, constructing a set of high-precision monocular and stereo pose correspondences. Next, these pose pairs are input into the least squares solution module, which uses rotation matrix, translation vector, and scale factor as unknown parameters to jointly estimate a seven-parameter similarity transformation matrix containing three rotational degrees of freedom, three translational degrees of freedom, and one scale degree of freedom. Subsequently, this similarity transformation matrix is ​​used to map the entire monocular trajectory, ensuring its precise alignment with the stereo trajectory in orientation and position. Global scale correction is then performed based on the actual physical scale represented by the stereo trajectory. Finally, the corrected monocular trajectory and its corresponding 3D map points are fed into the global bundle adjustment process. By nonlinearly minimizing reprojection error, the camera pose and map structure are further optimized, thereby generating a 3D underwater map with consistent scale, high precision, and global consistency.

[0134] In this embodiment, based on the initial estimation of monocular and binocular poses, a seven-DOF similarity transformation is constructed by estimating rotation, translation, and scale parameters through timestamp-based frame pair association and least squares method, respectively. This transformation is then applied to the entire monocular trajectory, using the binocular trajectory as the scale and orientation reference to achieve global alignment. Subsequently, global bundle adjustment is further performed to jointly optimize camera pose and 3D map points. This not only eliminates scale drift in the monocular view but also suppresses accumulated errors through global optimization, ultimately outputting a 3D underwater map with realistic physical scale, high consistency, and high accuracy, greatly enhancing the overall reliability and map quality of the system.

[0135] Please refer to Figure 4 , Figure 4 This is a schematic diagram of a monocular and binocular cooperative localization and scale correction process provided in an embodiment of the present invention.

[0136] In this embodiment, after completing geometric correction (including underwater refraction compensation and epipolar correction), the output image sequence is first subjected to illumination enhancement processing. Specifically, Gaussian blur is applied to each frame of the corrected image to estimate the background illumination distribution. Then, details are highlighted by difference or ratio calculation between the original image and the background image. Finally, global / local contrast adjustment is performed on the enhancement result by combining contrast stretching or histogram equalization to significantly improve the clarity and feature detectability of the underwater image.

[0137] Based on this, the system operates primarily in monocular vision mode. As shown in the "Monocular Trajectory" figure, ORB features are extracted and tracked sequentially for each continuously calibrated monocular frame: first, Oriented FAST is used to detect corner points, then Rotated BRIEF is used to calculate descriptors, and reliable inter-frame feature correspondences are obtained through brute-force matching, ratio testing, and RANSAC to eliminate mismatches. Simultaneously, the depth value of each corresponding feature in the 3D map or binocular depth measurement results is queried, and the median of all depths is used to normalize the translation vector of the current frame, making the magnitude of the vector proportional to the median depth, thereby obtaining a monocular pose with relative scale consistency.

[0138] In parallel, the binocular image pairs are processed in the background, as shown in the "Binocular Trajectory" figure. For each ORB feature point in the left image, based on the geometric constraints of horizontal and parallel epipolar lines, fast stereo matching is performed in the corresponding scan line in the right image, and disparity is calculated. Then, combined with the baseline length and focal length obtained from calibration, the three-dimensional coordinates of the point are directly calculated through triangulation to obtain depth information with real physical units. Finally, the pose of the binocular camera is estimated using methods such as RANSAC-PnP to generate a binocular pose sequence.

[0139] Once the system detects a loop closure and triggers scale correction using the bag-of-words model, it enters the "mono- and binocular trajectory merging stage" shown in the diagram. First, based on timestamps, the binocular trajectory frames are matched one-to-one with their corresponding monocular trajectory frames to construct a high-precision pose correspondence. Then, these correspondences are input into the least squares solution module to jointly estimate a seven-DOF similarity transformation matrix that includes rotation, translation, and scale. Finally, this transformation matrix is ​​applied to the entire monocular trajectory to precisely align it with the binocular trajectory in terms of orientation, position, and scale, and immediately triggers global bundle adjustment, outputting a target 3D map that fuses the true absolute scale and has high consistency accuracy.

[0140] In this embodiment, the ORB feature correspondence between the current frame and the previously selected keyframe is first used in monocular mode to generate an initial sparse 3D map through classical triangulation. In each subsequent frame, the observation count of new map points and associated keyframe information are continuously added to accumulate a rich sparse point cloud.

[0141] In this embodiment, monocular and binocular modes operate in parallel. During alignment, information fusion is performed between the binocular and monocular modes. Specifically, ORB feature stereo matching is performed on each pair of epipolar-corrected left and right images to calculate disparity values. Based on the calibrated focal length and baseline length, the physical 3D coordinates of each matching point are recovered through triangulation. These new map points with absolute scale are incrementally inserted into the local map. To maintain real-time performance and local accuracy, the system periodically triggers local bundle adjustment in the local map region. This adjustment is applied only to a set of keyframes within the current field of view and their co-observed map points. By minimizing reprojection errors, the camera pose and map point coordinates are simultaneously optimized, and points that have not been observed for a long time or have excessive errors are automatically removed to improve map quality. When loop closure detection identifies a loop, the system initiates global map optimization, including loop closure bundle adjustment and pose map optimization. The map points and keyframe poses are rearranged globally to ensure global scale, consistency, and geometric continuity. Finally, based on the optimized graph structure information, the system adaptively selects and retains representative keyframes and dynamically manages the topological relationships between local and global map points, thereby achieving stable and efficient mapping of large-scale underwater environments.

[0142] Please refer to Figure 5 , Figure 5 This is another flowchart illustrating an underwater refraction-compensated monocular and binocular cooperative localization and mapping method provided in an embodiment of the present invention.

[0143] In this embodiment, checkerboard calibration board images captured from multiple angles in an underwater environment and camera intrinsic parameters pre-obtained from an air environment are used as initial inputs. In each underwater image, the pixel coordinates (u,v) of the checkerboard corner points are first detected and extracted; then, using the intrinsic parameter matrix obtained from the air environment calibration, these pixel coordinates are mapped to the normalized image plane to obtain the corresponding unit ray direction vector d. air Next, according to Snell's law, first consider d... air Refraction vector correction is performed at the glass-air interface, followed by a second refraction correction at the water-glass interface to accurately characterize the deflection behavior of light as it passes through the two media layers of the camera's waterproof housing and the water. After these two refraction compensations, the final corrected direction vector is reprojected onto the pixel plane to generate ideal checkerboard corner pixel coordinates (u,v) that have eliminated underwater refraction effects. Finally, using these ideal pixel coordinates and their corresponding world coordinate pairs as input, the system performs joint optimization and calibration of the binocular camera's intrinsic and extrinsic parameters to obtain accurate binocular camera intrinsic and extrinsic parameters suitable for underwater environments. Based on this, imaging distortion caused by medium refraction is effectively eliminated, laying a reliable foundation for subsequent high-precision underwater stereo matching and 3D reconstruction.

[0144] In this embodiment, by extracting the corner points of the checkerboard pattern and calibrating the intrinsic and extrinsic parameters of the binoculars, a solid foundation is provided for subsequent refraction compensation. Two vector corrections of the light direction using Snell's law effectively restore the true underwater light path, significantly reducing refraction errors. Epipolar correction is then applied to the corrected left and right images to ensure geometric consistency in stereo matching. Finally, the monocular and binocular poses are fused and a seven-DOF similarity transformation is applied for global alignment. This not only effectively solves the uncertainty of monocular scale but also, by introducing the true binocular scale, achieves high-precision and robust 3D mapping. By integrating key steps such as camera pre-calibration, refraction compensation, image correction, monocular tracking, binocular depth measurement, and scale alignment into a unified process, the geometric distortion and monocular scale drift caused by medium refraction during underwater imaging are systematically solved, improving the accuracy of underwater visual positioning and mapping.

[0145] In this embodiment, an optical refraction model based on a three-medium system of water, glass, and air is constructed to accurately compensate for the refraction effect during underwater imaging, significantly improving the accuracy of camera parameter calibration. This model effectively corrects geometric distortions caused by refraction, providing a more reliable input basis for subsequent 3D reconstruction and pose estimation, and significantly reducing the impact of calibration errors on the overall system performance.

[0146] In this embodiment, a monocular and binocular vision collaborative mechanism is employed. During SLAM, the subject's pose estimation and mapping tasks are completed using monocular vision mode. When the system detects a loop closure event, the binocular vision module is activated to acquire absolute scale information, and the monocular trajectory is corrected using trajectory alignment and scale correction methods. This collaborative strategy effectively suppresses scale drift caused by monocular estimation while maintaining front-end computational efficiency, thus enhancing the mapping capabilities. Figure 1 Consistency and continuity of pose estimation.

[0147] In this embodiment, by deeply integrating the refraction compensation model with the monocular and binocular collaborative correction mechanism, the present invention significantly improves the adaptability and mapping quality of the SLAM system in complex underwater environments. The system exhibits good environmental adaptability and localization robustness in dynamic underwater scenarios, ensuring the stability and reliability of mapping during critical mission execution.

[0148] In a second aspect, the present invention also provides an underwater refraction-compensated monocular and binocular cooperative positioning and mapping device, comprising: an intrinsic and extrinsic parameter acquisition module, an optimization module, a correction module, a monocular pose estimation module, a binocular pose estimation module, and a mapping module;

[0149] The intrinsic and extrinsic parameter acquisition module is used to acquire chessboard calibration board images, extract the corner pixel coordinates of the chessboard calibration board images, and acquire the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the images.

[0150] The optimization module is used to obtain the compensation parameters of the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, construct a global optimization model based on the compensation parameters, and optimize the global optimization model based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters; the compensation parameters are intrinsic and extrinsic parameters after refraction compensation.

[0151] The correction module is used to decompose and align the relative rotation matrix and translation vector of the binocular camera based on the intrinsic and extrinsic parameters of the target, construct the correction matrix of the left and right images, correct the original image based on the correction matrix, and obtain the monocular image sequence and the binocular image sequence.

[0152] The monocular pose estimation module is used to extract and track ORB features from the monocular image sequence, and to normalize and translate the ORB features into a translation vector based on a preset depth median to obtain the monocular pose at a relative scale.

[0153] The binocular pose estimation module is used to perform stereo matching on the ORB features of the binocular image sequence based on epipolar constraints to obtain disparity, and to perform triangulation based on the calibrated baseline and focal length to obtain the three-dimensional coordinates of the feature points, and to obtain the binocular pose at the absolute scale based on the three-dimensional coordinates.

[0154] The mapping module is used to construct a seven-degree-of-freedom similarity transformation matrix for the monocular pose and binocular pose based on the least squares method, and to perform alignment optimization on the monocular pose based on the similarity transformation matrix using the binocular pose as a reference, thereby outputting a target 3D map.

[0155] In this embodiment, the intrinsic and extrinsic parameter acquisition module is used to acquire a chessboard calibration board image, extract the corner pixel coordinates of the chessboard calibration board image, and acquire the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the image, including:

[0156] Acquire a chessboard calibration board image, extract initial corner pixel coordinates based on the chessboard calibration board image, perform anomaly correction on the initial corner pixel coordinates, and obtain the corner pixel coordinates;

[0157] The initial intrinsic and extrinsic parameters are obtained based on the corner pixel coordinates and the direct linear transformation method. The initial intrinsic and extrinsic parameters include an initial intrinsic parameter matrix and an initial extrinsic parameter matrix.

[0158] In this embodiment, the optimization module is used to obtain compensation parameters for the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, construct a global optimization model based on the compensation parameters, and optimize the global optimization model based on a preset optimization algorithm to obtain target intrinsic and extrinsic parameters, including:

[0159] Based on the target intrinsic and extrinsic parameters, the corner pixel coordinates are normalized and transformed to obtain a first direction vector; the first direction vector is the unit ray direction vector in the air medium.

[0160] Based on Snell's law, the direction of light rays in the first direction vector is vector-corrected during the two refraction processes from air to glass and from glass to water to obtain compensation parameters and ideal pixel coordinates of corner points;

[0161] Based on the compensation parameters, preset radial distortion coefficients, and tangential distortion coefficients, a global optimization model is constructed. With the goal of minimizing the reprojection error, for each corner point's ideal pixel coordinates, the Levenberg-Marquardt algorithm is used to perform joint nonlinear optimization processing on the global optimization model to obtain the target's intrinsic and extrinsic parameters and target distortion parameters.

[0162] In this embodiment, the binocular camera includes a left camera and a right camera; the correction module is used to decompose and align the relative rotation matrix and translation vector of the binocular camera based on the target intrinsic and extrinsic parameters, construct correction matrices for the left and right images, correct the original image based on the correction matrices, and obtain monocular image sequences and binocular image sequences, including:

[0163] Based on the target intrinsic and extrinsic parameters and the Rodriguez formula, the rotation matrix of the left camera relative to the right camera is decomposed into a first rotation matrix and a second rotation matrix;

[0164] The rotation transformation matrix is ​​constructed based on the Rodriguez formula and the translation vector of the left camera relative to the right camera;

[0165] The correction matrices for the left and right images are constructed based on the first rotation matrix, the second rotation matrix, and the rotation transformation matrix.

[0166] In this embodiment, the mapping module is used to construct a seven-degree-of-freedom similarity transformation matrix for the monocular and binocular poses based on the least squares method, and to perform alignment optimization on the monocular pose based on the similarity transformation matrix using the binocular pose as a reference, thereby outputting a target 3D map, including:

[0167] The monocular and binocular poses are aligned and scaled based on timestamps to generate pose correspondences.

[0168] The rotational, translational, and scaling degrees of freedom parameters are estimated using the least squares method, and a seven-degree-of-freedom similarity transformation matrix is ​​constructed based on these parameters.

[0169] The similarity transformation matrix is ​​applied to the monocular pose, and global alignment is performed using the binocular pose as the scale and orientation reference to obtain the target monocular trajectory.

[0170] Global beam adjustment is performed based on the target monocular trajectory to output a 3D map of the target.

[0171] In this embodiment of the invention, a terminal device is also provided, including a processor, a memory, and a computer program stored in the memory and configured to be executed by the processor. When the processor executes the computer program, it implements the above-described underwater refraction compensation monocular and binocular cooperative localization and mapping method.

[0172] In this embodiment of the invention, a computer-readable storage medium is also provided, which includes a stored computer program, wherein, when the computer program is running, it controls the device where the computer-readable storage medium is located to execute the above-described underwater refraction compensation monocular and binocular cooperative localization and mapping method.

[0173] For example, a computer program can be divided into one or more modules, one or more of which are stored in memory and executed by a processor to perform the present invention. The one or more modules can be a series of computer program instruction segments capable of performing a specific function, which describe the execution process of the computer program in a terminal device.

[0174] The terminal device can be a desktop computer, laptop, handheld computer, or cloud server, etc. The terminal device may include, but is not limited to, a processor, memory, and display. Those skilled in the art will understand that the above components are merely examples of terminal devices and do not constitute a limitation on the terminal device. It may include more or fewer components than the specified components, or a combination of certain components, or different components. For example, the terminal device may also include input / output devices, network access devices, buses, etc.

[0175] The processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is the control center of the terminal device, connecting all parts of the terminal device through various interfaces and lines.

[0176] Memory can be used to store computer programs and / or modules. The processor implements various functions of the terminal device by running or executing the computer programs and / or modules stored in the memory, and by accessing data stored in the memory. Memory can mainly include a program storage area and a data storage area. The program storage area can store the operating system, application programs required for at least one function (such as sound playback, text conversion, etc.), etc.; the data storage area can store data created based on the use of the mobile phone (such as audio data, text message data, etc.). In addition, memory can include high-speed random access memory, and can also include non-volatile memory, such as hard disks, RAM, plug-in hard disks, smart media cards (SMC), secure digital cards (SD cards), flash cards, at least one disk storage device, flash memory device, or other volatile solid-state storage devices.

[0177] The module for monocular and binocular cooperative localization and mapping based on underwater refraction compensation, if implemented as a software functional unit and sold or used as an independent product, can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the above embodiments of the present invention can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. Those skilled in the art can understand and implement this without any inventive effort.

[0178] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention for those skilled in the art.

Claims

1. A method for underwater refraction-compensated monocular and binocular cooperative localization and mapping, characterized in that, include: Acquire images of a chessboard calibration board, extract the corner pixel coordinates of the chessboard calibration board images; and obtain the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the images. The compensation parameters of the binocular camera are obtained based on the initial intrinsic and extrinsic parameters and Snell's law. A global optimization model is constructed based on the compensation parameters, and the global optimization model is optimized based on a preset optimization algorithm to obtain the target intrinsic and extrinsic parameters. This includes: normalizing the corner pixel coordinates based on the initial intrinsic and extrinsic parameters to obtain a first direction vector; the first direction vector is the unit ray direction vector in the air medium; vector correction is performed on the ray direction of the first direction vector in the two refraction processes from air to glass and from glass to water based on Snell's law to obtain compensation parameters and ideal pixel coordinates of the corner points; a global optimization model is constructed based on the compensation parameters, preset radial distortion coefficients, and tangential distortion coefficients, with the goal of minimizing reprojection error. For each ideal pixel coordinate of the corner points, the Levenberg-Marquardt algorithm is used to perform joint nonlinear optimization processing on the global optimization model to obtain the target intrinsic and extrinsic parameters and target distortion parameters; the compensation parameters are the intrinsic and extrinsic parameters after refraction compensation. Based on the target intrinsic and extrinsic parameters, the relative rotation matrix and translation vector of the binocular camera are decomposed and aligned to construct the correction matrix of the left and right images. The original image is then corrected based on the correction matrix to obtain the monocular image sequence and the binocular image sequence. ORB features are extracted and tracked from the monocular image sequence, and the ORB features are normalized and translated into a vector based on a preset depth median to obtain the monocular pose at a relative scale. Based on epipolar constraints, stereo matching is performed on the ORB features of the binocular image sequence to obtain disparity. Triangulation is then performed based on the calibrated baseline and focal length to obtain the three-dimensional coordinates of the feature points. Finally, the binocular pose at absolute scale is obtained based on the three-dimensional coordinates. A seven-DOF similarity transformation matrix is ​​constructed for the monocular and binocular poses using the least squares method. Based on the binocular pose, the monocular pose is aligned and optimized using the similarity transformation matrix to output a target 3D map.

2. The underwater refraction-compensated monocular and binocular cooperative localization and mapping method as described in claim 1, characterized in that, The process involves acquiring an image of the chessboard calibration board and extracting the pixel coordinates of the corner points of the chessboard calibration board image. The initial intrinsic and extrinsic parameters of the stereo camera are obtained based on the direct linear transformation method and the pixel coordinates of the image corner points, including: Acquire a chessboard calibration board image, extract initial corner pixel coordinates based on the chessboard calibration board image, perform anomaly correction on the initial corner pixel coordinates, and obtain the corner pixel coordinates; The initial intrinsic and extrinsic parameters are obtained based on the corner pixel coordinates and the direct linear transformation method. The initial intrinsic and extrinsic parameters include an initial intrinsic parameter matrix and an initial extrinsic parameter matrix.

3. The underwater refraction-compensated monocular and binocular cooperative localization and mapping method as described in claim 2, characterized in that, The binocular camera includes a left camera and a right camera; the process of decomposing and aligning the relative rotation matrix and translation vector of the binocular camera based on the target intrinsic and extrinsic parameters to construct correction matrices for the left and right images, and correcting the original images based on the correction matrices to obtain monocular image sequences and binocular image sequences includes: Based on the target intrinsic and extrinsic parameters and the Rodriguez formula, the rotation matrix of the left camera relative to the right camera is decomposed into a first rotation matrix and a second rotation matrix; The rotation transformation matrix is ​​constructed based on the Rodriguez formula and the translation vector of the left camera relative to the right camera; The correction matrices for the left and right images are constructed based on the first rotation matrix, the second rotation matrix, and the rotation transformation matrix.

4. The underwater refraction-compensated monocular and binocular cooperative localization and mapping method as described in claim 3, characterized in that, The process involves constructing a seven-DOF similarity transformation matrix for the monocular and binocular poses using the least squares method, then using the binocular pose as a reference, and performing alignment optimization on the monocular pose based on the similarity transformation matrix to output a target 3D map, including: The monocular and binocular poses are aligned and scaled based on timestamps to generate pose correspondences. The rotational, translational, and scaling degrees of freedom parameters are estimated using the least squares method, and a seven-degree-of-freedom similarity transformation matrix is ​​constructed based on these parameters. The similarity transformation matrix is ​​applied to the monocular pose, and global alignment is performed using the binocular pose as the scale and orientation reference to obtain the target monocular trajectory. Global beam adjustment is performed based on the target monocular trajectory to output a 3D map of the target.

5. An underwater refraction-compensated monocular and binocular cooperative positioning and mapping device, characterized in that, include: The system includes modules for acquiring intrinsic and extrinsic parameters, optimization, correction, monocular pose estimation, binocular pose estimation, and mapping. The intrinsic and extrinsic parameter acquisition module is used to acquire chessboard calibration board images, extract the corner pixel coordinates of the chessboard calibration board images, and acquire the initial intrinsic and extrinsic parameters of the stereo camera based on the direct linear transformation method and the corner pixel coordinates of the images. The optimization module is used to obtain compensation parameters for the binocular camera based on the initial intrinsic and extrinsic parameters and Snell's law, construct a global optimization model based on the compensation parameters, and optimize the global optimization model based on a preset optimization algorithm to obtain target intrinsic and extrinsic parameters. This includes: normalizing the corner pixel coordinates based on the initial intrinsic and extrinsic parameters to obtain a first direction vector; the first direction vector is the unit ray direction vector in air; performing vector correction on the ray direction of the first direction vector during the two refraction processes from air to glass and from glass to water based on Snell's law to obtain compensation parameters and ideal corner pixel coordinates; constructing a global optimization model based on the compensation parameters, preset radial distortion coefficients, and tangential distortion coefficients, with the goal of minimizing reprojection error; and for each ideal corner pixel coordinate, using the Levenberg-Marquardt algorithm to perform joint nonlinear optimization processing on the global optimization model to obtain target intrinsic and extrinsic parameters and target distortion parameters; the compensation parameters are intrinsic and extrinsic parameters after refraction compensation. The correction module is used to decompose and align the relative rotation matrix and translation vector of the binocular camera based on the intrinsic and extrinsic parameters of the target, construct the correction matrix of the left and right images, correct the original image based on the correction matrix, and obtain the monocular image sequence and the binocular image sequence. The monocular pose estimation module is used to extract and track ORB features from the monocular image sequence, and to normalize and translate the ORB features into a translation vector based on a preset depth median to obtain the monocular pose at a relative scale. The binocular pose estimation module is used to perform stereo matching on the ORB features of the binocular image sequence based on epipolar constraints to obtain disparity, and to perform triangulation based on the calibrated baseline and focal length to obtain the three-dimensional coordinates of the feature points, and to obtain the binocular pose at the absolute scale based on the three-dimensional coordinates. The mapping module is used to construct a seven-degree-of-freedom similarity transformation matrix for the monocular pose and binocular pose based on the least squares method, and to perform alignment optimization on the monocular pose based on the similarity transformation matrix using the binocular pose as a reference, thereby outputting a target 3D map.

6. The underwater refraction-compensated monocular and binocular cooperative positioning and mapping device as described in claim 5, characterized in that, The internal and external parameter acquisition module is used to acquire chessboard calibration board images and extract the corner pixel coordinates of the chessboard calibration board images; The initial intrinsic and extrinsic parameters of the stereo camera are obtained based on the direct linear transformation method and the pixel coordinates of the image corner points, including: Acquire a chessboard calibration board image, extract initial corner pixel coordinates based on the chessboard calibration board image, perform anomaly correction on the initial corner pixel coordinates, and obtain the corner pixel coordinates; The initial intrinsic and extrinsic parameters are obtained based on the corner pixel coordinates and the direct linear transformation method. The initial intrinsic and extrinsic parameters include an initial intrinsic parameter matrix and an initial extrinsic parameter matrix.

7. The underwater refraction-compensated monocular and binocular cooperative positioning and mapping device as described in claim 6, characterized in that, The binocular camera includes a left camera and a right camera; the correction module is used to decompose and align the relative rotation matrix and translation vector of the binocular camera based on the target intrinsic and extrinsic parameters, construct correction matrices for the left and right images, correct the original images based on the correction matrices, and obtain monocular image sequences and binocular image sequences, including: Based on the target intrinsic and extrinsic parameters and the Rodriguez formula, the rotation matrix of the left camera relative to the right camera is decomposed into a first rotation matrix and a second rotation matrix; The rotation transformation matrix is ​​constructed based on the Rodriguez formula and the translation vector of the left camera relative to the right camera; The correction matrices for the left and right images are constructed based on the first rotation matrix, the second rotation matrix, and the rotation transformation matrix.

8. The underwater refraction-compensated monocular and binocular cooperative positioning and mapping device as described in claim 7, characterized in that, The mapping module is used to construct a seven-DOF similarity transformation matrix for the monocular and binocular poses based on the least squares method, and to perform alignment optimization on the monocular pose based on the similarity transformation matrix using the binocular pose as a reference, outputting a target 3D map, including: The monocular and binocular poses are aligned and scaled based on timestamps to generate pose correspondences. The rotational, translational, and scaling degrees of freedom parameters are estimated using the least squares method, and a seven-degree-of-freedom similarity transformation matrix is ​​constructed based on these parameters. The similarity transformation matrix is ​​applied to the monocular pose, and global alignment is performed using the binocular pose as the scale and orientation reference to obtain the target monocular trajectory. Global beam adjustment is performed based on the target monocular trajectory to output a 3D map of the target.

Images