Method for estimating target's 3D pose using underwater monocular vision

By improving the YOLOv3 neural network and refraction compensation model, the problem that monocular cameras are difficult to estimate the target three-dimensional pose underwater is solved, and high-precision underwater target detection and autonomous operation capabilities are achieved.

CN114549629BActive Publication Date: 2024-11-26OCEAN UNIV OF CHINA
View PDF 3 Cites 0 Cited by

Patent Information

Application Number
CN202210166778.4
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-02-23
Publication Date
2024-11-26
Estimated Expiration
2042-02-23

AI Technical Summary

Technical Problem

In the existing underwater detection methods, it is difficult for a monocular camera to directly obtain target depth and attitude information, which makes it difficult to estimate target position information. The detection accuracy in complex and changeable underwater environments is low, which cannot meet the needs of autonomous operations.

Method used

Using the improved YOLOv3 neural network model, a refraction compensation model is constructed by adding recombinant layers and example normalization, and a priori information is used to establish a pose estimation model to solve the three-dimensional pose of the target object.

Benefits of technology

It improves the accuracy of underwater target detection and the autonomy of autonomous operations, can accurately detect targets in complex environments, meet the pose estimation needs of practical applications, and reduces the consumption of computing resources.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114549629B_ABST
    Figure CN114549629B_ABST
Patent Text Reader

Abstract

The present invention relates to a method for estimating the three-dimensional pose of a target by underwater monocular vision. The method adopts an improved YOLOv3 neural network to construct a neural network model and train the neural network model, uses the trained model to detect the target in the target image, and obtains the pixel coordinates of four feature points on the target; calculates the compensation coefficients before and after refraction according to the constructed refraction compensation model, and obtains the pixel coordinates of the four feature points after compensation according to the compensation coefficients before and after refraction; establishes a pose estimation model based on the pixel coordinates of the four feature points after compensation and in combination with known prior information; solves the pose estimation model to obtain the three-dimensional pose of the target relative to the camera. The present invention is used to estimate the three-dimensional pose of underwater regular targets, meets the requirements of accurate detection of targets in complex and changeable underwater environments, and has high pose estimation accuracy.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of underwater detection technology, and in particular relates to a method for estimating the three-dimensional position and posture of a target by using underwater monocular vision. Background Art

[0002] During the autonomous operation of underwater robots, a manipulator is usually used to grab the target. At this time, it is particularly important to obtain information such as the target posture. At present, underwater robots use binocular cameras to measure the three-dimensional posture of underwater targets. However, binocular cameras have a small field of view and a large size, and are not portable during the operation of underwater robots. Monocular cameras have the characteristics of simple structure and large field of view, and are more suitable for underwater operations. However, due to imaging limitations, monocular cameras cannot directly obtain target depth and posture information. Therefore, if monocular cameras are used, there is a problem of difficulty in estimating target posture information. Target marking or human intervention is still required, and the degree of autonomy in the application is low. Due to the complex and changeable underwater environment, imaging has problems such as low contrast, and the target features are reduced. It is difficult for existing commonly used detection methods to accurately detect the target and subsequent work cannot be carried out. At the same time, due to the refraction between different media in underwater imaging, the usual estimation method cannot be applied underwater. In addition, since the posture of underwater targets is uncertain, most of them are three-dimensional rotations, and the existing posture estimation methods only have a good estimation effect on the two-dimensional posture of the target perpendicular to the surface, it is difficult to meet the needs of real applications. Summary of the invention

[0003] In view of the above-mentioned problems in the target posture estimation in the existing underwater detection process, such as low target detection accuracy and poor estimation precision, the present invention provides a method for estimating the three-dimensional posture of a target using underwater monocular vision, which is used to estimate the three-dimensional posture of underwater regular targets and meets the requirements of accurate detection of targets in complex and changeable underwater environments, and has high posture estimation accuracy.

[0004] In order to achieve the above object, the present invention provides a method for estimating the three-dimensional position and posture of a target by underwater monocular vision, and the specific steps are as follows:

[0005] Steps for building a neural network model: Add a reorganization layer to the backbone network of the YOLOv3 neural network instead of the convolution layer and pooling layer, introduce instance normalization to replace the batch normalization in the early layers of the YOLOv3 neural network, and build a neural network model;

[0006] Steps for training the neural network model: training the neural network model to obtain a trained neural network model;

[0007] Target detection step: Detect the target in the acquired underwater target image through the trained neural network model, obtain the prediction frame of the four corner points of the target, take the midpoint pixel coordinates of the prediction frame as the feature point coordinates of the target, and obtain the pixel coordinates of the four feature points on the target;

[0008] Steps for constructing a refraction compensation model: construct a refraction compensation model according to the actual position of the target object, the refraction position, and the projection position after refraction in water;

[0009] Refraction compensation step: calculate the compensation coefficients before and after refraction according to the refraction compensation model, and calculate the pixel coordinates of the four feature points after compensation according to the compensation coefficients before and after refraction;

[0010] Steps to construct the pose estimation model: According to the compensated pixel coordinates of the four feature points, combined with known prior information, a world coordinate system is established on the target object to obtain the 3D coordinates of the four feature points in the real world coordinate system, and a camera coordinate system is established with the direction perpendicular to the camera plane as the Z axis. The four feature points establish a conversion relationship between the world coordinate system and the camera coordinate system. This conversion relationship is the constructed pose estimation model;

[0011] Three-dimensional pose estimation steps: Solve the pose estimation model to obtain the transformation relationship between the world coordinate system of the target object and the camera coordinate system. This transformation relationship is the three-dimensional pose of the target object relative to the camera.

[0012] Preferably, in the step of constructing a neural network model, the reorganization layer is to disassemble 4 pixels in an image block of size 2*2 on each channel, and then rearrange them into a structural layer of image blocks of size 1*1 for 4 channels.

[0013] Preferably, in the step of constructing a neural network model, the instance normalization is represented as:

[0014]

[0015] In the formula, y ncij is the normalized value, x ncij is the ncijth element of the feature map in a layer, where i and j are spatial dimensions, c is the feature channel, and n is the nth image in the batch; μ nc is the average difference in the cth feature channel of the nth image, σ nc is the variance in the cth feature channel of the nth image, ∈ is a constant, H0 is the height of the feature map, and W0 is the width of the feature map.

[0016] Preferably, in the step of constructing the neural network model, the loss function of the YOLOv3 neural network is improved and converted into the loss of the diagonal of the region box, and the loss function of the neural network model is expressed as:

[0017]

[0018] In the formula, C m is the target category; P m is the probability of belonging to a certain category; Is the jth anchorbox of the ith grid responsible for predicting the target? If so, it is 1, otherwise it is 0; w0 is the width of the prediction box, h0 is the height of the prediction box; w0' is the width of the real box, h0' is the height of the real box, are all grids in the feature map, s is the side length of the feature map, To traverse the prediction box corresponding to each grid, x i and i is the coordinate of the prediction box of the i-th grid in the feature map, x i ′ and y i ′ is the coordinate of the real box of the i-th grid in the feature map, λ coord is coord The weight coefficient of the corresponding item, λ noobj is noobj The weight coefficient of the corresponding item, Whether the j-th anchor box of the ith grid is not responsible for predicting the target, if not, it is 1, otherwise it is 0. is the category of the predicted box of the jth grid in the feature map, is the category of the real box of the jth grid in the feature map, is the confidence of the prediction box of the jth grid in the feature map, is the confidence of the ground-truth box of the jth grid in the feature map.

[0019] Preferably, in the step of constructing the neural network model, the large branch of the large, medium and small branches in the YOLOv3 neural network is also pruned, leaving only the medium and small branches for detecting medium and small targets.

[0020] Preferably, in the step of training the neural network model, before training the neural network model, the data set of the area to be detected is first annotated, and then the size of the anchor box is set, and the optimal 9 clusters of anchor boxes are obtained by clustering, and then the network model training is applied.

[0021] Preferably, the method for labeling the dataset of the detection area is as follows: (1) rotating the existing dataset image by a random angle and randomly adjusting the image brightness within a set range; (2) performing adaptive histogram equalization on the adjusted image, randomly adjusting the color contrast threshold and the grid size for pixel equalization within a set range, obtaining dataset samples under different contrast and brightness conditions, and using labelimg2 to label the dataset after the samples meet the set requirements.

[0022] Preferably, in the step of constructing the refraction compensation model, the specific steps of constructing the refraction supplement model are:

[0023] Convert the pixel coordinate system to the image coordinate system:

[0024]

[0025] Where x0 is the x-axis coordinate in the image coordinate system, i.e. the camera coordinate system; y0 is the y-axis coordinate in the image coordinate system, i.e. the camera coordinate system; u and v are the pixel coordinates in the pixel coordinate system, respectively; u0 and v0 are the positions of the origin of the image coordinate system in the pixel coordinate system. is the pixel size on the x-axis in the image coordinate system, is the pixel size on the y-axis in the image coordinate system;

[0026] The coordinates of the refraction point in the camera coordinate system are calculated by formula (4), which is expressed as:

[0027]

[0028] Where z0 is the z-axis coordinate in the camera coordinate system, h is the distance from the image coordinate system to the refracting water surface, and f is the focal length of the camera;

[0029] The compensation coefficient before and after refraction is calculated by formula (5), which is expressed as:

[0030]

[0031] in,

[0032]

[0033] O c O=f (7)

[0034] O c H=h c +d (8)

[0035] O c O′=h c (9)

[0036] hc =f+h (10)

[0037] Where k is the compensation coefficient of the target before and after refraction, O is the origin of the image coordinate system, O' is the origin of the refracted water surface coordinate system, P is the projected pixel point after refraction, α is the incident angle of the light on the water surface, β is the exit angle of the light on the water surface, and O c is the origin of the camera coordinate system, G is the vertical auxiliary point of the refraction point Q relative to the actual position Q' of the target, H is the vertical auxiliary point of the origin O' of the refraction surface coordinate system relative to the actual position Q' of the target, and d is the distance from the refraction surface coordinate system to the actual position of the target;

[0038] Then we have:

[0039]

[0040]

[0041] Then the refraction compensation model is expressed as:

[0042]

[0043] in, Very small, approximately 0 processing.

[0044] Preferably, in the step of constructing a pose estimation model and the step of three-dimensional pose estimation, the transformation relationship between the world coordinate system and the camera coordinate system is [R|T], where R is the rotation matrix between the two coordinate systems, and T is the translation matrix between the two coordinate systems.

[0045] Preferably, in the three-dimensional pose estimation step, the specific steps of solving the pose estimation model to obtain the conversion relationship [R|T] between the world coordinate system and the camera coordinate system of the target object are:

[0046] Let A′, B′, C′, and D′ be the actual positions of the four feature points, a′, b′, c′, and d′ be the pixel coordinate positions of the four detected feature points, and o′ be the optical center of the camera, connecting A′, C′ with a′, c′ respectively;

[0047] For triangles Δo′a′b′ and Δo′A′B′, the cosine theorem gives:

[0048] o′A′ 2 +o′B′ 2 -2o′A′·o′B′·cos<a′,b′> =A′B′ 2 (14)

[0049] For triangles Δo′b′c′ and Δo′B′C′, triangles Δo′a′c′ and Δo′A′C′, the same logic can be obtained:

[0050]

[0051] Process formula (14) and formula (15) and divide both sides of the equation by o′C′ 2 , record X=o′A′ / oC′, Y=o′B′ / o′C′, we get:

[0052] X 2 +Y 2 -2XYcos<a′,b′>=A′B′ 2 / o′C′ 2

[0053] Y 2 +1 2 -2Ycos<b′,c′>=B′C′ 2 / o′C′ 2 (16)

[0054] X 2 +1 2 -2Xcos<a′,c′>=A′C′ 2 / o′C′ 2

[0055] Let V = A′B′ 2 / o′C′ 2 ,UV=B′C′ 2 / o′C′ 2 ,WV=A′C′ 2 / o′C′ 2 ,but:

[0056] X 2 +Y 2 -2XYcos<a′,b′> -V=0

[0057] Y 2 +1 2 -2Ycos<b′,c′> -UV=0 (17)

[0058] X 2 +1 2 -2Xcos<a′,c′> -WV=0

[0059] The deformation can be obtained:

[0060]

[0061] In the formula, cos<a′,b′> ,cos<b′,c′> ,cos<a′,c′> The three cosine angles are obtained from the position of the midpoint of the image, u = B'C' 2 / A′B′ 2 ,w=A′C′2 / A′B′ 2 Calculated by combining the real-world coordinate system with prior information;

[0062] Formula (18) is converted into an equation about X and Y. Three characteristic points are substituted into four solutions. Then the fourth characteristic point is substituted into an optimal solution of [R|T].

[0063] The least squares method is used to iterate the optimal solution of [R|T]. The gain ratio p is calculated after each iteration until the gain ratio p is greater than the threshold. The iteration ends and whether it converges is determined. Finally, the transformation relationship [R|T] between the optimized world coordinate system and the camera coordinate system is obtained.

[0064] Compared with the prior art, the advantages and positive effects of the present invention are:

[0065] (1) The method for estimating the three-dimensional position of a target by underwater monocular vision provided by the present invention improves the YOLOv3 neural network, constructs a neural network model, and then trains the neural network model. The target object is detected by the trained neural network model. Compared with traditional detection methods and original neural networks, the present invention can better detect targets in complex underwater imaging environments and improve the detection accuracy of targets.

[0066] (2) The present invention adopts an improved YOLOv3 neural network and removes the branch responsible for large target detection, which can improve the calculation speed and meet the requirements of an environment with limited underwater computing resources.

[0067] (3) The method of training a neural network model of the present invention can achieve better training results with a small amount of data set samples.

[0068] (4) The present invention compensates for the refraction problem of the camera when working underwater, greatly reducing the error. The pose estimation has high accuracy and can meet the operation requirements of small targets in practical applications. It is of great significance for improving the autonomy of underwater autonomous operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 This is a flow chart of the method for estimating the three-dimensional position and posture of a target using underwater monocular vision according to an embodiment of the present invention;

[0070] Figure 2 A diagram showing a detection method of the method for estimating the three-dimensional position and posture of a target using underwater monocular vision according to an embodiment of the present invention;

[0071] Figure 3 is a schematic diagram of the structure of the recombination layer according to an embodiment of the present invention;

[0072] Figure 4A schematic diagram of the early layers of the improved YOLOv3 neural network according to an embodiment of the present invention;

[0073] Figure 5 A schematic diagram of branch improvements of the YOLOv3 neural network according to an embodiment of the present invention;

[0074] Figure 6 This is a branch block diagram of the YOLOv3 neural network after pruning processing according to an embodiment of the present invention;

[0075] Figure 7 A schematic diagram of constructing a refraction compensation model according to an embodiment of the present invention;

[0076] Figure 8 A schematic diagram of a world coordinate system according to an embodiment of the present invention;

[0077] Fig. 9 A schematic diagram of solving the transformation relationship [R|T] between the world coordinate system and the camera coordinate system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0078] The present invention is described in detail below by way of exemplary embodiments. However, it should be understood that elements, structures, and features in one embodiment may also be beneficially combined in other embodiments without further description.

[0079] See also Figure 1 The embodiment of the present invention provides a method for estimating the three-dimensional position and posture of a target through underwater monocular vision. The detected target object is a characteristic region of a regular-shaped object. For example, a cylinder is used as an example. Figure 2 , detect the four corner points of the cylinder, where the cylinder represents the regular shape target and the rectangular box represents the detection area. The specific steps are:

[0080] S1. Steps for building a neural network model: Add a reorganization layer to the backbone network of the YOLOv3 neural network instead of the convolution layer and the pooling layer, introduce instance normalization to replace the batch normalization in the early layers of the YOLOv3 neural network, and build a neural network model.

[0081] Specifically, see Figure 3 The reorganization layer is to disassemble the 4 pixels in the 2*2 image block on each channel and then rearrange them into a structure layer of 1*1 image blocks of 4 channels. Adding the reorganization layer can better preserve the local details in the pixels, which is conducive to providing pixel information for subsequent pose estimation.

[0082] Specifically, the instance normalization is expressed as:

[0083]

[0084] In the formula, yncij is the normalized value, x ncij is the ncijth element of the feature map in a layer, where i and j are spatial dimensions, c is the feature channel, and n is the nth image in the batch; μ nc is the average difference in the cth feature channel of the nth image, σ nc is the variance in the cth feature channel of the nth image, ∈ is a constant, H0 is the height of the feature map, and W0 is the width of the feature map.

[0085] It should be noted that since the images taken in underwater environments are blurry and have low contrast, the network used underwater should have better feature extraction capabilities. Figure 4 In the present invention, instance normalization is introduced to replace batch normalization in some early layers of the YOLO neural network, which can help discard the contrast of the image and leave the content information, indirectly improve the underwater detection ability, and have better learning ability for low-level features (such as lines and textures). At the same time, when the training batch is small (that is, there are fewer pictures for one-time training, and the model has less grasp of the overall features), it has better effect and is more suitable for underwater applications.

[0086] In this step, when building the neural network model, the loss function of the YOLOv3 neural network is also improved and converted into the loss of the diagonal of the region box. The loss function of the neural network model is expressed as:

[0087]

[0088] In the formula, C m is the target category; P m is the probability of belonging to a certain category; Is the jth anchorbox of the ith grid responsible for predicting the target? If so, it is 1, otherwise it is 0; w0 is the width of the prediction box, h0 is the height of the prediction box; w0' is the width of the real box, h0' is the height of the real box, are all grids in the feature map, s is the side length of the feature map, To traverse the prediction box corresponding to each grid, x i and i is the coordinate of the prediction box of the i-th grid in the feature map, x i ′ and y i ′ is the coordinate of the real box of the i-th grid in the feature map, λ coord is coord The weight coefficient of the corresponding item, λ noobj is noobj The weight coefficient of the corresponding item, Whether the j-th anchor box of the ith grid is not responsible for predicting the target, if not, it is 1, otherwise it is 0. is the category of the predicted box of the jth grid in the feature map, is the category of the real box of the jth grid in the feature map, is the confidence of the prediction box of the jth grid in the feature map, is the confidence of the ground-truth box of the jth grid in the feature map.

[0089] It should be noted that in actual underwater applications, the target's posture is uncertain, so the aspect ratio of the target in the collected image is uncertain. The present invention improves the loss function of the neural network model and converts it into the loss of the diagonal of the region box. It can improve the generalization ability of the YOLOv3 neural network and enable the target to be accurately detected in various postures.

[0090] In this step, when building a neural network model, refer to Figure 5 , the large branches of the three branches in the YOLOv3 neural network are pruned, and only the medium and small branches for detecting medium and small targets are retained. The block diagram of the improved YOLOv3 network can be found in Figure 6 When detecting small targets, only two branches responsible for small targets and medium targets are in effect, and the branch responsible for large target detection is only consuming computing resources. The present invention only detects small targets and removes the branch responsible for large target detection, which can improve the computing speed and meet the requirements of underwater computing resource-limited environments.

[0091] S2. Step of training the neural network model: training the neural network model to obtain a trained neural network model.

[0092] Specifically, before training the neural network model, the data set of the area to be detected is first labeled, and then the size of the anchor box is set. The optimal 9 clusters of anchor boxes are obtained through clustering, and then the network model training is applied.

[0093] It should be noted that, since the training process of the neural network model is to continuously adjust the anchor box to obtain the correct prediction box, before training the neural network model, it is necessary to set the anchor box of appropriate size to improve the accuracy of algorithm recognition. In the application scenario of the present invention, the target feature area to be detected is small, and the K-Means clustering algorithm is introduced to obtain the optimal 9 clusters of anchor boxes required by YOLOv3 through clustering. In the clustering process, firstly, 9 points are randomly selected as the initial centroid, and then all points in the entire sample are traversed to find the center point closest to each point as a cluster, and then the new centroid of the generated 9 clusters is recalculated until the centroid does not change or the number of iterations reaches the maximum, and the clustering is completed. At this time, the optimal anchor box of 9 clusters is obtained.

[0094] Specifically, the method for labeling the data set for the detection area is: (1) rotating the existing data set image by a random angle, and randomly adjusting the image brightness within a set range; (2) performing adaptive histogram equalization on the adjusted image, randomly adjusting the color contrast threshold and the grid size for pixel equalization within a set range, obtaining data set samples under different contrast and brightness conditions, and using labelimg2 to label the data set after the samples meet the set requirements. It should be noted that in the process of detecting underwater targets, since samples in real underwater environments are difficult to obtain, there are only a few samples in most applications, which is difficult to meet training needs. The present invention uses the above-mentioned labeling method to play a greater role in the case of limited data sets and obtain better training results.

[0095] S3, target detection step: detect the target in the acquired underwater target image through the trained neural network model, obtain the prediction frame of the four corner points of the target, take the midpoint pixel coordinates of the prediction frame as the feature point coordinates of the target, and obtain the pixel coordinates of the four feature points on the target.

[0096] S4, step of constructing a refraction compensation model: constructing a refraction compensation model according to the actual position of the target object, the position where the refraction occurs, and the projection position after the refraction in water.

[0097] Specifically, see Figure 7 ,The specific steps of constructing the refraction supplement model are:

[0098] Convert the pixel coordinate system to the image coordinate system:

[0099]

[0100] Where x0 is the x-axis coordinate in the image coordinate system, i.e. the camera coordinate system; y0 is the y-axis coordinate in the image coordinate system, i.e. the camera coordinate system; u and v are the pixel coordinates in the pixel coordinate system, respectively; u0 and v0 are the positions of the origin of the image coordinate system in the pixel coordinate system. is the pixel size on the x-axis in the image coordinate system, is the pixel size on the y-axis in the image coordinate system;

[0101] The coordinates of the refraction point in the camera coordinate system are calculated by formula (4), which is expressed as:

[0102]

[0103] Where z0 is the z-axis coordinate in the camera coordinate system, h is the distance from the image coordinate system to the refractive water surface (i.e., the camera sealing cover), and f is the focal length of the camera;

[0104] The compensation coefficient before and after refraction is calculated by formula (5), which is expressed as:

[0105]

[0106] in,

[0107]

[0108] O c O=f (7)

[0109] O c H=h c +d (8)

[0110] O c O′=h c (9)

[0111] h c =f+h (10)

[0112] Where k is the compensation coefficient of the target before and after refraction, O is the origin of the image coordinate system, O' is the origin of the refracted water surface coordinate system, P is the projected pixel point after refraction, α is the incident angle of the light on the water surface, β is the exit angle of the light on the water surface, and O c is the origin of the camera coordinate system, G is the vertical auxiliary point of the refraction point Q relative to the actual position Q' of the target, H is the vertical auxiliary point of the origin O' of the refraction surface coordinate system relative to the actual position Q' of the target, and d is the distance from the refraction surface coordinate system to the actual position of the target;

[0113] Then we have:

[0114]

[0115]

[0116] Then the refraction compensation model is expressed as:

[0117]

[0118] in, Very small, approximately 0 processing.

[0119] S5, refraction compensation step: calculating the compensation coefficients before and after refraction according to the refraction compensation model, and obtaining the pixel coordinates of the four feature points after compensation according to the compensation coefficients before and after refraction.

[0120] Specifically, the compensated pixel coordinates are expressed as: (u′, v′)=k(u, v).

[0121] S6, step of constructing the pose estimation model: according to the compensated pixel coordinates of the four feature points, combined with the known prior information, a world coordinate system is established on the target object (see Figure 8 ), obtain the 3D coordinates of the four feature points in the real world coordinate system, and establish a camera coordinate system with the direction perpendicular to the camera plane as the Z axis. The four feature points establish the conversion relationship between the world coordinate system and the camera coordinate system. This conversion relationship is the constructed pose estimation model.

[0122] Specifically, the transformation relationship between the world coordinate system and the camera coordinate system is [R|T], where R is the rotation matrix between the two coordinate systems, and T is the translation matrix between the two coordinate systems.

[0123] S7, three-dimensional pose estimation step: Solve the pose estimation model to obtain the transformation relationship between the world coordinate system of the target object and the camera coordinate system. The transformation relationship is the three-dimensional pose of the target object relative to the camera.

[0124] Specifically, the specific steps for solving the pose estimation model to obtain the transformation relationship [R|T] between the world coordinate system and the camera coordinate system of the target object are:

[0125] See also Fig. 9 , let A′, B′, C′, D′ be the actual positions of the four feature points, a′, b′, c′, d′ be the pixel coordinate positions of the four detected feature points, o′ is the optical center of the camera, and connect A′, C′ with a′, c′ respectively.

[0126] For triangles Δo′a′b′ and Δo′A′B′, the cosine theorem gives:

[0127] o′A′ 2 +o′B′ 2 -2o′A′·o′B′·cos<a′,b′>=A′B′ 2(14)

[0128] For triangles Δo′b′c′ and Δo′B′C′, triangles Δo′a′c′ and Δo′A′C′, the same logic can be obtained:

[0129]

[0130] Process formula (14) and formula (15) and divide both sides of the equation by o′C′ 2 , record X=o′A′ / oC′, Y=o′B′ / o′C′, we get:

[0131] X 2 +Y 2 -2XYcos<a′,b′> =A′B′ 2 / o′C′ 2

[0132] Y 2 +1 2 -2Ycos<b′,c′> =B′C′ 2 / o′C′ 2 (16)

[0133] X 2 +1 2 -2Xcos<a′,c′> =A′C′ 2 / o′C′ 2

[0134] Let V = A′B′ 2 / o′C′ 2 ,UV=B′C′ 2 / o′C′ 2 ,WV=A′C′ 2 / o′C′ 2 ,but:

[0135] X 2 +Y 2 -2XYcos<a′,b′> -V=0

[0136] Y 2 +1 2 -2Ycos<b′,c′> -UV=0 (17)

[0137] X 2 +1 2 -2Xcos<a′,c′> -WV=0

[0138] The deformation can be obtained:

[0139]

[0140] In the formula, cos<a′,b′> ,cos<b′,c′> ,cos<a′,c′> The three cosine angles are obtained from the position of the midpoint of the image, u = B'C' 2 / A′B′ 2 ,w=A′C′ 2 / A′B′ 2 Calculated by combining the real-world coordinate system with prior information;

[0141] Formula (18) is converted into an equation about X and Y. The three feature points are substituted into four solutions, and then the fourth feature point is substituted into an optimal solution of [R|T].

[0142] The least squares method is used to iterate the optimal solution of [R|T]. The gain ratio p is calculated after each iteration until the gain ratio p is greater than the threshold. The iteration ends and whether it converges is determined. Finally, the optimized conversion relationship [R|T] between the world coordinate system and the camera coordinate system is obtained. The specific steps are as follows:

[0143] The Lie algebra of the transformation matrix is ​​represented as ξ, and the world coordinates of the actual point in the construction space are P i =[X i ,Y i ,Z i ], the pixel coordinate of the projection corresponding to the actual point is u i =[u i ,v i ], according to three-dimensional transformation and Lie algebra, we have:

[0144]

[0145] In the formula, s i is the depth from the target to the camera, and K is the projection relationship in the camera coordinate system;

[0146] The optimal solution of [R|T] is used as the initial value of exp(ξ∧), the errors on both sides of equation (19) are summed, the least squares problem is constructed, and the error is minimized by iterative solution. The error function is constructed as follows:

[0147]

[0148] Solve the derivative of each error term with respect to the optimization variable, that is, linearize:

[0149] e(x+Δx)≈e(x)+JΔx (21)

[0150] In the formula, x is the initial value, Δx is the iteration increment;

[0151] Substituting the Jacobian matrix into the chain rule for derivation, we have:

[0152]

[0153] In formula (22), P′=[X′,Y′,Z′] is the world coordinate P i Using the coordinates after Lie algebra perturbation, the first term is the derivative of the error with respect to the projection point, and the second term is the derivative of the transformed point with respect to the Lie algebra. The Jacobian matrix is ​​obtained by multiplying the first term with the second term as follows:

[0154]

[0155] In the formula, f x 、f y is the focal length of the camera in pixels;

[0156] Use the LM algorithm for iterative calculations, and the incremental formula for each iteration is:

[0157] (H'+λ)Δx=g (24)

[0158] In the formula, H'=J T J, λ is the Lagrangian operator, g = -J T F(x), F(x) is the cost function, T represents transpose;

[0159] Substituting Δx into the gain ratio formula:

[0160]

[0161] Where p is the gain ratio;

[0162] After each iteration, p is calculated once until p is greater than the threshold, at which point the iteration ends and whether it converges is determined. Finally, the transformation relationship [R|T] between the optimized world coordinate system and the camera coordinate system is obtained, that is, the final three-dimensional pose of the target relative to the camera is obtained.

[0163] At this point, autonomous operation of the target object can be achieved through coordinate conversion between the camera coordinate system and other actuators (such as a robotic arm).

[0164] The present invention first selects the P3P algorithm, uses three pairs of points out of the four pairs for calculation, and then obtains the optimal solution of the transformation relationship [R|T] according to the fourth point. Then, the least squares problem is used to iterate the solution to reduce the error, taking into account both the solution accuracy and the calculation speed, and realizing accurate real-time pose estimation.

[0165] The specific implementation scheme described above further describes the purpose and technical solutions of the present invention in detail. Those skilled in the art will understand that the above description is only a specific implementation scheme of the present invention and is not intended to limit the scope of protection of the present invention.

Claims

1. A method for estimating the three-dimensional pose of a target using underwater monocular vision, characterized in that: The specific steps are: Steps for building a neural network model: Add a reorganization layer to the backbone network of the YOLOv3 neural network instead of the convolution layer and pooling layer, introduce instance normalization to replace the batch normalization in the early layers of the YOLOv3 neural network, and build a neural network model; Steps for training the neural network model: training the neural network model to obtain a trained neural network model; Target detection step: Detect the target in the acquired underwater target image through the trained neural network model, obtain the prediction frame of the four corner points of the target, take the midpoint pixel coordinates of the prediction frame as the feature point coordinates of the target, and obtain the pixel coordinates of the four feature points on the target; Steps for constructing a refraction compensation model: construct a refraction compensation model according to the actual position of the target object, the refraction position, and the projection position after refraction in water; Refraction compensation step: calculate the compensation coefficients before and after refraction according to the refraction compensation model, and calculate the pixel coordinates of the four feature points after compensation according to the compensation coefficients before and after refraction; Steps to construct the pose estimation model: According to the compensated pixel coordinates of the four feature points, combined with known prior information, a world coordinate system is established on the target object to obtain the 3D coordinates of the four feature points in the real world coordinate system, and a camera coordinate system is established with the direction perpendicular to the camera plane as the Z axis. The four feature points establish a conversion relationship between the world coordinate system and the camera coordinate system. This conversion relationship is the constructed pose estimation model; Three-dimensional pose estimation steps: Solve the pose estimation model to obtain the transformation relationship between the world coordinate system of the target object and the camera coordinate system. This transformation relationship is the three-dimensional pose of the target object relative to the camera.

2. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the step of constructing the neural network model, the reorganization layer is to disassemble the 4 pixels in the image block of size 2*2 on each channel, and then rearrange them into a structural layer of 1*1 image blocks of size in 4 channels.

3. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the step of building a neural network model, the instance normalization is expressed as: In the formula, y ncij is the normalized value, x ncij is the ncijth element of the feature map in a layer, where i and j are spatial dimensions, c is the feature channel, and n is the nth image in the batch; μ nc is the average difference in the cth feature channel of the nth image, σ nc is the variance in the cth feature channel of the nth image, ∈ is a constant, H0 is the height of the feature map, and W0 is the width of the feature map.

4. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the step of building the neural network model, the loss function of the YOLOv3 neural network is improved and converted into the loss of the diagonal of the region box. The loss function of the neural network model is expressed as: In the formula, C i is the target category; P i is the probability of belonging to a certain category; Is the jth anchor box of the ith grid responsible for predicting the target? If so, it is 1, otherwise it is 0; w0 is the width of the prediction box, h0 is the height of the prediction box; w0' is the width of the real box, h0' is the height of the real box, are all grids in the feature map, s is the side length of the feature map, To traverse the prediction box corresponding to each grid, x i and i is the coordinate of the prediction box of the i-th grid in the feature map, x i ′ and y i ′ is the coordinate of the real box of the i-th grid in the feature map, λ coord is coord The weight coefficient of the corresponding item, λ noobj is noobj The weight coefficient of the corresponding item, Whether the j-th anchor box of the ith grid is not responsible for predicting the target, if not, it is 1, otherwise it is 0. is the category of the predicted box of the jth grid in the feature map, is the category of the real box of the jth grid in the feature map, is the confidence of the prediction box of the jth grid in the feature map, is the confidence of the ground-truth box of the jth grid in the feature map.

5. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the step of building the neural network model, the large branches of the three branches in the YOLOv3 neural network are pruned, leaving only the medium and small branches for detecting medium and small targets.

6. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the step of training the neural network model, before training the neural network model, firstly, the data set of the detection area is annotated, and then the size of the anchor box is set, and the optimal 9 clusters of anchor boxes are obtained by clustering. At this time, the network model training is applied.

7. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 6, characterized in that: The method for labeling the dataset of the detection area is as follows: (1) rotate the existing dataset image by a random angle and randomly adjust the image brightness within a set range; (2) perform adaptive histogram equalization on the adjusted image, randomly adjust the color contrast threshold and the grid size for pixel equalization within a set range, and obtain dataset samples under different contrast and brightness conditions. After the samples meet the set requirements, use labelimg2 to label the dataset.

8. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the step of constructing a refraction compensation model, the specific steps of constructing a refraction supplement model are as follows: Convert the pixel coordinate system to the image coordinate system: Where x0 is the x-axis coordinate in the image coordinate system, i.e. the camera coordinate system; y0 is the y-axis coordinate in the image coordinate system, i.e. the camera coordinate system; u and v are the pixel coordinates in the pixel coordinate system, respectively; u0 and v0 are the positions of the origin of the image coordinate system in the pixel coordinate system. is the pixel size on the x-axis in the image coordinate system, is the pixel size on the y-axis in the image coordinate system; The coordinates of the refraction point in the camera coordinate system are calculated by formula (4), which is expressed as: Where z0 is the z-axis coordinate in the camera coordinate system, h is the distance from the image coordinate system to the refracting water surface, and f is the focal length of the camera; The compensation coefficient before and after refraction is calculated by formula (5): in, O c O=f (7) O c H=h c +d (8) O c O′=h c (9) h c =f+h (10) Where k is the compensation coefficient of the target before and after refraction, O is the origin of the image coordinate system, O' is the origin of the refracted water surface coordinate system, P is the projected pixel point after refraction, α is the incident angle of the light on the water surface, β is the exit angle of the light on the water surface, and O c is the origin of the camera coordinate system, G is the vertical auxiliary point of the refraction point Q relative to the actual position Q' of the target, H is the vertical auxiliary point of the origin O' of the refraction surface coordinate system relative to the actual position Q' of the target, and d is the distance from the refraction surface coordinate system to the actual position of the target; Then we have: Then the refraction compensation model is expressed as: in, Very small, approximately 0 processing.

9. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 1, characterized in that: In the pose estimation model construction step and the 3D pose estimation step, the transformation relationship between the world coordinate system and the camera coordinate system is [R|T], where R is the rotation matrix between the two coordinate systems and T is the translation matrix between the two coordinate systems.

10. The method for estimating the three-dimensional position of a target by underwater monocular vision according to claim 9, characterized in that: In the 3D pose estimation step, the specific steps for solving the pose estimation model to obtain the transformation relationship [R|T] between the world coordinate system and the camera coordinate system of the target object are: Let A′, B′, C′, and D′ be the actual positions of the four feature points, a′, b′, c′, and d′ be the pixel coordinate positions of the four detected feature points, and o′ be the optical center of the camera, connecting A′, C′ with a′, c′ respectively; For triangles Δo′a′b′ and Δo′A′B′, the cosine theorem gives: o′A′ 2 +o′B′ 2 -2o′A′·o′B′·cos<a′,b′> =A′B′ 2 (14) For triangles Δo′b′c′ and Δo′B′C′, triangles Δo′a′c′ and Δo′A′C′, the same logic can be obtained: Process formula (14) and formula (15) and divide both sides of the equation by o′C′ 2 , record X=o′A′ / oC′, Y=o′B′ / o′C′, we get: Let \(V = A'B' 2 / o'C' 2 , \(UV = B'C' 2 / o'C' 2 , \(WV = A'C' 2 / o'C' 2 , then: The deformation can be obtained: In the formula, cos<a′,b′> ,cos<b′,c′> ,cos<a′,c′> The three cosine angles are obtained from the position of the midpoint of the image, u = B'C' 2 / A′B′ 2 ,w=A′C′ 2 / A′B′ 2 Calculated by combining the real-world coordinate system with prior information; Formula (18) is converted into an equation about X and Y. Three characteristic points are substituted into four solutions. Then the fourth characteristic point is substituted into an optimal solution of [R|T]. The least squares method is used to iterate the optimal solution of [R|T]. The gain ratio p is calculated after each iteration until the gain ratio p is greater than the threshold. The iteration ends and whether it converges is determined. Finally, the transformation relationship [R|T] between the optimized world coordinate system and the camera coordinate system is obtained.

Citation Information

Patent Citations

  • Underwater monocular vision target depth positioning fusion estimation method based on deep learning

    CN111915678A

  • Underwater monocular sub-pixel relative pose estimation method

    CN112700501A

  • Monocular unsupervised depth estimation method based on contextual attention mechanism

    US20210390723A1