Depth camera calibration method for non-overlapping view

Abstract

This invention relates to a depth camera calibration method for non-overlapping fields of view. A rod structure with spheres at both ends is used as the calibration object. The distance between the centers of the two spheres is calculated as the objective function. A set of pose equations is established to solve for the distance between the center of the spheres under different poses of the calibration object. For each pose of the calibration object, point cloud data and Voronoi diagrams of the point cloud data are acquired using depth cameras located on the left and right sides of the rod. Based on the Voronoi diagrams, the coordinates of the center of each sphere in the coordinate system of the corresponding side depth camera are determined. The pose equations are solved using the established set of pose equations and the coordinates of the two sphere centers for each pose of the calibration object, yielding the sets of rotation and translation parameters. This method accurately and stably achieves depth camera calibration for non-overlapping fields of view, solving the problem that existing depth camera calibration methods are not applicable to non-overlapping fields of view.

Description

Technical Field

[0001] This invention relates to the field of industrial inspection, and more particularly to a method for calibrating depth cameras with non-overlapping fields of view. Background Technology

[0002] In the field of intelligent manufacturing, robots, as core actors, play a crucial role. Each robot's actions rely on its precise perception of the surrounding physical environment, and depth cameras, as 3D imaging devices, are essential for capturing this information. Assuming a robot in intelligent manufacturing is equipped with one or more depth cameras, each with a fixed position and orientation, the limited field of view of a single depth camera means it can only capture a small area of ​​scene information (i.e., surrounding physical environment information). To enable the robot to obtain a wider range of scene information, multiple depth cameras are typically used simultaneously to capture the scene information of the robot's surrounding physical environment. Therefore, camera calibration becomes the primary issue in determining the transformation relationship between the coordinate systems of each depth camera. Existing depth camera calibration methods typically use a checkerboard method to establish the transformation relationship between two camera coordinate systems.

[0003] However, existing depth camera calibration methods have shortcomings: checkerboard methods are typically based on 2D images and rely on the existence of a common field of view or overlapping region among the cameras (i.e., overlapping fields of view). This means that a certain degree of overlap between the cameras is required to find corresponding feature points or calibration boards in the images acquired by each camera. For cases with no overlapping regions (or no overlapping fields of view), the checkerboard method struggles to accurately and stably calibrate depth cameras with no overlapping fields of view because there is no direct overlap between the cameras. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a depth camera calibration method for non-overlapping field of view, which is in contrast to the above-mentioned prior art.

[0005] The technical solution adopted by the present invention to solve the above-mentioned technical problems is: a depth camera calibration method for non-overlapping field of view, applicable to at least two depth cameras, characterized by comprising the following steps 1 to 4:

[0006] Step 1: Prepare a rod in advance, fix a first ball to the left end of the rod and a second ball to the right end of the rod, and use the rod structure with the first ball and the second ball fixed as a calibration object;

[0007] Step 2: Using the distance between the centers of the two spheres as the objective function, establish pose equations corresponding to solving the distance between the centers of the calibration object under different poses. All established pose equations form a set of pose equations corresponding to the calibration object. Each depth camera has its own coordinate system. Each pose equation includes a rotation part and a translation part of the center of the sphere when transforming between the two depth cameras in different coordinate systems under the corresponding pose. The rotation part includes the set of rotation parameters when the center of the first sphere is transformed to be in the same coordinate system as the center of the second sphere. The translation part includes the set of translation parameters when the center of the first sphere, after rotation and transformation to the coordinate system of the center of the second sphere, is translated to be in the same coordinate system as the center of the second sphere. The total number of pose equations in the pose equation set is greater than or equal to the sum of the total number of rotation parameters in the rotation parameter set and the total number of translation parameters in the translation parameter set.

[0008] Step 3: For the pose of the calibration object corresponding to each pose equation in the pose equation set, use the first depth camera located on the left side of the rod to collect the point cloud data of the calibration object and the Voronoi diagram of the point cloud data, and determine the center coordinates of the first sphere in the coordinate system of the first depth camera based on the Voronoi diagram.

[0009] Step 4: For the pose of the calibration object corresponding to each pose equation in the pose equation set, use the second depth camera located on the right side of the rod to collect the point cloud data of the calibration object and the Voronoi diagram of the point cloud data, and determine the center coordinates of the second sphere in the coordinate system of the second depth camera based on the Voronoi diagram; wherein, the center coordinates of the first sphere, the center coordinates of the second sphere and the pose of the calibration object are in one-to-one correspondence.

[0010] Step 5: Based on the established pose equation set and the coordinates of the center of the first and second spheres of the calibration object at each pose, solve the pose equation set to obtain each rotation parameter in the rotation parameter set and each translation parameter in the translation parameter set, thus completing the calibration of the depth camera in the non-overlapping field of view.

[0011] Improved, in the depth camera calibration method for non-overlapping fields of view, in step 2, the pose equation set consists of M pose equations, and the m-th pose equation is constructed as follows:

[0012] ||RA m +TB m ||2=L;1≤m≤M;

[0013] Among them, A m B represents the coordinates of the center of the first sphere in the first depth camera coordinate system when the calibration object is in the m-th pose. mThis represents the coordinates of the center of the second sphere in the second depth camera coordinate system when the calibration object is in the m-th pose. R represents rotation, T represents translation, and ||RA m +TB m ||2 represents the vector RA m +TB m The L2 norm of the first sphere; L represents the distance between the centers of the first and second spheres.

[0014] Furthermore, in the depth camera calibration method for non-overlapping fields of view, in step 5, the solution process for each rotation parameter in the rotation parameter set and each translation parameter in the translation parameter set of the pose equation system includes the following steps a1 to a4:

[0015] Step a1: Quaternions are used to represent the rotation process during the coordinate system transformation between the two depth cameras. The pose equations established using quaternions are then used to adjust and obtain the corresponding pose equations represented by quaternions. The pose equations established using quaternions are as follows:

[0016]

[0017] q = q1i + q2j + q3k + q4;

[0018]

[0019] Where q is a quaternion. and All are imaginary quaternions, where i, j, and k are the imaginary parts of quaternion q, and θ, j, k are the imaginary parts of q. β and θ represent the degrees of freedom when transforming between different coordinate systems in three-dimensional space; degrees of freedom θ, β ... β and β are the rotation parameters within the set of rotation parameters;

[0020] Step a2: Based on the pose equations represented by the obtained quaternions, construct deviation functions for solving the rod length deviation; wherein, the deviation function for solving the rod length deviation is denoted as f. m The deviation function f m It is expressed as follows:

[0021]

[0022] Wherein, the deviation function f m It corresponds one-to-one with the m-th pose equation;

[0023] Step a3: Establish an accumulation function for the length deviation of the rod corresponding to each pose equation of the rod as the objective function; whereby the objective function is expressed as follows:

[0024]

[0025] Step a4: Optimize the objective function using the least squares method to obtain the optimization degrees of freedom and optimization translation parameters that meet the optimization requirements.

[0026] Improved, in the depth camera calibration method for non-overlapping fields of view, step 3, the process of determining the coordinates of the center of the first sphere in the first depth camera coordinate system based on the Voronoi diagram, includes the following steps 31-34:

[0027] Step 31: Exclude Voronoi vertices located within the Voronoi graph where the difference between the radius of the Voronoi graph vertex and the prior radius of the first sphere exceeds a preset threshold, and use the excluded Voronoi graph as the first target Voronoi graph; wherein, the radius of the Voronoi graph vertex is the shortest distance from the Voronoi graph vertex to any point on the boundary of the region where the Voronoi graph vertex is located; the prior radius of the first sphere is the estimated radius of the first sphere;

[0028] Step 32: Use a KD-tree to query the total number of Voronoi graph vertices in the δ-neighborhood of each Voronoi graph vertex in the first target Voronoi graph;

[0029] Step 33: Select the Voronoi graph vertex with the maximum total number of Voronoi graph vertices in its δ neighborhood within the first target Voronoi graph, and use the selected Voronoi graph vertex as the center of the first sphere.

[0030] Step 34: The coordinates of the selected Voronoi diagram vertex are used as the center coordinates of the first sphere in the first depth camera coordinate system.

[0031] Furthermore, in the depth camera calibration method for non-overlapping fields of view, step 4, the process of determining the coordinates of the center of the second sphere in the second depth camera coordinate system based on the Voronoi diagram, includes the following steps 41 to 44:

[0032] Step 41: Exclude Voronoi vertices located within the Voronoi graph where the difference between the radius of the Voronoi graph vertex and the prior radius of the second sphere exceeds a preset threshold, and use the excluded Voronoi graph as the second target Voronoi graph; wherein, the radius of the Voronoi graph vertex is the shortest distance from the Voronoi graph vertex to any point on the boundary of the region where the Voronoi graph vertex is located; the prior radius of the second sphere is the estimated radius of the second sphere;

[0033] Step 42: Use a KD-tree to query the total number of Voronoi graph vertices in the δ-neighborhood of each Voronoi graph vertex in the second target Voronoi graph.

[0034] Step 43: Select the Voronoi graph vertex with the maximum total number of Voronoi graph vertices in its δ neighborhood within the second target Voronoi graph, and use the selected Voronoi graph vertex as the center of the second sphere.

[0035] Step 44: The coordinates of the selected Voronoi diagram vertex are used as the center coordinates of the second sphere in the second depth camera coordinate system.

[0036] Compared with the prior art, the advantages of the present invention are as follows: The depth camera calibration method for non-overlapping fields of view of the present invention uses a rod structure with spheres at both ends as the calibration object, and uses the distance between the centers of the two spheres as the objective function to establish a set of pose equations for solving the distance between the centers of the calibration object in different poses. Then, for each pose of the calibration object, two depth cameras located on the left and right sides of the rod are used to collect the point cloud data of the calibration object and the Voronoi diagram of the point cloud data respectively. Based on the Voronoi diagram, the coordinates of the center of each sphere in the coordinate system of the corresponding depth camera are determined. Then, according to the established set of pose equations and the coordinates of the centers of the two spheres of the calibration object in each pose, the set of pose equations is solved to obtain each rotation parameter in the set of rotation parameters and each translation parameter in the set of translation parameters in the set of pose equations. This completes the calibration of the depth camera for non-overlapping fields of view, thereby accurately and stably realizing the calibration of the depth camera for non-overlapping fields of view, and solving the problem that the existing depth camera calibration methods cannot be applied to the case of non-overlapping fields of view of depth cameras. Attached Figure Description

[0037] Figure 1 This is a schematic diagram of the depth camera calibration method for non-overlapping fields of view in an embodiment of the present invention;

[0038] Figure 2 This is a schematic diagram showing the setup status of the calibration object and depth camera in an embodiment of the present invention.

[0039] Figure 3 This is a Voronoi diagram of the point cloud data captured in this embodiment of the invention;

[0040] Figure 4 This is a schematic diagram showing the positional relationship between the first sphere and the second sphere before and after calibration, after being captured by a depth camera in an embodiment of the present invention. Detailed Implementation

[0041] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0042] This embodiment provides a depth camera calibration method for two depth cameras with a non-overlapping field of view. Specifically, see [link to documentation]. Figure 1 As shown, the depth camera calibration method for non-overlapping field of view in this embodiment includes the following steps 1 to 4:

[0043] Step 1: Prepare a rod in advance, and fix a first ball to the left end of the rod and a second ball to the right end of the rod. Use the rod structure with the first and second balls fixed as a calibration object; wherein, the calibration object structure in this embodiment is shown in [reference needed]. Figure 2 As shown, the first sphere is labeled Q1, the center of the first sphere Q1 is labeled a, the second sphere is labeled Q2, and the second sphere Q2 is labeled b; the first sphere and the second sphere preferably have the same sphere radius;

[0044] Step 2: Using the calculation of the distance between the centers of the two spheres as the objective function, establish the pose equations corresponding to the calculation of the distance between the centers of the calibration object under different poses, and form a set of pose equations corresponding to the calibration object from all the established pose equations.

[0045] Each depth camera has its own coordinate system. Each pose equation includes a rotation component and a translation component of the sphere center when transforming between the two depth cameras in different coordinate systems under the corresponding pose. The rotation component includes the set of rotation parameters when the center of the first sphere is transformed to be in the same coordinate system as the center of the second sphere. The translation component includes the set of translation parameters when the center of the first sphere, after being rotated and transformed to be in the same coordinate system as the center of the second sphere, is translated to be in the same coordinate system as the center of the second sphere. The total number of pose equations in the pose equation set is greater than or equal to the sum of the total number of rotation parameters in the rotation parameter set and the total number of translation parameters in the translation parameter set.

[0046] Specifically, in this embodiment, the two depth cameras are a first depth camera C1 and a second depth camera C2; the pose equation set corresponding to the calibration object consists of M pose equations, and the m-th pose equation is constructed as follows:

[0047] ||RA m +TB m ||2=L;1≤m≤M;M≥7;

[0048] Among them, A m B represents the coordinates of the center a of the first sphere Q1 in the coordinate system of the first depth camera C1 when the calibration object is in the m-th pose. mLet b represent the coordinates of the center of the second sphere Q2 in the coordinate system of the second depth camera C2 when the calibration object is in the m-th pose. R represents rotation, R∈SO(3), and T represents translation, T=[t1,t2,t3]. T t1, t2, and t3 represent the translation amounts in the corresponding directions in three-dimensional space; ||RA m +TB m ||2 represents the vector RA m +TB m L is the L2 norm; L represents the center-to-center distance between the center a of the first sphere Q1 and the center b of the second sphere Q2.

[0049] Step 3: For the pose of the calibration object corresponding to each pose equation in the pose equation set, use the first depth camera located on the left side of the rod to acquire point cloud data of the calibration object and the Voronoi diagram of the point cloud data, and determine the center coordinates of the first sphere in the coordinate system of the first depth camera based on the Voronoi diagram; wherein, the process of determining the center coordinates of the first sphere in the coordinate system of the first depth camera based on the Voronoi diagram includes the following steps 31 to 34:

[0050] Step 31: Exclude Voronoi vertices located within the Voronoi graph where the difference between the radius of the Voronoi graph vertex and the prior radius of the first sphere exceeds a preset threshold, and use the excluded Voronoi graph as the first target Voronoi graph; wherein, the radius of the Voronoi graph vertex is the shortest distance from the Voronoi graph vertex to any point on the boundary of the region where the Voronoi graph vertex is located; the prior radius of the first sphere is the estimated radius of the first sphere;

[0051] Step 32: Use a KD tree to query the total number of Voronoi graph vertices in the δ-neighborhood of each Voronoi graph vertex in the first target Voronoi graph;

[0052] Step 33: Select the Voronoi graph vertex with the maximum total number of Voronoi graph vertices in its δ neighborhood within the first target Voronoi graph, and use the selected Voronoi graph vertex as the center of the first sphere.

[0053] Step 34: Use the coordinates of the selected Voronoi diagram vertex as the coordinates of the center of the first sphere in the coordinate system of the first depth camera.

[0054] Step 4: For the pose of the calibration object corresponding to each pose equation in the pose equation set, use the second depth camera located on the right side of the rod to collect the point cloud data of the calibration object and the Voronoi diagram of the point cloud data, and determine the center coordinates of the second sphere in the coordinate system of the second depth camera based on the Voronoi diagram; wherein, the center coordinates of the first sphere, the center coordinates of the second sphere and the pose of the calibration object are in one-to-one correspondence.

[0055] The process of determining the coordinates of the center of the second sphere in the second depth camera coordinate system based on the Voronoi diagram includes the following steps 41 to 44:

[0056] Step 41: Exclude Voronoi vertices located within the Voronoi graph where the difference between the radius of the Voronoi graph vertex and the prior radius of the second sphere exceeds a preset threshold, and use the excluded Voronoi graph as the second target Voronoi graph; wherein, the radius of the Voronoi graph vertex is the shortest distance from the Voronoi graph vertex to any point on the boundary of the region where the Voronoi graph vertex is located; the prior radius of the second sphere is the estimated radius of the second sphere;

[0057] Step 42: Use a KD-tree to query the total number of Voronoi graph vertices in the δ-neighborhood of each Voronoi graph vertex in the second target Voronoi graph.

[0058] Step 43: Select the Voronoi graph vertex with the maximum total number of Voronoi graph vertices in its δ neighborhood within the second target Voronoi graph, and use the selected Voronoi graph vertex as the center of the second sphere.

[0059] Step 44: The coordinates of the selected Voronoi diagram vertex are used as the coordinates of the center of the second sphere in the coordinate system of the second depth camera.

[0060] Step 5: Based on the established pose equations and the coordinates of the center of the first and second spheres of the calibration object for each pose, solve the pose equations to obtain the rotation parameters in the rotation parameter set and the translation parameters in the translation parameter set. Using these rotation and translation parameters, the coordinate system of one camera can be transformed to the coordinate system of another camera, completing the calibration of the depth camera with a non-overlapping field of view.

[0061] In step 5, the solution process for each rotation parameter in the rotation parameter set and each translation parameter in the translation parameter set of the pose equation system includes the following steps a1 to a4:

[0062] Step a1: Quaternions are used to represent the rotation process during the coordinate system transformation between the two depth cameras. The pose equations established using quaternions are then used to adjust and obtain the corresponding pose equations represented by quaternions. The pose equations established using quaternions are as follows:

[0063]

[0064] q = q1i + q2j + q3k + q4;

[0065]

[0066] Where q is a quaternion. and All are imaginary quaternions, where i, j, and k are the imaginary parts of quaternion q, and θ, j, k are the imaginary parts of q. β and θ represent the degrees of freedom when transforming between different coordinate systems in three-dimensional space; degrees of freedom θ, β ... β and β are the rotation parameters within the set of rotation parameters;

[0067] Step a2: Based on the pose equations represented by the obtained quaternions, construct deviation functions for solving the rod length deviation; wherein, the deviation function for solving the rod length deviation is denoted as f. m The deviation function f m It is expressed as follows:

[0068]

[0069] Wherein, the deviation function f m It corresponds one-to-one with the m-th pose equation;

[0070] Step a3: Establish an accumulation function for the length deviation of the rod corresponding to each pose equation of the rod as the objective function; whereby the objective function is expressed as follows:

[0071]

[0072] Step a4 involves optimizing the objective function using the least squares method to obtain the optimized degrees of freedom and the optimized translation parameters that meet the optimization requirements. The use of the least squares method to optimize the function is a standard application of the least squares method, and the specific optimization process will not be elaborated here.

[0073] To verify the effectiveness of the depth camera calibration method for non-overlapping field of view in this embodiment, actual experiments were also conducted. Specifically:

[0074] The depth camera used was the PhoXi 3D ScannerS from Photoneo, a high-resolution and high-precision scanning device suitable for scanning static scenes. In the actual experiment of this embodiment, the PhoXi 3D ScannerS was selected as the depth camera. Utilizing its efficient 3D scanning capabilities, the depth camera was calibrated and positioned by capturing point cloud data of multiple sets of calibration spheres. The first and second spheres on the calibration object were both ping-pong balls with a radius of 20mm. The camera resolution was set to 2064×1544, and the exposure time was set to 14.336 milliseconds. During the experiment, the positions of the two depth cameras remained constant. The corresponding calibration object data was obtained by changing the position between the first and second spheres, ensuring that there was a certain angle between the data sets to correspond to different poses of the calibration object.

[0075] In the depth camera calibration process for a non-overlapping field of view, the first step is to use a Voronoi diagram to capture the center of the first sphere and the center of the second sphere. Among these steps... Figure 3 The Voronoi diagram vertices of the captured point cloud data are shown. It can be observed that the region with the highest vertex density in the Voronoi diagram is the center of the sphere. In the experiment, the Voronoi diagram filtering threshold was set to 0.5 mm, and the neighborhood parameter δ of the Voronoi diagram vertices was 0.1 mm. The final calculated radius of the ping-pong ball was 19.82 mm.

[0076] A total of 14 sets of data were collected for calibration. Since the actual distance between the two cameras was 1m and the pole length was 740mm, the initial point of the quaternion was set to q = (1,0,0,0). T Translation parameter T = (-1000, 0, 0) T L = 740. The positional relationship between the first and second spheres before and after calibration is shown in [reference needed]. Figure 4 As shown, after calibration, the calibration objects (including the rod and the first and second spheres located at the left and right ends of the rod) remain in a straight line, achieving the effect of aligning the calibration rod.

[0077] In the actual test, even with challenges such as significant noise in the captured point cloud data and the inherently low precision of the ping-pong ball (as a sphere), the depth camera calibration method for non-overlapping fields of view in this embodiment can still calibrate two depth cameras relatively accurately. This indicates that the depth camera calibration method for non-overlapping fields of view in this embodiment possesses a certain degree of robustness. This robustness makes the depth camera calibration method for non-overlapping fields of view more suitable for applications in real-world environments and provides reliable technical support for achieving accurate 3D visual positioning measurements.

[0078] Although preferred embodiments of the present invention have been described in detail above, it should be clearly understood that various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A depth camera calibration method for non-overlapping field of view, applicable to at least two depth cameras, characterized in that, Includes the following steps 1-5: Step 1: Prepare a rod in advance, fix a first ball to the left end of the rod and a second ball to the right end of the rod, and use the rod structure with the first ball and the second ball fixed as a calibration object; Step 2: Using the distance between the centers of the two spheres as the objective function, establish the pose equations corresponding to solving the distance between the centers of the calibration object under different poses. All established pose equations form a set of pose equations corresponding to the calibration object. The pose equation set consists of... M The pose equations consist of the first, second, and third pose equations. m The pose equations are constructed as follows: ;1≤m≤M; in, A m This indicates that the center of the first sphere is located at the first reference point. m The pose and the coordinates of the center of the sphere in the first depth camera coordinate system. B m This indicates that the center of the second sphere is located at the first reference point. m The pose and the coordinates of the center of the sphere in the second depth camera coordinate system. R Indicates rotation, T Indicates translation. Representing vectors RA m + T - B m The second norm; L This represents the distance between the centers of the first sphere and the second sphere. Each depth camera has its own coordinate system. Each pose equation includes a rotation component and a translation component of the sphere center when transforming between the two depth cameras in different coordinate systems under the corresponding pose. The rotation component includes the set of rotation parameters when the center of the first sphere is transformed to be in the same coordinate system as the center of the second sphere. The translation component includes the set of translation parameters when the center of the first sphere, after being rotated and transformed to be in the same coordinate system as the center of the second sphere, is translated to be in the same coordinate system as the center of the second sphere. The total number of pose equations in the pose equation set is greater than or equal to the sum of the total number of rotation parameters in the rotation parameter set and the total number of translation parameters in the translation parameter set. Step 3: For the pose of the calibration object corresponding to each pose equation in the pose equation set, use the first depth camera located on the left side of the rod to collect the point cloud data of the calibration object and calculate the Voronoi diagram of the point cloud data, and determine the center coordinates of the first sphere in the coordinate system of the first depth camera based on the Voronoi diagram. Step 4: For the pose of the calibration object corresponding to each pose equation in the pose equation set, use the second depth camera located on the right side of the rod to collect point cloud data of the calibration object and calculate the Voronoi diagram of the point cloud data. Based on the Voronoi diagram, determine the center coordinates of the second sphere in the coordinate system of the second depth camera. The center coordinates of the first sphere, the center coordinates of the second sphere, and the pose of the calibration object are in one-to-one correspondence. Step 5: Based on the established pose equations and the coordinates of the first and second sphere centers of the calibration object at each pose, solve the pose equations using quaternions and least squares methods to obtain the rotation parameters in the rotation parameter set and the translation parameters in the translation parameter set, thus completing the calibration of the depth camera for the non-overlapping field of view.

2. The depth camera calibration method for non-overlapping field of view according to claim 1, characterized in that, In step 5, the solution process for each rotation parameter in the rotation parameter set and each translation parameter in the translation parameter set of the pose equation system includes the following steps a1~a4: Step a1: Quaternions are used to represent the rotation process during the coordinate system transformation between the two depth cameras. The pose equations established using quaternions are then used to adjust and obtain the corresponding pose equations represented by quaternions. The pose equations established using quaternions are as follows: ; ； ； ； in, q It is a quaternion. , and All are imaginary quaternions. i , j and k Quaternions q The imaginary part, θ , φ and β These represent the degrees of freedom when transforming between different coordinate systems in three-dimensional space; degrees of freedom θ , φ and β That is, the rotation parameters within the set of rotation parameters; Step a2: Based on the pose equations represented by the obtained quaternions, construct deviation functions for solving the rod length deviation; wherein, the deviation function for solving the rod length deviation is denoted as... f m The deviation function f m It is expressed as follows: Among them, the deviation function f m With the m Each pose equation corresponds one-to-one; Step a3: Establish an accumulation function for the length deviation of the rod corresponding to each pose equation of the rod as the objective function; whereby the objective function is expressed as follows: ； Step a4: Optimize the objective function using the least squares method to obtain the optimization degrees of freedom and optimization translation parameters that meet the optimization requirements.

Images