April Tag positioning and attitude resolving method based on LM algorithm

Abstract

The invention discloses an April Tag positioning and attitude resolving method based on an LM algorithm, and belongs to the technical field of computer vision, and the method comprises the following steps: 1) carrying out camera calibration on a camera for positioning; 2) an April Tag graph is arranged on a carrier plane needing to be positioned, and a camera is used for shooting; (3) the shot April Tag image is subjected to preprocessing; 4) analyzing the preprocessed April Tag graph, and marking the image; 5) carrying out preliminary attitude calculation on the image; 6) performing iteration by using an LM algorithm; and 7) constructing a three-dimensional coordinate system for representing the attitude of the camera, drawing an arrow on the image, and displaying the deflection direction of the arrow and the camera. The method has the beneficial effects that the attitude calculation result is more accurate by using the LM algorithm for iteration, and the accuracy of the attitude calculation is improved by calculating and drawing the arrow of the deflection angle. And the deflection difference between the direction of the carrier on a two-dimensional plane and the positive direction of the camera can be more intuitively and conveniently seen.

Description

Technical Field

[0001] The present invention relates to the technical field of computer vision, and particularly to an April Tag positioning and attitude calculation method based on the LM algorithm. Background Art

[0002] In recent years, with the rapid development of technology, the level of new intelligent transportation technology has been continuously improved. Among them, the application prospect of unmanned ships has become increasingly broad, playing an important role in many fields such as intelligent shipping and marine resource exploration. For the unmanned control technology, accurate positioning technology is the key prerequisite for realizing its autonomous navigation and operation.

[0003] Traditional positioning methods such as GPS positioning can provide relatively extensive position information globally, but there may be limitations such as signal interference, decreased positioning accuracy, and even signal loss in some scenarios. Visual positioning technology is a technology that uses visual sensors to obtain environmental image information and determines the position and attitude of the carrier by processing and analyzing these images. In a suitable environment, this technology can provide very accurate position and attitude information as well as other information about the environment. Moreover, visual positioning mainly relies on receiving environmental light signals and does not require active signal emission, making this technology have unique advantages in some occasions where active signal emission is not allowed.

[0004] April Tag is a visual fiducial marker system composed of tags with specific codes and patterns. By equipping a camera to capture April Tag image information and using image processing algorithms and computer vision technology to identify, decode, and extract and analyze features, key information such as the position and attitude of the tag can be obtained. Compared with two-dimensional codes, April Tag has simple encoding, focuses on quickly and accurately transmitting identification information, and is more efficient for the identification and positioning of unmanned ships. In addition, April Tag has strong anti-interference ability and can still maintain a high recognition accuracy under complex lighting conditions. For situations where the image is distorted or deformed, April Tag has better robustness. Moreover, April Tag also has the advantages of a long recognition distance, fast detection speed, relatively simple deployment, and relatively low hardware requirements, and is very suitable for visual positioning.

[0005] In traditional pose estimation methods based on April Tag images, the P4P algorithm is an important solution. It aims to calculate the position and pose of a camera through the projection information of four known spatial points on the image plane. However, the P4P algorithm also has deficiencies. It is extremely sensitive to noise. In the actual image acquisition process, due to factors such as lighting changes and noise of the image sensor, the acquired image data often contains noise interference, which can cause large errors in the solution process of the P4P algorithm and affect the accuracy of pose estimation. Summary of the Invention

[0006] To solve the above problems, the present invention provides an April Tag positioning and pose estimation method based on the LM algorithm, which is realized through the following technical solutions.

[0007] An April Tag positioning and pose estimation method based on the LM algorithm, the method comprising the following steps:

[0008] 1) Calibrate the camera used for positioning to obtain the parameters of the camera;

[0009] 2) Arrange an April Tag pattern on the carrier plane where April Tag images are needed for positioning, and use the camera to take pictures above it. The types and directions of the April Tag patterns arranged on different carriers are the same, and the ID numbers are different;

[0010] 3) Preprocess the April Tag images captured by the camera;

[0011] 4) Analyze the preprocessed April Tag pattern and mark the image;

[0012] 5) Perform preliminary pose estimation on the image, use the P4P algorithm to process the information of the captured April Tag pattern, and obtain the orthonormalized rotation matrix and translation vector;

[0013] 6) Iterate the translation vector and the orthonormalized rotation matrix using the LM algorithm to obtain a new rotation matrix and translation vector;

[0014] 7) Construct a three-dimensional coordinate system to represent the camera pose, draw an arrow on the image, and display the deflection direction of the arrow and the camera.

[0015] Further, the step 1) includes the following sub-steps:

[0016] 11) Arrange a camera calibration board with a 6×4 checkerboard pattern of corner points on a plane;

[0017] 12) Use a camera to take 20 images above the camera calibration board and store them in the specified path. The shooting height of the camera is the positioning height of the April Tag.

[0018] 13) Construct a three-dimensional coordinate matrix of the checkerboard points in the world coordinate system and its corresponding two-dimensional coordinates.

[0019] 14) Read a series of checkerboard images taken in the specified path and process and analyze the images.

[0020] 15) Perform camera calibration based on the information obtained from the analysis and processing.

[0021] Further, in step 14), the process of processing and analyzing the grayscale image is as follows:

[0022] Set the threshold standard for corner detection.

[0023] Convert the image to a grayscale image through the cv2.cvtColor function.

[0024] Loop to find the checkerboard corners in the grayscale image until the number of corners meets the requirements.

[0025] Based on the corner positions in the grayscale image and the detection threshold, use the cv2.cornerSubPix function to accurately refine the corner positions.

[0026] Obtain the two-dimensional coordinates and three-dimensional coordinates of each detected corner in the grayscale image and integrate them into arrays respectively.

[0027] Draw the detected corners on the grayscale image and adjust the size of the grayscale image.

[0028] In step 15), based on the obtained three-dimensional coordinate array, two-dimensional coordinate array, and the size of the grayscale image, use the cv2.calibrateCamera function to obtain the internal parameter matrix and distortion coefficients of the camera.

[0029] Further, in step 3), the following sub-steps are included:

[0030] 31) Initialize the April Tag detector and specify the type of April Tag to be detected as "tag36h11" through the pl.Detector function.

[0031] 32) Use the cv2.VideoCapture function to open the camera and loop to capture the camera video frame.

[0032] 33) Perform distortion correction on the captured images. The process of distortion correction is as follows:

[0033] Obtain the size of the captured image.

[0034] Use the cv2.getOptimalNewCameraMatrix function to process the camera internal parameter matrix K and the distortion coefficient dist to obtain the new undistorted camera parameter K new and the size roi;

[0035] Through K, dist and K new , use the cv2.undistort function to undistort the image to obtain the undistorted image;

[0036] Crop the undistorted image according to the roi parameter to obtain the final undistorted image;

[0037] 34) Grayscale the undistorted image;

[0038] 35) Identify and detect the April Tag image in the grayscaled image through the detector.detect function.

[0039] Furthermore, in the step 4), the following sub-steps are included:

[0040] 41) Obtain the encoded ID number tag_id of the April Tag, and use the cv2.putText function to display it at the specified position in the image with the specified font and color;

[0041] 42) Obtain the pixel coordinates of the four corner points and the center point of the April Tag image, and use the cv2.circle function to draw circles of different colors with these points as the centers to mark each corner point;

[0042] 43) Use the cv2.line function to draw green straight lines to connect two adjacent corner points to mark the entire April Tag image.

[0043] Furthermore, in the step 5), the following sub-steps are included:

[0044] 51) Set the three-dimensional coordinates pst3d of the four corner points according to the size of the arranged April Tag image, and obtain the two-dimensional pixel coordinates pst2d;

[0045] 52) Based on ps 3d, pst2d, the camera parameter K and the distortion coefficient dist, obtain the 3×3 rotation matrix R and the 3×1 translation vector t, and the method is as follows:

[0046] The coordinates pst3d of the four corner points of the April Tag are i is the number of corner points, here take i = 1,..., 4; then there are corresponding pixel coordinates M i = RXi +t;

[0047] The relationship between the feature point coordinates and the pixel coordinates in the image is denoted as If s is a defined constant of unknown pixel coordinates, it can be obtained from the above formula H = K(r 1 r 2 t); In the formula, r i is the i-th column of the rotation matrix R;

[0048] Since K is obtained by camera calibration, then (r 1 r 2 t) = K -1 H = sK -1 UY H (YY H ) -1 , in the formula, In the formula, the superscript H indicates the conjugate transpose matrix of this matrix;

[0049] Since r 3 = r 1 ×r 2 , then r 3 can be obtained according to the heterodyne formula, and then the rotation matrix R = [r 1 r 2 r 3 ;

[0050] 53) Perform Schmidt orthogonalization on the rotation matrix R to improve its orthogonality. The method is as follows:

[0051] Perform orthogonalization on the rotation matrix R = [r 1 r 2 r 3 , then β 1 = r 1 ,

[0052]

[0053] Then normalize it, and there is Then the orthogonalized rotation matrix R = [γ 1 ,γ 2 ,γ 3 ;

[0054] Furthermore, in step 6), the following sub-steps are included:

[0055] 61) Set the convergence threshold tolerance to 10-6, the maximum number of iterations max_iters to 100 times, and the initial damping factor λ to 0.01;

[0056] 62) Process the rotation matrix R, translation vector t, camera intrinsics K, the 3D coordinates pst3d and 2D coordinates pst2d of the April Tag corners to obtain the Jacobian matrix and the residual err:

[0057] The definition of the Jacobian matrix is where f is the error vector function and t is the translation vector;

[0058] Also, where is the camera transformation of the 3D corner coordinates pst3d in the camera coordinate system;

[0059] where k i,j is the element in the i-th row and j-th column of the camera intrinsic parameter matrix; is the actual 2D pixel coordinate of the corner; i is related to the number of image corners and takes values i = 1, …, 4;

[0060] Also, where I is the identity matrix, R is the rotation matrix, S is the 3D coordinates of the corner, and t is the translation vector;

[0061] For the definition of the residual err, we have where f 2i-1 = z′ i u′ i - z′ i u i , f 2i = z′ i v′ i - z′ i v i , i = 1, …, 4; K is the camera intrinsic parameter matrix, is the projected pixel coordinate calculated from the coordinates of the corner on the plane, R, and t;

[0062] 63) Process the Jacobian matrix, damping factor λ, and residual err to calculate the parameter increment Δθ:

[0063] Δθ = (H + λI) -1 J T r, where J is the Jacobian matrix, λ is the damping factor, r is the residual, and H is calculated from H = J T J, and here the superscript T is the matrix transpose;

[0064] 64) Calculate the increment ΔR of the rotation matrix and the increment Δt of the translation vector from the parameter increment, and update to obtain the new parameters R new and tnew ;

[0065] Since the parameter increment can be written as where ΔS and Δρ are both three-dimensional column vectors. The parameter increment is mapped to a matrix ΔT, which is multiplied by T to obtain the updated matrix T′ = ΔTT, and we have

[0066] Let then we have In the formula, a is a direction vector with a length of 1, and a^ is the skew-symmetric matrix of a;

[0067] Then the new parameter R new = ΔR·R, t new = ΔR·t + V·ΔS;

[0068] 65) After one iteration, convergence judgment is performed on the result of this iteration;

[0069] 66) If the value of the parameter increment Δθ is less than the convergence threshold tolerance, the result of this iteration converges, and the iteration ends;

[0070] 67) If it does not converge, then use R new and t new to calculate the new residual err new , and compare it with the residual err before this iteration, and dynamically adjust the value of the damping factor λ accordingly:

[0071] If the value of err new is greater than err, then increase λ by 10 times, otherwise adjust λ to 1 / 10, and use R new and t new as the input values for the next iteration until the parameter increment is less than the convergence threshold or the number of iterations reaches the number set by max_iters.

[0072] Furthermore, in step 7), the method for constructing a three-dimensional coordinate system is as follows:

[0073] 711) Set a group of three 3D points axis, store the three-dimensional coordinates of the three points, and use them to represent the coordinate information of the coordinate axes in three-dimensional space;

[0074] 712) Based on the iterated rotation vector, translation vector, and the internal parameter matrix and distortion coefficients of the camera, project the three-dimensional coordinates into two-dimensional pixel coordinates:

[0075] Convert the rotation matrix R new back to the corresponding rotation vector r new, use the cv2.projectPoints function to process the rotation vector r new , the translation vector t new and the camera parameters K and dist to obtain the projection coordinate group imgpts of the axis on the plane;

[0076] 713) Select the first corner corner in the corner coordinates pst2d of the April Tag as the origin of the coordinate system, and use the cv2.line function to connect corner with the three projection point coordinates in imgpts with straight lines of different colors respectively to draw the coordinate system.

[0077] Furthermore, in the step 7), the method of drawing an arrow on the image is as follows:

[0078] 721) Calculate the abscissa difference and ordinate difference between the projection point representing the Y-axis of the attitude coordinate system and the origin respectively:

[0079] Select the projection point coordinate imgpts[1] of the Y-axis drawn in the coordinate system, and calculate dx = imgpts[1][0] - corner[0], dy = imgpts[1][1] - corner[1]; where imgpts[1][0] and imgpts[1][1] are the x coordinate value and y coordinate value of the projection point respectively; corner[0] and corner[1] are the x coordinate value and y coordinate value of the origin respectively, and dx and dy are the abscissa difference and ordinate difference respectively;

[0080] 722) Based on the two coordinate differences, calculate the deflection angle through the arctangent function and convert the radian to an angle. If the abscissa difference is zero, judge the angle to be 90° or 270°:

[0081] Use the math.atan2 function to calculate the deflection angle, calculate the value, and use the math.degrees function to convert the radian to the angle angle;

[0082] 723) Starting from the center of the April Tag, draw an arrow in the same direction as the positive direction of the camera shooting screen, and display the deflection angle near the arrow in the image:

[0083] Calculate the abscissa difference Δx and ordinate difference Δy between two adjacent corners, and use cv2.arrowedLine to start from the center coordinates (cX, cY) of the April Tag map and end at (cX + Δx, cY + Δy) to draw an arrow, and display the deflection angle angle near the arrow.

[0084] The beneficial effects of the present invention are as follows. By using the LM algorithm for iteration, the result of attitude calculation is more accurate. And by calculating and drawing the arrows of the deflection angle, it is possible to more intuitively and conveniently see the deflection difference between the direction of the carrier and the positive direction of the camera on the two-dimensional plane. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] In order to more clearly illustrate the technical solutions of the present invention, the drawings required for use in the following description of the specific embodiments will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0086] Figure 1 : Schematic diagram of an April Tag image of type tag36h11;

[0087] Figure 2 : Flowchart of an April Tag positioning and attitude calculation method based on the LM algorithm according to the present invention;

[0088] Figure 3 : Schematic diagram of the process of camera calibration for the camera;

[0089] Figure 4 : Schematic diagram of the process of image de-distortion;

[0090] Figure 5 : Schematic diagram of the positioning mark of the April Tag;

[0091] Figure 6 : Schematic diagram of the process of LM algorithm iteration;

[0092] Figure 7 : Schematic diagram of the drawing of the attitude coordinate axes and deflection arrows of the April Tag. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0093] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art without creative efforts based on the embodiments of the present invention belong to the scope of protection of the present invention.

[0094] As Figure 1-7 shown, an April Tag positioning and attitude calculation method based on the LM algorithm includes the following steps:

[0095] 1) Perform camera calibration on the camera used for positioning to obtain the parameters of the camera.

[0096] As Figure 3 shown, it includes the following sub-steps:

[0097] 11) Arrange a camera calibration board with a checkerboard of 6×4 corner points on a plane;

[0098] 12) Use a camera to take 20 images above the camera calibration board and store them in the specified path. The shooting height of the camera is the positioning height of the April Tag;

[0099] 13) Construct a three-dimensional coordinate matrix of the checkerboard points in the world coordinate system and its corresponding two-dimensional coordinates;

[0100] 14) Read a series of checkerboard images taken in the specified path, process and analyze the images. The processing and analysis process of the grayscale image is as follows:

[0101] Set the threshold standard for corner detection;

[0102] Convert the image to a grayscale image through the cv2.cvtCol function;

[0103] Loop to find the checkerboard corners in the grayscale image until the number of corners meets the requirements;

[0104] Based on the corner positions in the grayscale image and the detection threshold, use the cv2.cornerSubPix function to accurately refine the corner positions;

[0105] Obtain the two-dimensional coordinates and three-dimensional coordinates of each detected corner in the grayscale image and integrate them into arrays respectively;

[0106] Draw the detected corners on the grayscale image and adjust the size of the grayscale image.

[0107] 15) Based on the obtained three-dimensional coordinate array, two-dimensional coordinate array, and the size of the grayscale image, use the cv2.calibrateCamera function to obtain the internal parameter matrix and distortion coefficients of the camera.

[0108] 2) Arrange an April Tag image on the carrier plane where the April Tag image is needed for positioning, and use a camera to take a picture above it. The types and directions of the April Tag images arranged on different carriers are the same, but the ID numbers are different.

[0109] 3) Preprocess the April Tag image captured by the camera. The specific process is as Figure 4 shown.

[0110] 31) Initialize the April Tag detector, and specify the type of April Tag to be detected as "tag36h11" through the pl.Detector function;

[0111] April Tag is a visual fiducial system commonly used in computer vision applications. There are various types of April Tags, such as Figure 1 As shown, the tag36h11 used in the present invention is the most common type. The 36 in tag36h11 represents that the tag encoding is 36 bits, and h11 represents the minimum Hamming distance, which is used to enhance the anti-noise ability. tag36h11 is a 6×6 black and white square with black frames on its four sides. The interior is suitable for the binary pattern of encoding, and its ID number can be from 0 to 586. The 6×6 square quantity can make the detected information more and more accurate with less error rate;

[0112] 32) Use the cv2.VideoCapture function to open the camera and loop to capture the camera video frame;

[0113] 33) Perform distortion correction on the captured image. The process of distortion correction is as follows:

[0114] Obtain the size of the captured image;

[0115] Use the cv2.getOptimalNewCameraMatrix function to process the internal camera parameter matrix K and the distortion coefficient dist of the camera to obtain the new undistorted camera parameter K new and the size roi;

[0116] Through K, dist and K new , use the cv2.undistort function to perform distortion correction on the image to obtain the distortion-corrected image;

[0117] Crop the distortion-corrected image according to the roi parameter to obtain the final distortion-corrected image;

[0118] 34) Perform grayscale processing on the distortion-corrected image;

[0119] 35) Identify and detect the April Tag image in the grayscale image through the detector.detect function.

[0120] 4) Analyze the preprocessed April Tag image and mark the image. The effect is as Figure 5 shown.

[0121] It includes the following sub-steps:

[0122] 41) Obtain the ID number tag_id of the April Tag's encoding, and use the cv2.putText function to display it at the specified position in the image with the specified font and color;

[0123] 42) Obtain the pixel coordinates of the four corner points and the center point of the April Tag image, and use the cv2.circle function to draw circles of different colors with these points as the centers to mark each corner point;

[0124] 43) Use the cv2.line function to draw green straight lines connecting adjacent corner points in pairs to mark the entire April Tag image.

[0125] 5) Perform preliminary pose solution on the image, use the P4P algorithm to process the information of the captured April Tag image, and obtain the orthonormalized rotation matrix and translation vector.

[0126] It includes the following sub-steps:

[0127] 51) Set the three-dimensional coordinates pst3d of the four corner points according to the size of the arranged April Tag image, and obtain the two-dimensional pixel coordinates pst2d;

[0128] 52) Based on pst3d, pst2d, the camera parameter K and the distortion coefficient dist, obtain the 3×3 rotation matrix R and the 3×1 translation vector t, and the method is as follows:

[0129] The coordinates pst3d of the four corner points of the April Tag are i is the number of corner points, here take i = 1,..., 4; then there are corresponding pixel coordinates M i = RX i + t;

[0130] The relationship between the coordinates of the feature points in the image and the pixel coordinates is denoted as s is an unknown pixel coordinate constant defined, then it can be obtained from the above formula H = K(r 1 r 2 t); in the formula, r i is the i-th column of the rotation matrix R;

[0131] Since K is obtained by camera calibration, then (r 1 r 2 t) = K -1 H = sK -1 UY H (YY H ) -1 , in the formula, In the formula, the superscript H indicates the conjugate transpose matrix of this matrix;

[0132] Since r 3 = r 1 × r 2 , then r 3 can be obtained according to the heterodyne formula, and then the rotation matrix R = [r 1 r 2 r 3 ;

[0133] 53) Schmidt orthogonalization is performed on the rotation matrix R to improve its orthogonality, and the method is as follows:

[0134] For the rotation matrix R = [r 1 r 2 r 3 , orthogonalization is performed, then β 1 = r 1 ,

[0135] Then, normalization is performed on it, and there is Then the orthogonalized rotation matrix R = [γ 1 , γ 2 , γ 3 is obtained.

[0136] 6) The LM algorithm is used to iterate on the translation vector and the orthogonalized rotation matrix to obtain a new rotation matrix and translation vector, and its specific process is as Figure 6 shown.

[0137] It includes the following sub-steps:

[0138] 61) Set the convergence threshold tolerance to 10-6, the maximum number of iterations max_iters to 100 times, and the initial damping factor λ to 0.01;

[0139] 62) Process the rotation matrix R, translation vector t, camera internal parameter K, the three-dimensional coordinates pst3d and two-dimensional coordinates pst2d of the April Tag corner points to obtain the Jacobi matrix and the residual err:

[0140] The definition of the Jacobi matrix is where f is the error vector function and t is the translation vector;

[0141] Also, where is the camera change of the corner point three-dimensional coordinates pst3d in the camera coordinate system;

[0142] where k i,jis the element at the i-th row and j-th column of the camera internal parameter matrix; is the actual two-dimensional pixel coordinate of the corner point; i is related to the number of image corner points, and takes values i = 1, …, 4;

[0143] Also, where I is the identity matrix, R is the rotation matrix, S is the three-dimensional coordinate of the corner point, and t is the translation vector;

[0144] For the definition of the residual err, there is where f 2i-1 = z′ i u′ i - z′ i u i , f 2i = z′ i v′ i - z′ i v i , i = 1, …, 4; K is the camera internal parameter matrix, is the projected pixel coordinate calculated from the coordinate of the corner point on the plane, R, and t;

[0145] 63) Process the Jacobi matrix, damping factor λ, and residual err, and calculate the parameter increment Δθ:

[0146] Δθ = (H + λI) -1 J T r, where J is the Jacobi matrix, λ is the damping factor, r is the residual, and H is calculated from H = J T J, here the superscript T is the matrix transpose;

[0147] 64) Calculate the increment ΔR of the rotation matrix and the increment Δt of the translation vector from the parameter increment, and update to obtain the new parameters R new and t new ;

[0148] Since the parameter increment can be written as where ΔS and Δρ are both three-dimensional column vectors. Map the parameter increment to the matrix ΔT, multiply it by T, and get the updated matrix T′ = ΔTT, and there is

[0149] Let Then there is In the formula, a is a direction vector with a length of 1, and a^ is the skew-symmetric matrix of a;

[0150] Then the new parameter R new = ΔR·R, t new= ΔR·t + V·ΔS;

[0151] 65) After one iteration ends, perform a convergence judgment on the result of this iteration;

[0152] 66) If the value of the parameter increment Δθ is less than the convergence threshold tolerance, the result of this iteration converges, and the iteration ends;

[0153] 67) If it does not converge, then use R new and t new to calculate the new residual err new , compare it with the residual err before this iteration, and dynamically adjust the value of the damping factor λ accordingly:

[0154] If err new 's value is greater than err, then increase λ by 10 times, otherwise adjust λ to 1 / 10, and use R new and t new as the input values for the next iteration until the parameter increment is less than the convergence threshold or the number of iterations reaches the number set by max_iters.

[0155] 7) Construct a three-dimensional coordinate system to represent the camera pose, draw an arrow on the image, and display the deflection direction of the arrow and the camera. The effect is as Figure 7 shown.

[0156] The method for constructing the three-dimensional coordinate system is as follows:

[0157] 711) Set a group of three 3D points axis, store the three-dimensional coordinates of the three points, and use them to represent the coordinate information of the coordinate axes in three-dimensional space;

[0158] 712) Based on the rotated vector and translation vector after iteration, as well as the internal parameter matrix and distortion coefficients of the camera, project the three-dimensional coordinates into two-dimensional pixel coordinates:

[0159] Convert the rotation matrix R new back to the corresponding rotation vector r new through the cv2.Rodrigues function, and use the cv2.projectPoints function to process the rotation vector r new , translation vector t new and the camera parameters K and dist to obtain the projected coordinate group imgpts of axis on the plane;

[0160] 713) Select the first corner point corner in the corner coordinates pst2d of the April Tag as the origin of the coordinate system, and use the cv2.line function to connect corner with the three projected point coordinates in imgpts with straight lines of different colors respectively to draw the coordinate system.

[0161] The method of drawing an arrow on an image is as follows:

[0162] 721) Calculate the differences in abscissa and ordinate between the projection point representing the Y-axis of the pose coordinate system and the origin respectively:

[0163] Select the coordinates imgpts[1] of the projection point of the Y-axis drawn in the coordinate system, and calculate dx = imgpts[1][0] - corner[0], dy = imgpts[1][1] - corner[1]; where imgpts[1][0] and imgpts[1][1] are the x-coordinate value and y-coordinate value of this projection point respectively; corner[0] and corner[1] are the x-coordinate value and y-coordinate value of the origin respectively, and dx and dy are the differences in abscissa and ordinate respectively;

[0164] 722) Based on the two coordinate differences, calculate the deflection angle through the arctangent function and convert the radian to an angle. If the difference in abscissa is zero, determine the angle to be 90° or 270°:

[0165] Use the math.atan2 function to calculate the deflection angle, Calculate the value, and use the math.degrees function to convert the radian to the angle angle;

[0166] 723) Starting from the center of the April Tag, draw an arrow in the same direction as the positive direction of the camera-captured image, and display the deflection angle near the arrow in the image:

[0167] Calculate the difference in abscissa Δx and the difference in ordinate Δy between two adjacent corner points. Use cv2.arrowedLine to start from the center coordinates (cX, cY) of the April Tag image and end at (cX + Δx, cY + Δy) to draw an arrow, and display the deflection angle angle near the arrow.

[0168] The preferred embodiments of the present invention disclosed above are only used to help illustrate the present invention. The preferred embodiments do not describe all details in detail, nor do they limit the invention to only the specific implementation manners. Obviously, according to the content of this specification, many modifications and changes can be made. This specification selects and specifically describes these embodiments to better explain the principle and practical application of the present invention, so that those skilled in the art in the relevant technical field can well understand and utilize the present invention. The present invention is only limited by the claims and their full scope and equivalents.

Claims

1. An April Tag positioning and posture solving method based on LM algorithm, characterized in that: The method comprises the following steps: 1) Calibrate the camera used for positioning to obtain the parameters of the camera; 2) Arrange an April Tag image on the carrier plane where the April Tag image is needed for positioning, and use a camera to shoot above it. The April Tag images arranged on different carriers have the same type and direction, but different ID numbers; 3) Preprocess the April Tag image captured by the camera; 4) Analyze the preprocessed April Tag image and mark the image; 5) Perform preliminary posture calculation on the image, use the P4P algorithm to process the information of the April Tag image, and obtain the orthogonalized rotation matrix and translation vector; 6) Use the LM algorithm to iterate the translation vector and the orthogonalized rotation matrix to obtain a new rotation matrix and translation vector; 7) Construct a three-dimensional coordinate system to represent the camera posture, draw an arrow on the image, and display the deflection direction of the arrow and the camera.

2. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 1 is characterized in that: The step 1) comprises the following sub-steps: 11) Arrange a camera calibration plate with 6×4 corner points on the plane; 12) Use the camera to take 20 images above the camera calibration board and store them in the specified path. The shooting height of the camera is the positioning height of the April Tag. 13) Construct the three-dimensional coordinate matrix of the chessboard points in the world coordinate system and their corresponding two-dimensional coordinates; 14) Read a series of checkerboard images taken under the specified path and process and analyze the images; 15) Based on the analyzed and processed information, perform camera calibration.

3. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 2 is characterized in that: In step 14), the grayscale image processing and analysis process is as follows: Set the threshold standard for corner detection; Convert the image to a grayscale image using the cv2.cvtColor function; Loop to find the checkerboard corner points in the grayscale image until the number of corner points meets the requirement; Based on the corner point position in the grayscale image and the detection threshold, the cv2.cornerSubPix function is used to accurately refine the corner point position; Get the 2D coordinates and 3D coordinates of each corner point detected in the grayscale image and integrate them into arrays respectively; Draw the detected corner points on the grayscale image and resize the grayscale image; In step 15), based on the obtained three-dimensional coordinate array, two-dimensional coordinate array and the size of the grayscale image, the cv2.calibrateCamera function is used to obtain the intrinsic parameter matrix and distortion coefficient of the camera.

4. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 3 is characterized in that: The step 3) includes the following sub-steps: 31) Initialize the April Tag detector and specify the type of April Tag to be detected as "tag36h11" through the pl.Detector function; 32) Use the cv2.VideoCapture function to open the camera and loop through the captured camera video images; 33) The captured image is subjected to dedistortion processing. The dedistortion process is as follows: Get the size of the captured image; Use the cv2.getOptimalNewCameraMatrix function to process the camera internal parameter matrix K and the distortion coefficient dist to obtain the new undistorted camera parameter K new and size roi; Through K, dist and K new , use the cv2.undistort function to dedistort the image and get the dedistorted image; The dedistorted image is cropped according to the roi parameters to obtain the final dedistorted image; 34) graying the dedistorted image; 35) Use the detector.detect function to identify and detect the April Tag image in the grayscale image.

5. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 4 is characterized in that: The step 4) includes the following sub-steps: 41) Get the encoded ID number tag_id of April Tag and use cv2.putText function to display it in the specified position in the image with the specified font and color; 42) Get the pixel coordinates of the four corner points and the center point of the April Tag image, and use the cv2.circle function to draw circles of different colors with these points as the center to mark each corner point; 43) Use the cv2.line function to draw green straight lines connecting two adjacent corner points to mark the entire April Tag graph.

6. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 5 is characterized in that: The step 5) includes the following sub-steps: 51) Set the three-dimensional coordinates pst3d of the four corner points according to the size of the arranged April Tag image, and obtain the two-dimensional pixel coordinates pst2d of the corner points; 52) Based on pst3d, pst2d, camera parameters K and distortion coefficient dist, obtain the 3×3 rotation matrix R and 3×1 translation vector t as follows: The coordinates of the four corner points of April Tag pst3d are i is the number of corner points, here i=1,…,4; then the corresponding pixel coordinates are M i =RX i +t; The relationship between the feature point coordinates and pixel coordinates in the image is recorded as s is a defined unknown pixel coordinate constant, then it can be obtained from the above formula In the formula, r i is the i-th column of the rotation matrix R; Since K is obtained by camera calibration, we can get (r1 r2 t) = K -1 H=sK -1 U Y H (YY H ) -1 , where In the formula, the superscript H represents the conjugate transposed matrix of the matrix; Since r3=r1×r2, r3 can be obtained according to the heterodyne formula, and then the rotation matrix R=[r1 r2 r3] can be obtained; 53) The rotation matrix R is Schmidt orthogonalized to improve its orthogonality as follows: Orthogonalize the rotation matrix R = [r1 r2 r3], then β1 = r1, Then normalize it, we have Then we get the orthogonalized rotation matrix R = [γ1,γ2,γ3].

7. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 6 is characterized in that: The step 6) includes the following sub-steps: 61) Set the convergence threshold tolerance to 10-6, the maximum number of iterations max_iters to 100, and the initial damping factor λ to 0.01; 62) Process the rotation matrix R, translation vector t, camera intrinsic parameter K, and the three-dimensional coordinates pst3d and two-dimensional coordinates pst2d of the April Tag corner point to obtain the Jacobi matrix and residual err: The Jacobi matrix is ​​defined as Where, f is the error vector function, t is the translation vector; There are in The camera change of the corner point three-dimensional coordinate pst3d in the camera coordinate system; Among them, k i,j is the element in the i-th row and j-th column of the camera intrinsic parameter matrix; is the actual 2D pixel coordinate of the corner point; i is related to the number of image corner points, and its value is i=1,…,4; There are Among them, I is the unit matrix, R is the rotation matrix, S is the three-dimensional coordinate of the corner point, and t is the translation vector; For the definition of residual err, we have Among them, f 2i-1 =z i ′ u i ′ -z i ′ u i , f 2i =z i ′ v i ′ -z i ′ v i , i=1,…,4; K is the camera intrinsic parameter matrix, That is, the projected pixel coordinates calculated from the coordinates of the corner point on the plane, R, and t; 63) Process the Jacobi matrix, damping factor λ and residual err to calculate the parameter increment Δθ: Δθ=(H+λI) -1 J T r, where J is the Jacobi matrix, λ is the damping factor, r is the residual, and H is given by H = J T J is calculated, where the superscript T means matrix inversion; 64) The increment of the rotation matrix ΔR and the increment of the translation vector Δt are calculated by the parameter increment, and the new parameter R is obtained by updating them. new and t new ; because The parameter increment can be written as Among them, ΔS and Δρ are both three-dimensional column vectors. The parameter increment is mapped to the matrix ΔT and multiplied by T to obtain the updated matrix T ′ =ΔTT, there is make Then there is Where a is a direction vector with a length of 1, and a^ is the antisymmetric matrix of a; Then we can get the new parameter R new =ΔR·R,t new =ΔR·t+V·ΔS; 65) After one iteration is completed, the convergence of the iteration results is judged; 66) If the value of the parameter increment Δθ is less than the convergence threshold tolerance, the result of this iteration converges and the iteration ends; 67) If it does not converge, use R new and t new Calculate the new residual err new , compared with the residual err before this iteration, the value of the damping factor λ is dynamically adjusted: If err new If the value of is greater than err, increase λ by 10 times, otherwise adjust λ to 1 / 10 and use R new and t new As the input value for the next iteration, until the parameter increment is less than the convergence threshold or the number of iterations reaches the number set by max_iters.

8. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 7 is characterized in that: In step 7), the method of constructing a three-dimensional coordinate system is as follows: 711) setting a group of three 3D points axis, storing the three-dimensional coordinates of the three points, and used to represent the coordinate information of the coordinate axis in the three-dimensional space; 712) Based on the iterated rotation vector and translation vector as well as the camera's intrinsic parameter matrix and distortion coefficient, the three-dimensional coordinates are projected into two-dimensional pixel coordinates: The rotation matrix R is transformed by the cv2.Rodrigues function new Convert back to the corresponding rotation vector r new , use the cv2.projectPoints function to rotate the vector r new , translation vector t new And the camera parameters K and dist are processed to obtain the projection coordinate group imgpts of axis on the plane; 713) Select the first corner point corner in the corner point coordinates of April Tag pst2d as the origin of the coordinate system, and use the cv2.line function to connect corner with the three projection point coordinates in imgpts with straight lines of different colors to draw the coordinate system.

9. The method for April Tag positioning and posture calculation based on LM algorithm according to claim 8, characterized in that: In step 7), the method of drawing an arrow on the image is as follows: 721) respectively calculate the horizontal coordinate difference and the vertical coordinate difference of the projection point and the origin of the Y axis of the attitude coordinate system: Select the coordinates of the projection point imgpts[1] in the coordinate system to draw the Y axis, and calculate dx=imgpts[1][0]-corner[0], dy=imgpts[1][1]-corner[1]; where imgpts[1][0] and imgpts[1][1] are the x-coordinate value and y-coordinate value of the projection point respectively; corner[0] and corner[1] are the x-coordinate value and y-coordinate value of the origin respectively, and dx and dy are the difference between the horizontal and vertical coordinates respectively; 722) Based on the difference between the two coordinates, the deflection angle is calculated by the inverse tangent function, and the radian is converted into an angle. If the horizontal coordinate difference is zero, the angle is judged to be 90° or 270°: Use math.atan2 function to calculate the deflection angle. Calculate the value and use the math.degrees function to convert radians to angles; 723) Starting from the center of April Tag, draw an arrow in the same direction as the positive direction of the camera image, and display the deflection angle near the arrow in the image: Calculate the difference Δx between the horizontal coordinates and Δy between the two adjacent corner points, use cv2.arrowedLine to start from the center coordinates (cX, cY) of the AprilTag graph and end at (cX+Δx, cY+Δy), draw an arrow, and display the deflection angle angle near the arrow.

Images