A box workpiece pose measurement method based on point features

Abstract

The present application relates to the technical field of automation control, and especially relates to a box workpiece pose measurement method based on point features, comprising calibrating a monocular camera by Zhang Zhengyou calibration method, obtaining internal parameters and distortion parameters of the monocular camera; selecting four corner points of four box workpieces in a three-dimensional space as feature points in an image, establishing a world coordinate system, a camera coordinate system, an image coordinate system and a pixel coordinate system, and obtaining three-dimensional coordinates of the feature points in the world coordinate system; using the point features to perform image processing on the preprocessed image, obtaining two-dimensional coordinates of the feature points in the pixel coordinate system; combining the three-dimensional coordinates of the feature points in the world coordinate system, the two-dimensional coordinates in the pixel coordinate system and the internal parameters of the monocular camera, and using a PNP measurement method to solve the pose information of the box workpiece. The present application provides a solution to the problem that the existing pose measurement method is complicated and cannot measure the pose of small box workpieces.

Description

Technical Field

[0001] This invention relates to the field of automation control technology, and in particular to a method for measuring the pose of a box-shaped workpiece based on point features. Background Technology

[0002] In recent years, with the continuous development of science and technology, industrial robots have been widely used in large-scale industries such as aviation, automotive, product packaging and electronic packaging, replacing manual labor in many tasks including handling, processing, packaging and sorting. Grasping workpieces is a common operation that industrial robots on production lines need to perform. On traditional production lines, industrial robots that perform grasping operations generally use methods such as teach-and-playback or offline programming. The initial and final poses of the grasped target are strictly set in advance. When the pose of the workpiece on the platform is not given, the robot cannot accurately grasp the workpiece because the initial pose cannot be obtained.

[0003] Chinese patent CN111089569A discloses a method for measuring large boxes based on monocular vision, comprising: installing a recognition target on the box to be measured; acquiring images of the box containing the recognition target using a monocular camera; establishing a world coordinate system and obtaining the three-dimensional coordinates of the recognition target marker points; installing two monocular cameras, wherein the first monocular camera is used to acquire side images of the box to be measured, and the second monocular camera is used to acquire top images of the box to be measured; calibrating the two monocular cameras to obtain camera intrinsic parameters; acquiring images of the box containing the recognition target using each monocular camera to obtain the pixel coordinates of the target marker point images for pose measurement of the box to be measured; measuring the position of the recognition target on the side of the box relative to the first monocular camera and the orientation of the recognition target on the top of the box relative to the second monocular camera using the P5P measurement method; and transmitting the position and orientation of the box to be measured to the robotic arm control system to assist in guiding the robotic arm to complete the grasping operation. However, existing pose measurement methods based on monocular vision require the use of two monocular cameras simultaneously, and the recognition target needs to be installed on the box being measured beforehand, which is not suitable for small boxes. Based on this, we propose a pose measurement method for box workpieces based on point features. Summary of the Invention

[0004] To address the shortcomings of existing algorithms, this invention provides a solution to the problems of cumbersome measurement methods and inability to measure the pose of small boxes. Furthermore, existing box pose measurement methods require the simultaneous use of two monocular cameras and the prior installation of recognition targets on the box, which is often impossible due to the size limitations of small boxes.

[0005] The technical solution adopted in this invention is: a method for measuring the pose of a box-shaped workpiece based on point features, comprising the following steps:

[0006] Step 1: Install the monocular camera and calibrate it using the Zhang Zhengyou calibration method to obtain the internal parameters and distortion parameters of the monocular camera;

[0007] Step 2: Use a monocular camera to acquire images of the box-shaped workpiece. Select four corner points of the box-shaped workpiece that are coplanar in three-dimensional space in the image as feature points. Establish the world coordinate system, camera coordinate system, image coordinate system and pixel coordinate system, and obtain the three-dimensional coordinates of the feature points in the world coordinate system.

[0008] Step 3: Preprocess each frame of the acquired raw image;

[0009] Furthermore, the image preprocessing mainly includes the following steps:

[0010] 1) Use the distortion parameters of the monocular camera obtained in step one to correct the distortion of the original image;

[0011] 2) Detect the box-shaped workpiece in the image, select the box-shaped workpiece, and perform masking on the remaining part of the image.

[0012] Step 4: Perform image processing on the preprocessed image using point features to obtain the two-dimensional coordinates of the feature points in the pixel coordinate system;

[0013] Furthermore, image processing is performed on the preprocessed image using point features to obtain the two-dimensional coordinates of the feature points in the pixel coordinate system, including the following steps:

[0014] Step 41: Perform edge detection on the preprocessed image to obtain the edge pixels of the image;

[0015] Step 42: Use Hough transform to perform line detection on the edge pixels of the image;

[0016] Step 43: Add a line width d to the detected line, and use the weighted least squares method to iteratively fit the line to the detection domain with a line width of d;

[0017] Step 44: Merge multiple straight lines that break at the same edge;

[0018] Furthermore, step 44 specifically includes:

[0019] Step 441: Extract the slope of the lines with the same edge and calculate the average slope v. v is the slope of the fitted line.

[0020] Step 442: Calculate the slope c of the line perpendicular to the fitted line and the similar line;

[0021] Step 443: Extract the coordinates A1 and B1 of the endpoints of the line with the largest difference in slope from the fitted line, and find the perpendicular lines with slope c passing through A1 and B1 respectively.

[0022] Step 444: Find the coordinates of the intersection points of the two perpendicular lines with other lines on the same edge. Calculate the average value of the coordinates of the intersection points of each perpendicular line with other lines on the same edge to obtain two coordinate points A and B.

[0023] Step 445: Replace the original straight line with the straight line connecting points A and B.

[0024] Step 45: Calculate the coordinates of the intersection of adjacent straight lines, which are the two-dimensional coordinates of the corner points of the box workpiece in the pixel coordinate system, and then filter out the two-dimensional coordinates of the feature points.

[0025] Step 5: Combining the three-dimensional coordinates of the feature points in the world coordinate system, the two-dimensional coordinates in the pixel coordinate system, and the internal parameters of the monocular camera, the pose information of the box-shaped workpiece is solved using the PNP measurement method.

[0026] Furthermore, the coordinates of the feature points of the box-shaped workpiece in the image coordinate system correspond to the coordinates of the feature points in the three-dimensional world coordinate system;

[0027] Furthermore, the PNP measurement method combines the calibrated camera intrinsic parameters with the known three-dimensional coordinates of feature points in the world coordinate system and their two-dimensional coordinates projected onto the image plane to calculate the rotation and translation relationship between the world coordinate system and the camera coordinate system where the box-shaped workpiece is located.

[0028] Furthermore, the PNP measurement method is as follows:

[0029] A point P in space w The transformation relationship between the coordinates of the point in the world coordinate system and the coordinates projected onto the image coordinate system can be written as the following model:

[0030]

[0031] In the formula, (X w ,Y w Z w ,1) is the homogeneous coordinate form of a point in space in the world coordinate system, (X c ,Y c Z c Z is the coordinate form of a point in space in the camera coordinate system. c (u, v, 1) represents the coordinates of a spatial point on the axis perpendicular to the image plane in the camera coordinate system. (u, v, 1) is the homogeneous coordinate form of the spatial point in the image coordinate system. f, dX, dY, u0, v0 are all intrinsic parameters of the camera, where f is the focal length of the camera lens, dX and dY are the width and height of each pixel on the camera image sensor, and u0 and v0 are the coordinates of the origin of the image coordinate system in the pixel coordinate system. ij These are the camera's rotation matrix parameters, ti Let be the translation vector parameter of the camera. From the camera imaging model, the following expression can be derived:

[0032]

[0033] Substituting equation (3) into equations (1) and (2), we get:

[0034]

[0035] In the formula, after obtaining the coordinates in the world coordinate system, the coordinates in the pixel coordinate system, and the camera's intrinsic parameters, the unknown quantity is r. 11 ,r 12 ,r 13 ,r 21 ,r 22 ,r 23 ,r 31 ,r 32 ,r 33 There are 12 unknowns in total, t1, t2, and t3. Since the rotation matrix is ​​an orthogonal matrix, with each row and column being a unit vector that is pairwise orthogonal, we only need to obtain 3 unknowns of the rotation matrix to get the other 6. Adding the 3 unknowns of the translation vector, there are a total of 6 unknowns to solve, thus requiring 6 equations. Each feature point can provide 2 equations, so at least 3 feature points are needed to solve for the extrinsic parameters. The CV_ITERATIVE algorithm provided by OpenCV can find the solution with the minimum reprojection error through iteration. The correct pose data can be obtained if and only if the number of feature points is 4 and the 4 feature points are coplanar.

[0036] The beneficial effects of this invention are:

[0037] 1. Using a single monocular camera to complete the measurement work, compared with a binocular camera or two individual cameras, the structure is simpler and the real-time performance is superior while ensuring measurement accuracy.

[0038] 2. No auxiliary tools such as target identification are needed, making the measurement process more convenient.

[0039] 3. The method of combining Hough line detection and least squares method is used to measure the coordinates of feature points, which has good robustness and high accuracy. Attached Figure Description

[0040] Figure 1 This is a flowchart of the box-shaped workpiece pose measurement method based on point features according to the present invention;

[0041] Figure 2 This is a schematic diagram showing the four coordinate systems that need to be established in the camera calibration and pose measurement experiment of this invention;

[0042] Figure 3 This is a flowchart illustrating the process of obtaining the two-dimensional coordinates of feature points in the pixel coordinate system according to the present invention.

[0043] Figure 4 This is a schematic diagram of the present invention for merging multiple straight lines with similar slopes along the same edge. Detailed Implementation

[0044] The present invention will be further described below with reference to the accompanying drawings and embodiments. The drawings are simplified schematic diagrams, which only illustrate the basic structure of the present invention in a schematic manner, and therefore only show the components related to the present invention.

[0045] like Figure 2 The diagram shows the four coordinate systems required for pose measurement experiments using monocular vision:

[0046] Coordinates in a three-dimensional world coordinate system are the three-dimensional coordinates of an object in the real world. Generally, the plane where feature points are coplanar is set as the z-axis of the coordinate system. The world coordinate system is a reference frame for describing other coordinate systems, denoted by (X...). w ,Y w Z w The camera coordinate system is defined as follows: the z-axis is perpendicular to the imaging plane, the optical axis of the camera is the z-axis of the camera coordinate system, and the optical center is the origin O of the camera coordinate system. c , using (X c ,Y c Z c ) represents the origin O of the image coordinate system. i Located at the origin O of the camera coordinate system c At the imaging plane at the rear end f, the point where the optical axis intersects the imaging plane is located. The x-axis and y-axis are parallel to the long and wide sides of the image sensor, respectively, and are denoted by (x, y). The pixel coordinate system and the image coordinate system are coplanar, with their origin O. p It is the upper left corner of the camera image sensor. The u-axis and v-axis are parallel to the x-axis and y-axis of the image coordinate system, respectively, and are represented by (u,v).

[0047] Existing methods for measuring the pose of box-shaped workpieces require the simultaneous use of two monocular cameras and the prior installation of identification targets on the box. Due to the size limitations of small boxes, there are situations where target installation is not feasible. Therefore, this invention provides a point feature-based method for measuring the pose of box-shaped workpieces, such as... Figure 1 The diagram shown is a general flowchart of the system of the present invention. The specific implementation steps include:

[0048] Step 1: Use Zhang Zhengyou's calibration method to calibrate the monocular camera and obtain its internal parameters and distortion parameters;

[0049] Step 2: Install a monocular camera and use it to acquire images of the box-shaped workpiece. Select four corner points of the box-shaped workpiece that are coplanar in three-dimensional space in the image as feature points. Establish the world coordinate system, camera coordinate system, image coordinate system, and pixel coordinate system, and obtain the three-dimensional coordinates of the feature points in the world coordinate system.

[0050] Step 3: Preprocess each frame of the acquired raw image;

[0051] Step 4: Perform image processing on the preprocessed image using point features to obtain the two-dimensional coordinates of the feature points in the pixel coordinate system;

[0052] Step 5: Combining the three-dimensional coordinates of the feature points in the world coordinate system, the two-dimensional coordinates in the pixel coordinate system, and the internal parameters of the monocular camera, the pose information of the box-shaped workpiece is solved using the PNP measurement method.

[0053] Zhang Zhengyou's calibration algorithm is a very simple and practical calibration method that requires only a chessboard grid. The process of using Zhang Zhengyou's calibration algorithm to calibrate the parameters of a monocular camera can be briefly described as follows:

[0054] In Zhang Zhengyou's calibration, if the world coordinate system is established on the checkerboard plane, the checkerboard in the world coordinate system is Z... w =0, the rotation vector on the z-axis is 0, and we have:

[0055]

[0056] In the formula, k is the scaling factor, (u,v,1) is the homogeneous coordinate form of the spatial point in the image coordinate system, K is the camera intrinsic parameter matrix, and r1 and r2 are X and V, respectively. w With Y w The rotation vector on the axis, t is the translation vector, (X w ,Y w ,1) is the spatial point in the world coordinate system Z w Homogeneous coordinates in a two-dimensional plane where 0 = 0.

[0057] Let H = [h1,h2,h3] = λK[r1,r2,t]. Since the rotation matrix is ​​an orthonormal matrix, it is easy to see that... but:

[0058]

[0059] In the formula, H is the product of the intrinsic parameter matrix and the extrinsic parameter matrix, h i λ is a vector of the product matrix, and λ is a scaling factor.

[0060] make Since K is an upper triangular matrix, it is easy to know that matrix B is a real symmetric matrix. Let L = [b 11 ,b12 ,b 13 ,b 22 ,b 23 ,b 33 ] T Finding the camera's intrinsic parameters is essentially equivalent to finding the vector L. Since vector L has 6 unknowns, at least 3 systems of equations are needed, meaning 3 images are required to solve for the vector. The vector H providing the systems of equations is a homography matrix with 8 degrees of freedom, requiring at least 4 systems of equations to solve for this matrix. Each feature point corresponds to one system of equations, so at least 4 feature points are needed to solve for matrix H. Therefore, theoretically, only 3 images of the calibration board, each with 4 feature points, are needed to obtain the camera's intrinsic parameters. In practical calibration, more images of the calibration board and more feature points can improve calibration accuracy.

[0061] Besides the intrinsic parameters, distortion parameters are another important parameter obtained during calibration. Distortion will cause errors in the coordinates of feature points in the obtained pixel coordinate system, so it is necessary to obtain the camera's distortion parameters to compensate for the errors. Distortion is divided into radial distortion and tangential distortion. Tangential distortion has a negligible impact on the operation. In practical applications, radial distortion k1, k2, k3... is mainly considered. Generally, only k1 and k2 need to be required.

[0062] Figure 3 This illustrates the specific workflow for obtaining the two-dimensional coordinates of feature points in the pixel coordinate system, including the following steps:

[0063] Step 41: Perform edge detection on the image to obtain the edge pixels of the image;

[0064] While the scattered texture on the surface of the workpiece exhibits high contrast between adjacent pixels, the overall contrast is relatively low. Conversely, the edges of the workpiece show high contrast both locally and overall. Applying mean-shift filtering to the image can reduce the interference of scattered texture while preserving edge details. Therefore, a hybrid filter combining Gaussian filtering and mean-shift filtering is used to replace the Gaussian filter in the traditional Canny algorithm for image filtering.

[0065] In the Canny algorithm's double threshold edge determination process, whether a pixel in the image is considered an edge is determined by the gradient value of that pixel. The specific determination steps are as follows:

[0066] 1) If the gradient value of the current edge pixel is greater than or equal to maxVal, then mark the current edge pixel as a strong edge;

[0067] 2) If the gradient value of the current edge pixel is between maxVal and minVal, then mark the current edge pixel as a virtual edge, but retain it;

[0068] 3) If the gradient value of the current edge pixel is less than or equal to minVal, then mark the current edge pixel as non-edge and discard it;

[0069] 4) Determine if a virtual edge is connected to a strong edge. If it is, mark it as a weak edge. Otherwise, treat it as a false edge and discard it. Finally, Canny considers both strong and weak edges as edges of the image.

[0070] The Canny algorithm's double-threshold edge determination method effectively avoids false positives and false negatives. However, for scattered textures that are not completely smoothed after filtering, the Canny algorithm easily identifies them as edges. For scattered textures with discontinuous features and edges of workpiece wear, whose pixel gradient values ​​are not high, mostly between maxVal and minVal, falling into the category of virtual edges, strong edge pixels and retained virtual edge pixels are marked separately. In the cumulative voting process of Hough line detection, strong edge pixels and retained virtual edge pixels are assigned different weights. Weighting the gradient of edge pixels will not affect the detection of edges with a small amount of wear, and can reduce the interference of scattered textures on line detection.

[0071] Step 42: Perform line detection on the edges of the image obtained in Step 41 using the cumulative probability Hough transform;

[0072] The Hough transform is a method for detecting and locating lines in an image. It has advantages such as good robustness and no need for heuristic information. The standard Hough transform (SHT) maps each foreground pixel, so the computational cost of SHT becomes very high when there are enough pixels in the image. The progressive probabilistic Hough transform (PPHT) improves upon SHT by using a subset of random points sufficient for line detection to represent all points in the image.

[0073] Step 43: Add a line width d to the line detected in step 42, and use the weighted least squares method to iteratively fit a line to the detection domain with a line width of d;

[0074] During the PPHT detection process, a line is identified as an edge line as long as the number of votes in the accumulator reaches a certain threshold. Therefore, multiple lines may be detected for the same edge. To address this, a line width d is added to the line detected by PPHT. The rectangular region with a length equal to the line width d connecting the two endpoints of the line is used as the detection domain for that line. Overlapping detection domains are merged into a larger detection domain. Then, the candidate points in the detection domain are fitted with the line using the least squares (LS) method. This process merges multiple lines on the same edge and provides a more refined line description.

[0075] Unlike the statistical point count method of HT, LS fits a straight line by minimizing the perpendicular error of each point to the line. After one fitting result, it iterates to select points closer to the line for LS fitting, thus achieving a detection accuracy that HT cannot achieve. Furthermore, LS can merge lines with high overlap at the same edge during the fitting process, avoiding the situation where HT detects a large number of similar lines at the same edge. However, LS is highly susceptible to outliers. The farther an outlier is from the true line, the greater its weight in the fitting process. Weighted Least Squares (WLS) is an improvement on LS. The idea of ​​WLS is to use a weighted approach to these distance errors, assigning different weights to each sample point, with larger deviations receiving smaller weights and smaller deviations receiving larger weights.

[0076] Step 44: Merge multiple straight lines that have broken at the same edge;

[0077] Due to the addition of the linewidth d, WLS will fit similar straight lines detected by PPHT into a single straight line. However, when a notch appears at the edge of the workpiece, PPHT will detect the same edge as multiple straight lines. These lines with similar slopes need to be merged. The specific merging process is as follows: Figure 4 As shown, the steps are as follows:

[0078] Step 441: Extract the slopes of similar lines (lines L1, L2, and L3 on the same edge are similar lines) and calculate the average value v, which is the slope of the fitted line L4.

[0079] Step 442: Calculate the slope c of the line that is perpendicular to the fitted line L4;

[0080] Step 443: Extract the coordinates of the endpoints A1 and B1 of the line with the largest difference in slope from the fitted line, and find the two perpendicular lines L5 and L6 with slope c passing through A1 and B1 respectively.

[0081] Step 444: Find the coordinates of the intersection points of the two perpendicular lines L5 and L6 with other lines L1, L2 and L3 on the same edge. Calculate the average value of the coordinates of the intersection points of each perpendicular line with other similar lines to obtain two coordinate points A and B.

[0082] Step 445: The straight line connecting points A and B is the fitted line L7. Replace the original straight line with L7.

[0083] Step 45: Calculate the coordinates of the intersection of adjacent straight lines, which are the two-dimensional coordinates of the corner points of the box workpiece in the pixel coordinate system, and then select the two-dimensional coordinates of the feature points from them.

[0084] This invention can calculate the accurate position and orientation information of a box-shaped workpiece using a single two-dimensional image captured by a monocular camera, thereby ensuring real-time and rapid positioning of the box-shaped workpiece and facilitating the gripping and handling of the workpiece by a mobile robot.

[0085] In summary, the point feature-based box workpiece pose measurement method provided by this invention preprocesses the original image acquired by a monocular camera through image segmentation, distortion correction, etc. For the complex scene where the box workpiece is located, a method of cascading neural network and traditional point-pair feature matching algorithm is applied to complete the similarity matching and recognition positioning of different features.

[0086] Based on the above-described preferred embodiments of the present invention, and through the foregoing description, those skilled in the art can make various changes and modifications without departing from the inventive concept. The technical scope of this invention is not limited to the contents of the specification, but must be determined according to the scope of the claims.

Claims

1. A method for measuring the pose of a box-shaped workpiece based on point features, characterized in that, Includes the following steps: Step 1: Install the monocular camera and calibrate it using the Zhang Zhengyou calibration method to obtain the internal parameters and distortion parameters of the monocular camera; Step 2: Use a monocular camera to acquire images of the box-shaped workpiece. Select four corner points of the box-shaped workpiece that are coplanar in three-dimensional space in the image as feature points. Establish the world coordinate system, camera coordinate system, image coordinate system and pixel coordinate system, and obtain the three-dimensional coordinates of the feature points in the world coordinate system. Step 3: Preprocess each frame of the acquired raw image; Step 4: Perform image processing on the preprocessed image using point features to obtain the two-dimensional coordinates of the feature points in the pixel coordinate system; Step four specifically includes: Step 41: Perform edge detection on the preprocessed image to obtain the edge pixels of the image; Step 42: Use Hough transform to perform line detection on the edge pixels of the image; Step 43: Add a line width d to the detected line. Use the rectangular area with a length equal to the line width d connecting the two ends of the line as the detection domain of the line. Merge overlapping detection domains into a larger detection domain. Then, use weighted least squares to fit the line to the candidate points in the detection domain. The larger the perpendicular error of each candidate point to the line, the greater the weight it is assigned. After one fitting result, iterate continuously to select points closer to the line and use weighted least squares to fit the line. Step 44: Merge multiple straight lines that break at the same edge; Step 44 specifically includes: Step 441: Extract the slopes of lines with the same edge and calculate the average slope v; v is the slope of the fitted line; Step 442: Calculate the slope c of the line perpendicular to the fitted line along the edge; Step 443: Extract the coordinates of the endpoints A1 and B1 of the line with the largest slope difference from the fitted line, and find the perpendicular lines passing through A1 and B1 with a slope of c. Step 444: Find the coordinates of the intersection points of the two perpendicular lines with other lines on the same edge. Calculate the average value of the coordinates of the intersection points of each perpendicular line with other lines on the same edge to obtain two coordinate points A and B. Step 445: Replace the original straight line with the straight line connecting points A and B; Step 45: Calculate the coordinates of the intersection of adjacent straight lines, which are the two-dimensional coordinates of the corner points of the box workpiece in the pixel coordinate system. Then, select the two-dimensional coordinates of the feature points from the two-dimensional coordinates of the corner points of the box workpiece in the pixel coordinate system. The feature points are the corner points of the four box workpieces that are coplanar in three-dimensional space. Step 5: Combining the three-dimensional coordinates of the feature points in the world coordinate system, the two-dimensional coordinates of the feature points in the pixel coordinate system, and the internal parameters of the monocular camera, the pose information of the box-shaped workpiece is solved using the PNP measurement method.

2. The method for measuring the pose of a box-shaped workpiece based on point features according to claim 1, characterized in that, Step three specifically includes: Step 31: Correct the distortion of the original image using the distortion parameters of the monocular camera obtained in Step 1; Step 32: Detect the box-shaped workpiece in the image, select the box-shaped workpiece, and perform masking on the remaining part of the image.

3. The method for measuring the pose of a box-shaped workpiece based on point features according to claim 1, characterized in that: Edge detection uses a hybrid filter that combines Gaussian filtering and mean shift filtering.

4. The method for measuring the pose of a box-shaped workpiece based on point features according to claim 1, characterized in that: The three-dimensional coordinates of the feature point in the world coordinate system refer to the correspondence between the coordinates of the feature point of the box workpiece in the image coordinate system and the two-dimensional coordinates of the feature point in the three-dimensional world coordinate system.

5. The method for measuring the pose of a box-shaped workpiece based on point features according to claim 1, characterized in that, The PNP measurement method combines the three-dimensional coordinates of the feature points in the world coordinate system, the two-dimensional coordinates of the feature points in the pixel coordinate system, and the internal parameters of the monocular camera to calculate the rotation and translation relationship between the world coordinate system and the camera coordinate system of the box-shaped workpiece.

6. The method for measuring the pose of a box-shaped workpiece based on point features according to claim 5, characterized in that, PNP measurement methods specifically include: a point in space Coordinates and points in the world coordinate system The transformation relationship model between coordinates projected onto the image coordinate system is as follows: ； in, It is the homogeneous coordinate form of a point in space in the world coordinate system. It is the coordinate form of a point in space in the camera coordinate system. It is the coordinate of a point in space on the axis perpendicular to the imaging plane in the camera coordinate system; It is the homogeneous coordinate form of a point in space in the image coordinate system. It is the focal length of the camera lens. It refers to the width and height of each pixel on the camera's image sensor. These are the coordinates of the origin of the image sensor on the pixel sensor. These are the camera's rotation matrix parameters. These are the translation vector parameters of the camera; The following expression is derived from the camera imaging model: ； Substituting equation (3) into equations (1) and (2), we get: ； The CV_ITERATIVE algorithm provided by OpenCV is used to iteratively find the solution with the minimum reprojection error. Correct pose data is obtained if and only if the number of feature points is 4 and the 4 feature points are coplanar.

Images