April tag positioning and attitude solving method based on LM algorithm

Abstract

The application discloses an April Tag positioning and pose solution method based on an LM algorithm, and belongs to the technical field of computer vision. 1) camera calibration is performed on a camera used for positioning; 2) an April Tag image is arranged on a carrier plane to be positioned, and the camera is used for shooting; 3) the shot April Tag image is preprocessed; 4) the preprocessed April Tag image is analyzed, and the image is labeled; 5) the image is preliminarily subjected to pose solution; 6) the LM algorithm is used for iteration; 7) a three-dimensional coordinate system is constructed to represent the camera pose, an arrow is drawn on the image, and the deflection direction of the arrow and the camera is displayed. The application has the beneficial effects that the result of the pose solution is more accurate through the iteration of the LM algorithm, and the deflection angle of the arrow can be calculated and drawn, so that the deflection difference between the direction of the carrier on the two-dimensional plane and the positive direction of the camera can be more intuitively and conveniently observed.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of computer vision, and particularly relates to an April Tag positioning and pose solving method based on an LM algorithm. BACKGROUND

[0002] In recent years, with the rapid development of science and technology, the level of new intelligent transportation technology is also continuously improved, among which, the application prospect of unmanned ships is becoming more and more broad, and plays an important role in intelligent shipping, ocean self-source exploration and many other fields. For unmanned control technology, accurate positioning technology is a key prerequisite for realizing autonomous navigation and operation.

[0003] Although the traditional positioning method such as GPS positioning can provide relatively extensive position information in the global range, there may be limitations such as signal interference, positioning accuracy decline 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 and obtain other information of the environment, and visual positioning mainly relies on receiving environmental light signals without actively emitting signals, so that this technology has a unique advantage in some occasions where active signal emission is not allowed.

[0004] April Tag is a visual fiducial marker system composed of labels with specific encoding and patterns. Equipped with a camera to capture April Tag image information, through image processing algorithms and computer vision technology, the label's position, attitude and other key information can be obtained by recognizing, decoding, and feature extraction and analysis. Compared with two-dimensional codes, April Tag has simple encoding and focuses on quickly and accurately transmitting identification information, which is more efficient for the identification and positioning of unmanned ships; and April Tag has strong anti-interference ability and can maintain high recognition accuracy under complex lighting conditions, and has better robustness for image distortion, deformation and other situations; in addition, April Tag also has the advantages of long recognition distance, fast detection speed, relatively simple deployment and relatively low hardware requirements, which is very suitable for visual positioning.

[0005] In a conventional April Tag image-based pose solving method, the P4P algorithm is an important solving method, which aims to calculate the position and pose of the camera through the projection information of four known space points on the image plane. However, the P4P algorithm also has some shortcomings. It is extremely sensitive to noise. In the actual image acquisition process, due to factors such as light changes and image sensor noise, the image data obtained often contains noise interference, which can cause the P4P algorithm to produce large errors in the solving process, affecting the accuracy of the pose solution. SUMMARY

[0006] To solve the above problems, the present application provides an April Tag positioning and pose solving method based on the LM algorithm. The present application is realized by the following technical solutions.

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

[0008] 1) Camera calibration of the positioning camera is performed to obtain the parameters of the camera;

[0009] 2) An April Tag image is arranged on the carrier plane that needs to be positioned using the April Tag image, and the camera is used to take pictures above it. The types and directions of the April Tag images arranged on different carriers are consistent, and the ID numbers are different;

[0010] 3) The April Tag image taken by the camera is preprocessed;

[0011] 4) The preprocessed April Tag image is analyzed, and the image is labeled;

[0012] 5) The image is subjected to preliminary pose solving, and the P4P algorithm is used to process the information of the taken April Tag image to obtain an orthogonalized rotation matrix and a translation vector;

[0013] 6) The translation vector and the orthogonalized rotation matrix are iterated using the LM algorithm to obtain a new rotation matrix and a translation vector;

[0014] 7) A three-dimensional coordinate system is constructed to represent the camera pose, an arrow is drawn on the image, and the deflection direction of the arrow and the camera is displayed.

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

[0016] 11) A camera calibration board with a 6x4 chessboard pattern of corner points is arranged on the plane;

[0017] 12) Use the camera to take 20 images above the camera calibration board, store them in the specified path, and 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) Based on the information obtained by analysis and processing, calibrate the camera.

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

[0022] Set the threshold standard for corner detection;

[0023] Convert the image to a gray image by cv2.cvtColor function;

[0024] Loop to find the checkerboard corner points in the gray image until the number of corner points meets the requirements;

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

[0026] Get the two-dimensional coordinates and three-dimensional coordinates of each corner point detected in the gray image, and integrate them into arrays respectively;

[0027] Draw the detected corner points on the gray image and adjust the size of the gray image;

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

[0029] Further, in the 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 take and capture the camera video frame;

[0032] 33) De-distort the taken image, and the de-distortion process is as follows:

[0033] Get the size of the taken image;

[0034] The camera intrinsic parameter matrix K and the distortion coefficient dist are processed using the cv2.getOptimalNewCameraMatrix function to obtain a new non-distorted camera parameter K new and size roi;

[0035] The K, dist and K new undistort function is used to undistort the image to obtain a non-distorted image;

[0036] The non-distorted image is cropped according to the roi parameter to obtain the final non-distorted image;

[0037] 34) The non-distorted image is processed by grayscale processing;

[0038] 35) The April Tag image in the grayscale image is recognized by the detector.detect function.

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

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

[0041] 42) The pixel coordinates of the four corner points and the center point of the April Tag image are obtained, and the cv2.circle function is used to draw different colored circles to mark each corner point;

[0042] 43) The cv2.line function is used to draw a green straight line to connect two adjacent corner points to mark the entire AprilTag image.

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

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

[0045] 52) Based on ps 3d, pst2d, camera parameter K and distortion coefficient dist, a 3x3 rotation matrix R and a 3x1 translation vector t are obtained, and the method is as follows:

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

[0047] The relationship between the feature point coordinates and the pixel coordinates in the image is recorded as s is a constant unknown pixel coordinate, and then the above formula can be obtained as H=K(r1r2t); in the formula, r i is the i-th column of the rotation matrix R;

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

[0049] Since r3=r1xr2, r3 can be obtained according to the extrinsic formula, and then the rotation matrix R=[r1r2r3] is obtained;

[0050] 53) The rotation matrix R is subjected to Schmidt orthogonalization to improve its orthogonality, and the method is as follows:

[0051] Orthogonalizing the rotation matrix R=[r1r2r3], then β1=r1,

[0052]

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

[0054] Further, in the 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, the translation vector t, the camera intrinsic parameter K, the three-dimensional coordinates pst3d and the two-dimensional coordinates pst2d of the April Tag corner point, to obtain the Jacobi matrix and the residual error err:

[0057] The definition of the Jacobi matrix is Wherein, f is an error vector function, and t is a translation vector;

[0058] Also Wherein is the camera change of the three-dimensional coordinate pst3d of the corner point in the camera coordinate system;

[0059] where k i,j is the element of the i-th row and j-th column of the camera intrinsic 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 the value i = 1, …, 4;

[0060] Again, there are where 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;

[0061] For the definition of the residual error err, there are 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 point on the plane, R, and t;

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

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

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

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

[0066] Let then there are In the formula, a is a direction vector with a length of 1, a^ is the anti-symmetric matrix of a;

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

[0068] 65) After one iteration, the convergence of the current iteration result is judged;

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

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

[0071] If the value of err new is greater than err, 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 of times set by max_iters.

[0072] Further, in the 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 the three-dimensional space;

[0074] 712) Based on the rotation vector and the translation vector after iteration, as well as the intrinsic parameter matrix and the distortion coefficient 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 by the cv2.Rodrigues function, process the rotation vector r new , the translation vector t new , and the camera parameters K and dist by the cv2.projectPoints function, and obtain the projection coordinate group imgpts of axis on the plane;

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

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

[0078] 721)Calculate the horizontal coordinate difference and the vertical coordinate difference of the projection point and the origin of the Y-axis of the pose coordinate system respectively:

[0079] Select the projection point coordinate imgpts[1] of the Y-axis in the coordinate system, calculate dx=imgpts[1][0]-corner[0] and dy=imgpts[1][1]-corner[1]; wherein 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 horizontal coordinate difference and the vertical coordinate difference respectively;

[0080] 722)Based on the two coordinate differences, calculate the deflection angle by the inverse tangent function, and convert the radian to angle, if the horizontal coordinate difference is zero, judge the angle as 90° or 270°:

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

[0082] 723)Draw an arrow with the center of the April Tag as the starting point, which is in the same direction as the positive direction of the camera shooting picture, and display the deflection angle near the arrow in the image:

[0083] Calculate the horizontal coordinate difference Δx and the vertical coordinate difference Δy of two adjacent corners, use cv2.arrowedLine to draw an arrow from the center coordinate (cX, cY) of the April Tag picture to (cX+Δx, cY+Δy), and display the deflection angle angle near the arrow.

[0084] The beneficial effects of the present application are that the result of the pose solution is more accurate by using the LM algorithm iteration, and the deflection difference between the direction of the carrier in the two-dimensional plane and the positive direction of the camera can be more intuitively and conveniently seen by calculating and drawing the arrow of the deflection angle. BRIEF DESCRIPTION OF DRAWINGS

[0085] In order to more clearly illustrate the technical solutions of the present application, the drawings needed in the specific implementation description will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

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

[0087] Figure 2 : flow chart of the April Tag positioning and pose solving method based on the LM algorithm;

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

[0089] Figure 4 : schematic diagram of the image de-warping process;

[0090] Figure 5 : schematic diagram of the positioning marker of April Tag;

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

[0092] Figure 7 : schematic diagram of the drawing of the pose coordinate axis and deflection arrow of April Tag. DETAILED DESCRIPTION

[0093] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0094] As shown in Figures 1-7 , an April Tag positioning and pose solving method based on the LM algorithm includes the following steps:

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

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

[0097] 11) A camera calibration board with a 6x4 chessboard pattern and 6x4 corner points is arranged on a plane;

[0098] 12) Use the camera to take 20 images above the camera calibration board, store them in the specified path, and 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 at the specified path, process and analyze the images, and the processing and analysis process of the gray-scale image is as follows:

[0101] Set the threshold standard for corner detection;

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

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

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

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

[0106] Draw the detected corner points 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 cv2.calibrateCamera function to get the intrinsic parameter matrix and distortion coefficient of the camera.

[0108] 2) Arrange an April Tag on the carrier plane that needs to be positioned using the April Tag image, and use the camera to take a picture above it. The types and directions of the April Tags arranged on different carriers are consistent, and the ID numbers are different.

[0109] 3) Preprocess the April Tag image taken by the camera, and the specific process is as shown in Figure 4

[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 reference system commonly used in computer vision applications. April Tag has multiple types, such as Figure 1 ​As shown, the tag 36h11 used in the present application is the most common type, 36 in tag 36h11 represents that the tag code is 36 bits, h11 represents the minimum Hamming distance, which is used to enhance the anti-noise ability, tag 36h11 is a 6*6 black and white square, the four sides are black frames, the internal binary mode suitable for coding, and the ID number can be 0 to 586, the 6*6 square can make the information checked more accurate and less error rate;

[0112] 32) Open the camera using the cv2.VideoCapture function, and loop to capture the camera video picture;

[0113] 33) The image taken is deformed, and the deforming process is as follows:

[0114] Get the size of the captured image;

[0115] Use the cv2.getOptimalNewCameraMatrix function to process the camera internal parameter matrix K and the distortion coefficient dist, and obtain the new non-distorted camera parameter K new And size roi;

[0116] Through K, dist and K new , use the cv2.undistort function to de-distort the image, and get the de-distorted image;

[0117] According to the roi parameter, the de-distorted image is cropped to obtain the final de-distorted image;

[0118] 34) The de-distorted image is grayed;

[0119] 35) The April Tag image in the gray image is recognized and detected by the detector.detect function.

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

[0121] Including the following sub-steps:

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

[0123] 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 centers to mark the corner points;

[0124] 43) Draw a straight line with green color to connect two adjacent corner points using cv2.line function to mark the whole AprilTag image.

[0125] 5) Perform the initial pose calculation on the image, use P4P algorithm to process the information of the captured AprilTag image, and get the orthogonalized rotation matrix and translation vector.

[0126] Including the following sub-steps:

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

[0128] 52) Based on pst3d, pst2d, camera parameters K and distortion coefficients dist, obtain a 3x3 rotation matrix R and a 3x1 translation vector t, as follows:

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

[0130] The relationship between the feature point coordinates and the pixel coordinates in the image is recorded as s is a constant unknown pixel coordinate, then from the above formula H = K(r1r2 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 (r1r2 t) = K -1 H = sK -1 UY H (YY H ) -1 , in which, In the formula, the superscript H represents the conjugate transpose matrix of the matrix;

[0132] Since r3 = r1x r2, then r3 can be obtained according to the extraneous formula, and then the rotation matrix R = [r1r2r3] is obtained;

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

[0134] Orthogonalize the rotation matrix R = [r1r2r3], then β1 = r1,

[0135] After normalization, we have Then we get the orthogonalized rotation matrix R = [γ1, γ2, γ3].

[0136] 6) Using the LM algorithm to iterate the translation vector and the orthogonalized rotation matrix, we get a new rotation matrix and translation vector, the specific process is shown in Figure 6 .

[0137] Including 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, the translation vector t, the camera intrinsic parameter K, the three-dimensional coordinates pst3d of the April Tag corner point and the two-dimensional coordinates pst2d, to obtain the Jacobi matrix and the residual error 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 three-dimensional coordinates pst3d of the corner point in the camera coordinate system;

[0142] 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 two-dimensional pixel coordinates of the corner point; i is related to the number of image corner points, and takes the value i = 1, …, 4;

[0143] Also Where I is the identity matrix, R is the rotation matrix, S is the three-dimensional coordinates of the corner point, and t is the translation vector.

[0144] For the definition of the residual error 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 coordinates calculated by the coordinates of the corner point on the plane, R, and t.

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

[0146] Δθ = (H + λI) -1 J T r, where J is the Jacobi matrix, λ is the damping factor, and r is the residual. H is obtained by H = J T J, where the superscript T denotes matrix inversion;

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

[0148] Since the parameter increment can be written as where ΔS and Δρ are three-dimensional column vectors that map the parameter increment to a matrix ΔT, which is multiplied by T to obtain the updated matrix T' = ΔTT, and

[0149] Let then where a is a direction vector with a length of 1, and a^ is the skew-symmetric matrix of a;

[0150] The new parameters R new = ΔR·R, t new = ΔR·t + V·ΔS

[0151] 65) After one iteration, perform convergence judgment on the current iteration result;

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

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

[0154] If the value of err new is greater than err, increase λ by a factor of 10, 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 of iterations set by max_iters.

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

[0156] The method of constructing a 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 the translation vector after iteration, as well as the intrinsic parameter matrix and distortion coefficient 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 , process the rotation vector r new , the translation vector t new , and the camera parameters K and dist using the cv2.projectPoints function, and obtain the projection coordinate group imgpts of axis on the plane;

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

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

[0162] 721) Calculate the horizontal coordinate difference and vertical coordinate difference between the projection point and the origin of the Y-axis of the pose coordinate system respectively:

[0163] Select the projection point coordinate imgpts[1] of the Y-axis in the coordinate system, calculate dx = imgpts[1][0] - corner[0] and 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 horizontal coordinate difference and vertical coordinate difference respectively;

[0164] 722) Based on the two coordinate differences, calculate the deflection angle by the inverse tangent function, and convert the radian to degrees. If the horizontal coordinate difference is zero, the angle is judged to be 90° or 270°:

[0165] The math.atan2 function is used to calculate the deflection angle, The numerical value is calculated and the math.degrees function is used to convert the radian to the angle angle;

[0166] 723) An arrow is drawn from the center of the April Tag in the same direction as the positive direction of the camera shooting picture, and the deflection angle is displayed near the arrow in the image:

[0167] The horizontal coordinate difference Δx and the vertical coordinate difference Δy of two adjacent corner points are calculated, an arrow is drawn from the center coordinates (cX, cY) of the April Tag picture to (cX+Δx, cY+Δy) using cv2.arrowedLine, and the deflection angle angle is displayed near the arrow.

[0168] The preferred embodiments of the application disclosed above are only used to help explain the application. The preferred embodiments do not describe all the details and limit the application to the specific embodiments. Obviously, many modifications and changes can be made according to the content of the specification. The specification selects and describes these embodiments in order to better explain the principles and practical applications of the application, so that those skilled in the art can well understand and utilize the application. The application is limited by the claims and their full scope and equivalents.

Claims

1. An April Tag positioning and pose solving method based on an LM algorithm, characterized in that, The method comprises the following steps: 1) camera calibration is performed on the camera used for positioning to obtain the parameters of the camera; 2) an April Tag image is arranged on the carrier plane that needs to be positioned using the April Tag image, and the camera is used to take a picture above the April Tag image; the types, directions, and ID numbers of the April Tag images arranged on different carriers are consistent; 3) the April Tag image taken by the camera is preprocessed; 4) the preprocessed April Tag image is analyzed, and the image is labeled; 5) the image is subjected to preliminary pose calculation, and the P4P algorithm is used to process the information of the taken April Tag image to obtain an orthogonalized rotation matrix and a translation vector; Step 5) comprises the following sub-steps: 51) Set the three-dimensional coordinates of the four corner points according to the size of the arranged April Tag picture , get the two-dimensional pixel coordinates of the corner points ; 52) based on , , camera parameters K and distortion coefficients , a 3x3 rotation matrix and a 3x1 translation vector are obtained as follows: The coordinates of the four corners of the April Tag For , The number of corners is The corresponding pixel coordinates are ; The relationship between the feature point coordinates and the pixel coordinates in the image is denoted as , is a constant unknown pixel coordinate defined by the above equation , ; in which, is the first column of the rotation matrix R; and is the second column of the rotation matrix R. Since K is obtained from camera calibration, we have , where , , where the superscript H denotes the conjugate transpose of the matrix. Since r3 is then obtained from the heterodyne formula, and the rotation matrix ; 53) to the rotation matrix Schmidt orthogonalization is performed to improve its orthogonality, as follows: On a rotation matrix Orthogonalization is performed, then , , ; After normalization, we have The rotation matrix after orthogonalization is ; 6) the translation vector and the orthogonalized rotation matrix are subjected to iteration using the LM algorithm to obtain a new rotation matrix and a translation vector; 7) a three-dimensional coordinate system is constructed to represent the camera pose, an arrow is drawn on the image, and the deflection direction of the arrow and the camera is displayed.

2. The April Tag positioning and pose solving method based on the LM algorithm according to claim 1, characterized in that, The step 1) comprises the following sub-steps: 11) a camera calibration board with a 6×4 chessboard pattern is arranged on a plane; 12) 20 images are taken above the camera calibration board using the camera, and are stored in a specified path; the height of the camera for taking pictures is the positioning height of the April Tag; 13) a three-dimensional coordinate matrix of the chessboard points in the world coordinate system and its corresponding two-dimensional coordinates are constructed; 14) a series of chessboard images taken in the specified path are read, and the images are processed and analyzed; 15) camera calibration is performed based on the information obtained by the analysis and processing.

3. The April Tag positioning and pose solving method based on the LM algorithm according to claim 2, characterized in that, In step 14), the processing and analysis process of the gray image is as follows: The threshold standard for corner detection is set; By The function converts the image to a grayscale image; The chessboard corners in the gray image are searched in a loop until the number of corners meets the requirements; Based on the corner position in the gray-scale image and a detection threshold, the corner position is refined using a function. The two-dimensional coordinates and three-dimensional coordinates of the detected corners in the gray image are obtained and integrated into arrays, respectively; The detected corners are drawn on the gray image, and the size of the gray image is adjusted; In step 15), based on the obtained three-dimensional coordinate array, two-dimensional coordinate array, and size of the gray-scale image, the intrinsic parameter matrix and distortion coefficient of the camera are obtained using the function function.

4. The April Tag positioning and pose solving method based on the LM algorithm according to claim 3, characterized in that, In step 3), the following sub-steps are included: 31) initialize the April Tag detector by the function specifies the kind of April Tag to be detected as "tag36h11"; 32) using The function opens the camera and loops to take captures of the camera video frames. 33) the taken image is subjected to distortion correction, and the process of distortion correction is as follows: The size of the taken image is obtained; Using the function to the camera intrinsic parameter matrix and distortion coefficients processing, get new non-distorted camera parameters and size ; By , and , the image is de-warped using the function to obtain a de-warped image; According to The parameters are used to crop the de-distorted image to obtain a final de-distorted image. 34) the distortion-corrected image is subjected to grayscale processing; 35) by The function recognizes the April Tag image in the grayscale image.

5. The April Tag positioning and pose solving method based on the LM algorithm according to claim 4, characterized in that, In step 4), the following sub-steps are included: 41) Get the encoded ID number of the April Tag , using the function to display it in the image at the specified location with the specified font and color; 42) Get the pixel coordinates of the four corners and the center point of the April Tag image, using The function draws circles of different colors to mark the corners, respectively, with these points as the centers. 43) using The function draws a green line connecting every two adjacent corner points to mark the entire April Tag image.

6. The April Tag positioning and pose solving method based on the LM algorithm according to claim 5, characterized in that, In step 6), the following sub-steps are included: 61) Set convergence threshold to 10"6, maximum number of iterations to 100, initial damping factor to 0.01; 62) rotation matrix , translation vector , camera intrinsic , three-dimensional coordinates of April Tag corners and two-dimensional coordinates processing, obtaining matrix and residual : The matrix is defined as where f is the error vector function and t is the translation vector. Also wherein is the camera change of the corner point three-dimensional coordinate pst3d in the camera coordinate system; , wherein, is an element of the i-th row and the j-th column of the intrinsic camera parameter matrix; is the actual two-dimensional pixel coordinate of the corner point; i relates to the number of image corners and takes the values i = 1,..., 4;​​ Also where I is the identity matrix, R is a rotation matrix, S is the three-dimensional coordinates of the corner point, and t is a translation vector. For the definition of the residual , where , , i = 1, …, 4; , K is the intrinsic parameter matrix of the camera, , i.e. the projection pixel coordinates calculated from the coordinates of the corner points on the plane, , . 63) to matrix, damping factor and residual processing, to obtain parameter increments : where J is the Jacobian matrix, l is the damping factor, r is the residual, and H is computed as where the superscript T denotes matrix inversion. 64) Compute the increment of the rotation matrix from the parameter increment and the increment of the translation vector and update the new parameters accordingly and ; Since , the parameter increment can be written as where are three-dimensional column vectors, mapping the parameter increment to a matrix , multiplying by T, gives the updated matrix , with ; Let then , , where is a directional vector of length 1, is antisymmetric matrix of then new parameters can be obtained , ; 65) after one iteration, the convergence of the iteration result is judged; 66) If the value of the parameter increment is less than the convergence threshold , then the result of this iteration converges, and the iteration ends. 67) If not converged, use and Compute new residual Compare to residual before this iteration to dynamically adjust the damping factor value: If is greater than , then is increased by a factor of 10, otherwise is adjusted by a factor of 1 / 10, and and are used as the input values for the next iteration until the parameter increment is less than a convergence threshold or the number of iterations reaches the number of iterations set.

7. The April Tag positioning and pose solving method based on the LM algorithm according to claim 6, characterized in that, In step 7), the method for constructing the three-dimensional coordinate system is as follows: 711) setting a set of three 3D points , storing three-dimensional coordinates of the three points, for representing coordinate information of the coordinate axes in the three-dimensional space; 712) based on the iteration result, the rotation vector and the translation vector, and the intrinsic parameter matrix and the distortion coefficient of the camera, the three-dimensional coordinates are projected into two-dimensional pixel coordinates: By the function RotMatrix2RotVector converts a rotation matrix back to the corresponding rotation vector By the function RotVector2RotMatrix , the rotation vector , the translation vector and the camera parameters are processed to obtain a set of projection coordinates on the plane 713) Select the corner point coordinates of April Tag The first corner point in As the origin of the coordinate system, use The function respectively connects With The three projection point coordinates in, draw the coordinate system.

8. The April Tag positioning and pose solving method based on the LM algorithm according to claim 7, characterized in that, In step 7), the method for drawing an arrow on the image is as follows: 721) the horizontal coordinate difference and the vertical coordinate difference of the projection point and the origin of the Y-axis of the pose coordinate system are calculated, respectively: Selecting the projection point coordinates of the Y-axis in the coordinate system , calculating , ; wherein, and are the x-coordinate value and the y-coordinate value of the projection point, respectively; and are the x-coordinate value and the y-coordinate value of the origin, respectively, and are the horizontal coordinate difference value and the vertical coordinate difference value, respectively. 722) based on the two coordinate differences, 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°: Using the function calculates the deflection angle, , calculates the numerical value, and uses the function to convert radians to degrees ; 723)Take the center of the April Tag as the starting point, draw an arrow in the same direction as the positive direction of the camera shooting picture, and display the deflection angle near the arrow in the image: Calculate the horizontal coordinate difference of two adjacent corner points and the vertical coordinate difference , use the center coordinates of the AprilTag image as the starting point, draw an arrow to the end point , and display the yaw angle near the arrow.

Images