Multi-Camera-Based Universal Measurement Method for 6-DOF of Rigid Bodies in World Coordinate System

The measurement of six-degrees-of-freedom (6-DOF) of rigid bodies plays an important role in many industries, but it often requires the use of professional instruments and software, or has limitations on the shape of measured objects. In this paper, a 6-DOF measurement method based on multi-camera is proposed, which is accomplished using at least two ordinary cameras and is made available for most morphological rigid bodies. First, multi-camera calibration based on Zhang Zhengyou’s calibration method is introduced. In addition to the intrinsic and extrinsic parameters of cameras, the pose relationship between the camera coordinate system and the world coordinate system can also be obtained. Secondly, the 6-DOF calculation model of proposed method is gradually analyzed by the matrix analysis method. With the help of control points arranged on the rigid body, the 6-DOF of the rigid body can be calculated by the least square method. Finally, the Phantom 3D high-speed photogrammetry system (P3HPS) with an accuracy of 0.1 mm/m was used to evaluate this method. The experiment results show that the average error of the rotational degrees of freedom (DOF) measurement is less than 1.1 deg, and the average error of the movement DOF measurement is less than 0.007 m. In conclusion, the accuracy of the proposed method meets the requirements.


Introduction
The measurement of six-degrees-of-freedom (6-DOF) is important in industrial production and the 6-DOF of a measured object represents its position information, which will help the machine to operate efficiently, thus 6-DOF measurements are often used in fields such as precision machining, spacecraft docking, and manufacturing assembly [1].
The 6-DOF of a rigid body include the rotational degrees of freedom (Ψ, θ, ϕ) around the x, y, and z axes as well as the movement degrees of freedom (T x , T y , T z ) along the x, y, and z axes. The commonly used measuring instruments or methods include laser, Hall sensor, inertial measurement unit (IMU), total station, and vision. Laser measurement methods, which include laser interferometer [2], laser tracker [3], and laser collimation method [4], have high accuracy, but a special optical path needs to be designed using a lens [5][6][7]. As a result, it has requirements on the size or range of motion of the measured object. In addition, the refractive index of the laser is susceptible

6-DOF Basic Formula for Rigid Body
In space, an unrestricted rigid body has 6-DOF, which are rotation DOF around the x, y, and z axes as well as movement DOF along the three axes. The three DOF of movement are usually represented by the translation vector T, and the three DOF of rotation (three Euler angles) are represented by the rotation matrix R. Here, the rotation matrix R is a 3 × 3 order unit orthogonal matrix. In this paper, measuring the 6-DOF of a rigid body is performed to solve the rotation and movement DOF between the world coordinate system W (W system) and the rigid body coordinate system B (B system). According to the above, the rotation matrix R W B and the translation vector T W B/W between the two coordinate systems can be solved first, and then R W B can be converted into three Euler angles. The form of R W B is as follows: r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 The nine elements in the above matrix have the following relationship: r 11 r 12 + r 21 r 22 + r 31 r 32 = 0 r 11 r 13 + r 21 r 23 + r 31 r 33 = 0 r 12 r 13 + r 22 r 23 + r 32 r 33 = 0 r 2 11 + r 2 21 + r 2 31 = 1 r 2 12 + r 2 22 + r 2 32 = 1 r 2 13 + r 2 23 + r 2 According to the z-y-x rotation order, it can be named in turn as the rotation angle Ψ around the z-axis, the rotation angle θ around the y-axis, and the rotation angle ϕ around the x-axis. The conversion relationship between the elements of R W B and the three rotation angles (Ψ, θ, ϕ) is shown as follows: Sensors 2020, 20, 5547 4 of 21

Pinhole Camera Model
The imaging process of the camera can be regarded as the pinhole camera model. The three-dimensional (3D) scene is projected to the two-dimensional (2D) image plane. There is a certain mapping relationship during imaging, which can be simplified to perspective projection ( Figure 1). The method proposed in this article and the explanation of subsequent principles are all based on the ideal pinhole camera model. Accordingly, the relationship between the pixel coordinates of the spatial point P in the image plane and its coordinates in the W system can be expressed as Equation (4) [44].
where (u , v , 1) T is the homogeneous pixel coordinates of the imaging point p; x w , y w , z w , 1 T is the homogeneous world coordinates of the space point P; z C is the coordinate of the space point P in the z-axis direction of the camera coordinate system C (C system); f x = f d x and f y = f /d y , where d x and d y are the size of unit pixel on the x-axis and y-axis and f is the focal length; and (u 0 , v 0 ) is the pixel coordinate of the optical center, also known as the coordinate of the principal point. In the above, f x , f y , u 0 , and v 0 refer to the intrinsic parameters of the camera, while R C W and T C W/C refer to the extrinsic parameters of the camera, which are the rotation matrix and translation vector from the W system to the C system. It shows a need to be explicit about exactly what is meant by the elements of T C W/C , which are the components of the vector in the W system.
Sensors 2020, 20, x FOR PEER REVIEW 4 of 20 mapping relationship during imaging, which can be simplified to perspective projection ( Figure 1). The method proposed in this article and the explanation of subsequent principles are all based on the ideal pinhole camera model. Accordingly, the relationship between the pixel coordinates of the spatial point P in the image plane and its coordinates in the W system can be expressed as Equation (4) [44].
where ( , , 1) is the homogeneous pixel coordinates of the imaging point p; ( , , , 1) is the homogeneous world coordinates of the space point P; z is the coordinate of the space point P in the z-axis direction of the camera coordinate system C (C system); = and = / , where and are the size of unit pixel on the x-axis and y-axis and is the focal length; and ( , ) is the pixel coordinate of the optical center, also known as the coordinate of the principal point. In the above, , , , and refer to the intrinsic parameters of the camera, while and / refer to the extrinsic parameters of the camera, which are the rotation matrix and translation vector from the W system to the C system. It shows a need to be explicit about exactly what is meant by the elements of / , which are the components of the vector in the W system.

Multi-Camera Calibration
Camera calibration is an indispensable step in vision measurement, which determines the accuracy of extracting 3D information of spatial points from 2D images. This method uses at least two cameras to measure 6-DOF, so it is necessary to estimate the intrinsic and extrinsic parameters of cameras. In addition, in order to simplify the subsequent calculation of the 6-DOF, this paper also calibrates the pose relationship between the W system and one of the C systems. On the basis of Zhang Zhengyou's calibration method, this calibration called multi-camera calibration only uses a checkerboard pasted on a plate [45,46].
If the cameras are numbered according to any rules, the reference camera is described as the camera with number 1, and the non-reference cameras are described as the cameras with other numbers. Multi-camera calibration can best be treated under the following two parts: Part 1 is the calibration of the pose relationship between the non-reference cameras and the reference camera. This part of the calibration can obtain the intrinsic parameters of each camera, and the pose relationship between the reference camera coordinate system ( system) and the non-reference camera coordinate system. Part 2 is the calibration of the pose relationship between the system and the W system. This part of the calibration can obtain the pose relationship ( , ) between the

Multi-Camera Calibration
Camera calibration is an indispensable step in vision measurement, which determines the accuracy of extracting 3D information of spatial points from 2D images. This method uses at least two cameras to measure 6-DOF, so it is necessary to estimate the intrinsic and extrinsic parameters of cameras. In addition, in order to simplify the subsequent calculation of the 6-DOF, this paper also calibrates the pose relationship between the W system and one of the C systems. On the basis of Zhang Zhengyou's calibration method, this calibration called multi-camera calibration only uses a checkerboard pasted on a plate [45,46].
If the cameras are numbered according to any rules, the reference camera is described as the camera with number 1, and the non-reference cameras are described as the cameras with other numbers. Multi-camera calibration can best be treated under the following two parts: Part 1 is the calibration of Sensors 2020, 20, 5547 5 of 21 the pose relationship between the non-reference cameras and the reference camera. This part of the calibration can obtain the intrinsic parameters of each camera, and the pose relationship between the reference camera coordinate system C 1 (C 1 system) and the non-reference camera coordinate system. Part 2 is the calibration of the pose relationship between the C 1 system and the W system. This part of the calibration can obtain the pose relationship (R W C 1 , T W C 1 /W ) between the C 1 system and the W system. In Part 1 of the calibration principle, in order to estimate the intrinsic parameters of each camera by Zhang Zhengyou's calibration method, synchronous shooting by n (n ≥ 2) cameras obtains m (m ≥ 3) checkerboard images with different directions. Meanwhile, the pose relationship between the coordinate system B j (j = 1, 2 . . . , m) (B j system) established on the checkerboard and each camera coordinate system. Figure 2 shows a schematic diagram of using a checkerboard to calibrate. While P is an arbitrary corner point on the checkerboard, its coordinate in the B j system is described as P B j . In the C 1 system and C k (k = 2, 3..., n) system, the coordinates of P are described as P C 1 and P C k . The conversion relationship between P B j , P C 1 , P C k can be expressed as follows: where R C 1

B j
and R C k B j represent the rotation matrix from the B j system to the C 1 and C k systems; and T C 1 B j /C 1 and T C k B j /C k represent the translation vector from the B j system to the C 1 and C k systems, and its elements are the components of the vector in the C 1 and C k systems.
Eliminating in Equation (5), the following indicates the conversion relationship between and : , the above equation can be abbreviated as follows: where / is the rotation matrix and translation vector from the system to the system. In Part 2 of the calibration principle, there are m different systems in the m checkerboard images with different orientations, among which there are (3 ≤ ≤ ) checkerboard coordinate systems , which have the following relationship with the W system: (1) the coordinate axis direction of the system is consistent with the W system in its corresponding position; (2) the coordinates of the origin of the W system in the system are known. The system that conforms to the above-mentioned relationship is called the special checkerboard coordinate system ( system), and the relationship is shown in Figure 3a. As mentioned above, it can be seen that the pose relationship between the system and the W system is as follows: where is the rotation matrix from the system to the W system; I is the 3 × 3 identity matrix; / is the translation vector from the W system to the S system, and its elements are the components of the vector in the system; and ( , , ) is the origin of the W system, which is represented by the coordinate of the system. Eliminating P B j in Equation (5), the following indicates the conversion relationship between P C 1 and P C k : Assuming can be abbreviated as follows: where R C 1 C k is the rotation matrix and translation vector from the C k system to the C 1 system. In Part 2 of the calibration principle, there are m different B j systems in the m checkerboard images with different orientations, among which there are l (3 ≤ l ≤ m) checkerboard coordinate systems B j , which have the following relationship with the W system: (1) the coordinate axis direction of the B j system is consistent with the W system in its corresponding position; (2) the coordinates of the origin of the W system in the B j system are known. The B j system that conforms to the above-mentioned relationship is called the special checkerboard coordinate system S l (S l system), and the relationship Sensors 2020, 20, 5547 6 of 21 is shown in Figure 3a. As mentioned above, it can be seen that the pose relationship between the S l system and the W system is as follows: where R W S l is the rotation matrix from the S l system to the W system; I is the 3 × 3 identity matrix; T S l W/S l is the translation vector from the W system to the S l system, and its elements are the components of the vector in the S l system; and is the origin of the W system, which is represented by the coordinate of the S l system.   Figure 3b shows the geometric relationship of the translation vectors among some coordinate systems, and the geometric relationship can be expressed as follows: In the calibration process of Part 1, and / corresponding to the system and the system are known. In order to obtain the pose relationship between the system and the W system, Equation (10) can be obtained by combining Equations (8) and (9). So far, the multi-camera calibration is completed. The intrinsic parameters of each camera, and / between the system and the W system are obtained. According to the above principle, is calculated, but it is unknown whether is an orthogonal matrix. Therefore, the following works must to be performed, which are based on singular value decomposition (SVD) [47]: (1) assuming is the calculated above, calculating by SVD, we can obtain = ; (2) is a diagonal matrix and its elements are singular values of , the singular values of a 3 × 3 orthogonal matrix are all 1; (3) changing by an identity matrix I, we can obtain = , which is orthogonal.

6-DOF Measurement of Rigid Body in World Coordinate System
It can be seen from Section 2.1 that measuring the 6-DOF of a rigid body is performed to calculate the rotation matrix and the translation vector / between the W system and the B system. Suppose there are i control points on the measured rigid body, among which there are at least four non-coplanar control points. To ensure the accuracy of the measurement, the control points should be evenly distributed on the rigid body, covering the entire main structure of the rigid body. On the basis of the above-mentioned, and / can be obtained from Equation (11).  Figure 3b shows the geometric relationship of the translation vectors among some coordinate systems, and the geometric relationship can be expressed as follows: In the calibration process of Part 1, R C 1 S l and T C 1 S l /C 1 corresponding to the S l system and the C 1 system are known. In order to obtain the pose relationship between the C 1 system and the W system, Equation (10) can be obtained by combining Equations (8) and (9). So far, the multi-camera calibration is completed. The intrinsic parameters of each camera, R W C 1 and T W C 1 /W between the C 1 system and the W system are obtained. According to the above principle, R W C 1 is calculated, but it is unknown whether is an orthogonal matrix. Therefore, the following works must to be performed, which are based on singular value decomposition (SVD) [47]: (1) assumingR W

6-DOF Measurement of Rigid Body in World Coordinate System
It can be seen from Section 2.1 that measuring the 6-DOF of a rigid body is performed to calculate the rotation matrix R W B and the translation vector T W B/W between the W system and the B system. Suppose there are i control points P i on the measured rigid body, among which there are at least four non-coplanar control points. To ensure the accuracy of the measurement, the control points should be evenly distributed on the rigid body, covering the entire main structure of the rigid body. On the basis of the above-mentioned, R W B and T W B/W can be obtained from Equation (11).
The above equation is converted into homogeneous coordinate form as follows: In Equation (12), If P B i and P W i are known, R W B and T W B/W can be calculated. As P i is on the measured rigid body, its coordinates P B i in the B system can be obtained by manual measurement. The coordinates P W i , which is the coordinates of P i in the W system, may difficult to measure by manual measurement because the W system is set independently. Fortunately, P W i . can be measured according to the following principle.
According to the assumptions in Section 2.3, the reference camera and the non-reference cameras are defined. This section is based on the above assumption. In the measurement of P W i , the pixel coordinates of the imaging point p ik of P i on the camera k are applied to the calculation of the P C 1 i represented by the coordinate of C 1 system of P i . Following this, R W C 1 and T W C 1 /W obtained in Section 2.3 are used to calculate P W i of P i in the W system. The schematic diagram of the coordinate solution of P i in the C 1 system is shown in Figure 4. p ik is the imaging point of the control point P i on the image plane of camera k, and (u ik , v ik ) is the pixel coordinate of p ik . In camera k, the pixel coordinate of the intersection (principal point) O k of the optical axis and the image plane is defined as u k 0 , v k 0 , which is the principal point coordinates of camera k. According to Figure 4, there is the relationship below of the coordinate conversion of P i from the C 1 system to the C k system. The coordinates , which is the coordinates of in the W system, may difficult to measure by manual measurement because the W system is set independently. Fortunately, can be measured according to the following principle.
According to the assumptions in Section 2.3, the reference camera and the non-reference cameras are defined. This section is based on the above assumption. In the measurement of , the pixel coordinates of the imaging point of on the camera k are applied to the calculation of the represented by the coordinate of system of . Following this, and / obtained in Section 2.3 are used to calculate of in the W system. The schematic diagram of the coordinate solution of in the system is shown in Figure 4. is the imaging point of the control point on the image plane of camera k, and ( , ) is the pixel coordinate of . In camera k, the pixel coordinate of the intersection (principal point) of the optical axis and the image plane is defined as ( , ), which is the principal point coordinates of camera k. According to Figure 4, there is the relationship below of the coordinate conversion of from the system to the system.
Sensors 2020, 20, 5547 As shown in Figure 4, theoretically, in camera k, points P i , p ik , and O C k are on the same straight line, and (13) can be transformed into Equation (14). In addition, according to the geometric relationship between the imaging point p ik and the principal point, p ik of the i-th control point P i has the 3D coordinates p y are the sizes of unit pixel of camera k in the X and Y axis, and f k is the focal length of camera k.
where P C 1 i and P C k i are the 3D coordinates of P i in the C 1 system and the C k system; R C 1 C k and T C 1 , calculated by Equation (7) in Section 2.3, are the rotation matrix and translation vector from the C k system to the C 1 system; S k is the scale factor; and p C k ik is the 3D coordinate of the imaging point p ik of the i-th control point P i in the C k system.
In Equation (14), P C 1 i and S k are unknown quantities. For a single camera, while three equations can be listed, there are four unknowns; thereby, the unknown quantities cannot be measured. For k(k ≥ 2) cameras, there are (k + 3) unknowns quantities, and the number of equations is 3k. In this case, P C 1 i and S k can be calculated by the least square method. Equation (14) is now converted into matrix form because of the convenient calculation: For k cameras, 3k linear equations can be listed and expressed as equations: In Equation (16), The least square method can be used to solve for x in the following equation: Here, the 3D coordinate P C 1 i of the control point P i in the C 1 system is calculated, and then the pose relationship R W C 1 , T W C 1 /W from the C 1 system to the W system were calculated in Equation (10), and they are used in Equation (19) to convert the 3D coordinate P Isummary, the 3D coordinate P W i of the control point P i in the W system was calculated by the above principle, and the 3D coordinate P B i of the control point P i in the B system was obtained by manual measurement. P B i are denoted by (11) then illustrates the coordinate relationship of P i in the above two coordinate systems. If x = r 11 r 12 r 13 · · · r 33 T x T y T z T , where r is the components of R W B and T x T y T z Sensors 2020, 20, 5547 9 of 21 is the components of T W B/W , then for each P i (the number of non-coplanar control points is at least four), all have the following: For i control points, the least square method can be used to calculate x: Here, Here, the pose relationship parameters R W B . and T W B/W between the B system and the W system were calculated. To ensure the orthogonality of the rotation matrix R W B , the obtainedR W B = UDV T needs to be calculated by singular value decomposition (SVD), and D is replaced by unit matrix I. Now, R W B = UIV T , so as to ensure the orthogonality of the rotation matrix. After that, R W B is converted into three rotational DOF according to Section 2.1. So far, the degree of freedom of movement and rotation of the rigid body have been calculated.

6-DOF Measurement Method
According to the principle described in Chapter 2, a universal method for measuring 6-DOF of rigid bodies in the world coordinate system based on multiple cameras can be summarized, including three stages: preparation, camera calibration, and 6-DOF measurement. The proposed method including seven steps is shown in Figure 5.
Sensors 2020, 20, x FOR PEER REVIEW 9 of 20 converted into three rotational DOF according to Section 2.1. So far, the degree of freedom of movement and rotation of the rigid body have been calculated.

6-DOF Measurement Method
According to the principle described in Chapter 2, a universal method for measuring 6-DOF of rigid bodies in the world coordinate system based on multiple cameras can be summarized, including three stages: preparation, camera calibration, and 6-DOF measurement. The proposed method including seven steps is shown in Figure 5.

Preparation Stage
Step 1. Establish the W system and B system in place as required. The origin of the B system is usually set at the center of mass of the measured rigid body. The B system is fixed to the rigid body and moves with the movement of the rigid body. In addition, the aforementioned W system and B system are both right-handed coordinate systems. The i control points are evenly distributed on the measured rigid body, among which at least four control points are not coplanar.
Step 2. Use a scale or other simple tools to measure the 3D coordinate (rigid body coordinate)

Preparation Stage
Step 1. Establish the W system and B system in place as required. The origin of the B system is usually set at the center of mass of the measured rigid body. The B system is fixed to the rigid body and moves with the movement of the rigid body. In addition, the aforementioned W system and B system are both right-handed coordinate systems. The i control points P i are evenly distributed on the measured rigid body, among which at least four control points are not coplanar.
Step 2. Use a scale or other simple tools to measure the 3D coordinate P B i (rigid body coordinate) of P i . in the B system. The 3D coordinate P B i can be measured multiple times and the average value can be calculated to reduce errors. The coordinate data measured in this step will be used in step 7 to calculate the 6-DOF. Next, it is necessary to set up and connect the camera in a proper position. After adjusting, the camera number can be specified. The camera numbered 1 is the reference camera, and others are non-reference cameras.

Camera Calibration Stage
Step 3. Mark the position of the special checkerboard, and place the checkerboard at the position marked in advance during calibration. Here, the special checkerboard S l system and W system established on the checkerboard conform to the relationship of Equation (8). In order to mark the position of the special checkerboard, some tools such as line segment laser measuring instrument and the guide rail can be used to draw the line on the ground. Once the marking is completed, the coordinates of the origin of the W system in the S l system are measured by scale or other tools.
Step 4. Multiple cameras synchronously take a number of checkerboard images with different orientations (at least 10 images for each camera).
Step by step, place the checkerboard in the marked positions of the special checkerboard. The next step will use the special checkerboard images to calibrate the pose relationship between the C 1 . system and the W system. Following this, the experimenter holds the checkerboard and moves slowly in the shooting space when multiple cameras continue to synchronously capture checkerboard images. During this period, the checkerboard plane always faces multiple cameras so that the checkerboard is as full as possible in the public field of view of the camera.
Step 5. Select the eligible images from the checkerboard images taken in step 4, write the calibration code based on the principle of multi-camera calibration in Section 2.3 on the relevant software (MATLAB, OpenCV, and so on), and estimate the calibration parameters. Without considering the camera distortion, the calibration parameters include the intrinsic parameters of each camera ( f x , f y , u 0 , v 0 ), the pose relationship between the non-reference cameras coordinate system and the C 1 . system (rotation matrices, translation vectors), and the pose relationship between the C 1 . system and the W system (rotation matrix, translation vector). The calibration parameters will be used in step 7 to calculate 6-DOF.

6-DOF Measurement Stage
Step 6. After calibration, the camera cannot be moved, in order to ensure the whole rigid body is in the public field of view when multiple cameras synchronously shoot static images of rigid body (static measurement) or continuous motion images of rigid body (dynamic measurement). After the measurement images are taken, the pixel coordinates of the control points on the images taken by each camera are obtained. The pixel coordinates will be used in step 7 to calculate the 6-DOF.
Step 7. Use the data in step 2, 5, and 6, and write code to calculate the pose relationship between the W system and the B system (6-DOF of rigid body) based on the principle of Section 2.4. First, on the basis of the data in step 6 and Equations (15)-(18), the 3D coordinates P C 1 i of P i in the C 1 system are calculated using the least square method. Next, on the basis of the calibration parameters of step 5 (R W C 1 and T W C 1 /W ) and Equation (19), the 3D coordinates P W i of P i in the W system are calculated. Then, on the basis of the coordinates P B i , Coordinates P W i and Equation (11), R W B , T W B/W between the W system and the B system are figured out using the least square method. Finally, on the basis of R W B , the three rotation DOF are calculated by Equation (3) in Section 2.1.

Experimental Results and Discussions
In order to evaluate the performance of the proposed method, this paper takes two cameras as an example and takes the shovel of a paddy field grader as the measured rigid body. The proposed method and the Phantom 3D high-speed photogrammetry system were used to measure the 6-DOF of the rigid body at the same time. As the measurement accuracy of Phantom 3D high-speed photogrammetry system (P3HPS) can reach 0.1 mm/m, the results of P3HPS were defined as the true values and the results of the proposed method were defined as the measured values. The absolute error was calculated by the equation error = |x − x 0 |, where x represented the measured values, and x 0 represented the true values. The experiment can best be treated under two parts: evaluate the performance of the proposed method by the P3HPS and the sensitivity of the proposed method at different measurement distances. In the first part, a static measurement experiment was carried out to verify the feasibility and accuracy of the proposed method. In the second part, the experiment on the influence of measuring distance on measuring accuracy was carried out, which provides a research basis for further improving the accuracy of the method.

Experimental Setup
P3HPS, which was produced by Vision Research company, includes a Phantom VEO 410 high-speed camera, a Phantom M310 high-speed camera, a one-dimensional calibration rod, lights, a laptop, camera control software PCC 3.1, 3D measurement software TEMA 4.0, and wiring harnesses. Some parameters of two high-speed cameras are shown in Table 1. Otherwise, the two high-speed cameras have the same lens, which is Nikon 24-85 mm f/2.8-4D AF Zoom. The focal length range of this lens is 24-85 mm, and the f-number range is F22-F2.8. In addition, the images obtained by the two high-speed cameras during the measurement was also used for the measurement of the proposed method. The shovel of a paddy field grader was defined as the measured rigid body and the size of the region of interest on the shovel is 3009 × 203 mm. The elevation and horizontal cylinders on the shovel were used to control the 6-DOF change of the rigid body. Eleven control points were arranged on the shovel to assist in measuring 6-DOF. To ensure the accuracy and stability of the measurement, the control points should be evenly distributed on the rigid body and cover the main structure of the rigid body. It needs to be emphasized that the arrangement of the control points was completed by pasting paper markers (similar to the BMW logo), mainly for the convenient use of the P3HPS to identify the control points. The layout of the experiment is shown in Figure 6. The printed checkerboard pattern (11 × 8 in size, 40 mm × 40 mm for a single small square) was pasted on the plate for calibration. When marking the position of a special checkerboard, a line segment laser measuring instrument was applied to assist in drawing lines on the ground, including parallel lines of x and z axes of the W system. Install PCC 3.1 on the computer and connect it with the camera by the wire harness, then turn on the PCC 3.1 on the computer to control the cameras to shoot simultaneously. plate for calibration. When marking the position of a special checkerboard, a line segment laser measuring instrument was applied to assist in drawing lines on the ground, including parallel lines of x and z axes of the W system. Install PCC 3.1 on the computer and connect it with the camera by the wire harness, then turn on the PCC 3.1 on the computer to control the cameras to shoot simultaneously.

Camera Calibration
In the experiment environment of 6 m × 6 m × 2 m, two cameras were arranged at a distance of 3 m from the measured rigid body. The experiment operator gradually placed the checkerboard on the marked special checkerboard position and then held the checkerboard to rotate or move the checkerboard in space. Meanwhile, the two cameras simultaneously acquired images of the checkerboard. The experiment operator needed to move within the depth of field of the cameras to ensure that the checkerboard images taken are clear. The checkerboards in this experiment were all shot within 1-3 m from the cameras, which is within the depth of field. It is necessary to place the checkerboard at an angle less than 45° relative to the camera plane. In the experiment, a total of 20 pairs of suitable checkerboard images were taken for calibration, of which 7 pairs were special checkerboard images and the rest were non-special checkerboard images. Figure 7 shows a part of checkerboard images obtained during the experiment. The resolution of the checkerboard images was 1280 × 800, the f-number of the two cameras was f/22, and the focal length of the two cameras was 24 mm.

Camera Calibration
In the experiment environment of 6 m × 6 m × 2 m, two cameras were arranged at a distance of 3 m from the measured rigid body. The experiment operator gradually placed the checkerboard on the marked special checkerboard position and then held the checkerboard to rotate or move the checkerboard in space. Meanwhile, the two cameras simultaneously acquired images of the checkerboard. The experiment operator needed to move within the depth of field of the cameras to ensure that the checkerboard images taken are clear. The checkerboards in this experiment were all shot within 1-3 m from the cameras, which is within the depth of field. It is necessary to place the checkerboard at an angle less than 45 • relative to the camera plane. In the experiment, a total of 20 pairs of suitable checkerboard images were taken for calibration, of which 7 pairs were special checkerboard images and the rest were non-special checkerboard images. Figure 7 shows a part of checkerboard images obtained during the experiment. The resolution of the checkerboard images was 1280 × 800, the f-number of the two cameras was f/22, and the focal length of the two cameras was 24 mm. According to the principle of multi-camera calibration in Section 2.3 and the universal method in Section 2.5, the camera's intrinsic parameters ( , , , ), the pose relationship between the cameras, and the pose relationship from the system to W system were estimated after multicamera calibration. Different from other camera calibration, the pose relationship calibration between the W system and the system was carried out. Theoretically, the three rotational DOF from the system to the system in each special checkerboard image are the same. This condition can be used According to the principle of multi-camera calibration in Section 2.3 and the universal method in Section 2.5, the camera's intrinsic parameters f x , f y , u 0 , v 0 , the pose relationship between the cameras, and the pose relationship from the C 1 system to W system were estimated after multi-camera calibration. Different from other camera calibration, the pose relationship calibration between the W system and the C 1 system was carried out. Theoretically, the three rotational DOF from the C 1 system to the S l system in each special checkerboard image are the same. This condition can be used as a basis for judging whether a special checkerboard image is qualified. The intrinsic parameters of two cameras estimated by multi-camera calibration are shown in Table 2. The theoretical principal point coordinates of two cameras are (640, 400), but the principal point is offset as a result of errors in lens assembly, so the principal point coordinates in Table 2 are reasonable. The focal length we actually use is 24 mm and the focal length in the table is close to the real values, which preliminarily proves the reliability of the calibration results. The intermediate quantities in the calibration process of the pose relationship between the W system and the C 1 system are set out in Tables 3 and 4, where Table 3 shows the relevant data of the rotation matrix between the two coordinate systems, and Table 4 shows the relevant data of the translation vector between the two coordinate systems. The average values of rotational DOF around the x, y, and z axes between the W system and the C 1 . system were −179.17 • , −4.39 • , and −9.90 • , respectively, and the standard deviations around the three axes were 0.2121 • , 0.1183 • , and 0.3417 • (Table 3), respectively, which was roughly consistent with the actual experiment scene. The low dispersion degree of the data also indicated that the calibration data are highly reliable, with the lowest dispersion degree in the y-axis direction, the second in the x-axis direction, and the highest dispersion degree in the z-axis direction. It can be seen from Table 4 that the average values of movement DOF along the x, y, and z axes between the W system and the C 1 . system were −0.2663 m, 0.0712 m, and 3.6836 m, respectively, and the standard deviations were 0.0110 m, 0.0036 m, and 0.0145 m, respectively. The same as Table 3, the dispersion degree of the z-axis is the highest.
The results of the multi-camera calibration and the calibration results of P3HPS are compared in Table 5. The first two rows of Table 5 are the comparison of the calibration results R The latter two rows are the comparison of the calibration results R W C 1 , T W C 1 /W . Here, the calibration results of the P3HPS measurement system were defined as the true values. On the one hand, regarding the calibration of the pose relationship between two cameras, the average errors of rotation vector and translation vector were 0.26 • and 0.0028 m, respectively. As can be seen from the table below, the errors of the rotation vector and translation vector in the z-axis direction were the largest, while those in the x-axis direction were the smallest. Although the absolute error of the rotation vector and translation vector between the two cameras seems to be small, the relative error is significantly large. Therefore, we read the relevant work and check the equipment used in the experiment to analyze the reasons. In conclusion, it is caused by the following reasons: (1) The printed checkerboard has poor clarity, resulting in blurry corner points; the plane where the checkerboard was pasted is not an absolute plane, which leads to errors in corner extraction; and the size of a single small square in the checkerboard is inconsistent. (2) In the calibration images, the checkerboard has a large amount of movement in the depth direction. When the checkerboard is far from the camera, the checkerboard in the calibration image is too small, which makes the calibration inaccurate. (3) Because this paper is based on the pinhole imaging model, the multi-camera calibration does not consider the distortion, which makes the relative error large. One the other hand, regarding the calibration of the pose relationship between the W system and the C 1 system, the average errors of the rotation vector and translation vector were 0.32 • and 0.0042 m. The error of rotation vector in the z-axis direction was the largest, while that in the x-axis direction was the smallest. The maximum and minimum errors of translation vector were opposite to the rotation vector. The accuracy of the calibration method can be considered to meet the requirements according to the maximum error and the average errors. On the basis of the above, the multi-camera calibration is practical and feasible, and the calibration of the pose relationship between the W system and the C 1 system is completed while calibrating the pose relationship between the cameras. Table 3. Rotation matrices from the C 1 system to the W system.  Table 4. Translation vectors from the C 1 system to the W system.  Table 5. Calibration comparisons between the proposed method and Phantom 3D high-speed photogrammetry system (P3HPS).

Experimental Results for 6-DOF Measurement
On the basis of the calibration in Section 3.2.1, 50 static rigid body images with different poses were taken for 50 static measurements. Here, the positions of two camera were the same as that in Section 3.2.1. Otherwise, the accuracy of the proposed method was verified by P3HPS. The resolution of the captured images was 1280 × 800 and the cameras parameters settings were the same as in Section 3.2.1. The results of 50 measurements by the proposed method were calculated based on Section 2.4.
In 50 static measurements, the average error of the rotational DOF measured by the proposed method was 1.0557 • and the average error of the movement DOF was 0.0065 m. The standard deviations of the DOF of rotation and the DOF of movement were 0.3396 • and 0.0027 m, respectively. If ϕ, θ, Ψ, T x , T y , T z was used to represent the measured values of the rotational DOF and movement DOF of the rigid body in x, y, and z axes, ϕ 0 , θ 0 , Ψ 0 , T x0 , T y0 , T z0 represented the true values of the 6-DOF of the rigid body. The average errors of each DOF and the standard deviations of errors can be calculated as set out in Table 6. By longitudinal comparison of the table, it can be found that the average error of the z-axis was the largest when measuring the rotational DOF, which can reach 1.4117 • . The average error of the z-axis was also the largest when measuring the movement DOF, which can reach 0.0072 m. In addition, the standard deviations of the measurement errors of the rotational DOF and movement DOF in the z-axis direction were larger than those of the other two axes. Draw the true value, measured value, and the errors of 50 static measurements in the form of line diagrams, as shown in Figure 8. In 50 static measurements, the maximum errors of ϕ, θ, Ψ, T x , T y , and T z were 1.3325 • , 1.2058 • , 2.1257 • , 0.0073 m, 0.0094 m, and 0.0144 m, respectively, and the minimum errors were 0.3371 • , 0.6730 • , 0.9147 • , 0.0040 m, 0.0037 m, and 0.0002 m, respectively. The longitudinal comparison results in Table 6 can be seen again by observing the degree of fit (Figure 8) between the measured values line (black) and the true values line (blue). Table 6. Average errors and standard deviations of six-degrees-of-freedom (6-DOF) measurement of rigid body.

Parameters
Average Combining Table 6 and Figure 8, the following two conclusions can be drawn: (1) the accuracy of the proposed method to measure the 6-DOF of rigid bodies is routine. In general, at a measuring distance of 3 m, the average error of the rotational DOF was better than 1.1°, and the average error of the movement DOF was better than 0.007 m. (2) The measurement errors and standard deviations of the movement DOF and rotational DOF in the z-axis direction were larger than those in the other two directions. From this point of view, the measurement accuracy and stability in the z-axis direction are slightly worse than the other two directions, which is the same as in [41]. In measurement, the camera's depth of field was constant. Because the range of depth of field is limited, the control points on the rigid body may not be able to shoot clearly as the measured rigid body leaves the range of depth of field, which results in errors. Furthermore, the measured values in the z-axis direction are the largest. When we use Equation (14) to calculate, the error amplification in the z-axis direction will be larger than in the other two directions. In addition, the f-number, focal length, and distance of the focused object will all have an effect on the depth of field. A smaller f-number and shorter focal length will have a larger depth of field. In the experiment, we have used the smallest f-number and focal length. If a lens with a smaller f-number and focal length is used, the result may be better. Combining Table 6 and Figure 8, the following two conclusions can be drawn: (1) the accuracy of the proposed method to measure the 6-DOF of rigid bodies is routine. In general, at a measuring distance of 3 m, the average error of the rotational DOF was better than 1.1 • , and the average error of the movement DOF was better than 0.007 m. (2) The measurement errors and standard deviations of the movement DOF and rotational DOF in the z-axis direction were larger than those in the other two directions. From this point of view, the measurement accuracy and stability in the z-axis direction are slightly worse than the other two directions, which is the same as in [41]. In measurement, the camera's depth of field was constant. Because the range of depth of field is limited, the control points on the rigid body may not be able to shoot clearly as the measured rigid body leaves the range of depth of field, which results in errors. Furthermore, the measured values in the z-axis direction are the largest. When we use Equation (14) to calculate, the error amplification in the z-axis direction will be larger than in the other two directions. In addition, the f-number, focal length, and distance of the focused object will all have an effect on the depth of field. A smaller f-number and shorter focal length will have a larger depth of field. In the experiment, we have used the smallest f-number and focal length. If a lens with a smaller f-number and focal length is used, the result may be better.

Sensitivity at Different Measurement Distances
In this section, whether the measurement distance has an impact on accuracy was studied and a total of three measurement distances (2.50 m, 2.75 m, 3.00 m) were selected. In addition, the measurement was repeated 30 times at each measurement distance. In the experiment, the arrangement of control points and the other settings were the same as in Section 3.2, but the focal length was slightly adjusted with the distance changes. The measured values obtained by the proposed method were compared with the true values of P3HPS, and the average errors and standard deviations of the rotational DOF and the movement DOF of each distance were calculated, as shown in Table 7. The maximum measurement average error of the rotational DOF reached 1.0737 • and the minimum measurement average error reached 0.7578 • , which appeared at the measurement distances of 3.00 m and 2.50 m, respectively. The maximum and minimum values of the standard deviations appeared at the measurement distances of 3.00 m and 2.75 m, respectively. The maximum and minimum measurement average error of the movement DOF were 0.0067 m and 0.0039 m, respectively, which appeared at the measurement distances of 3.00 m and 2.50 m, and the maximum and minimum values of standard deviations were also found in these two measurement distances. On the whole, the errors of the measurement results of the 2.50 m measurement distance and the degree of dispersion of the data are better than the results of the other measurement distances. Moreover, in the three measurement distances, the accuracy and stability of the measurement results increase as the measurement distance decreases. If the measurement errors of different measurement distances are drawn separately according to different DOF ϕ, θ, Ψ, T x , T y , T z , the line diagrams shown in Figure 9 can be obtained. From the lines in Figure 9a-c, the measurement errors comparison of the three measurement distances can be seen as follows: measurement errors of 2.50 m < measurement errors of 2.75 m < measurement errors of 3.00 m. It can also be seen from the volatility of the lines that the standard deviation of the measurement error of 2.50 m was the smallest, and that of 3.00 m was the largest. This is consistent with the results presented in Table 7. In addition, Figure 9c shows the measurement errors of the rotational DOF and the movement DOF in the z-axis direction. Compared with the x-axis measurement error lines in Figure 9a and the y-axis measurement error lines in Figure 9b, the measurement errors in the z-axis direction and the degree of dispersion were slightly larger than those in the x and y axes directions; this again verified the experimental results in Section 3.2.

Sensitivity at Different Measurement Distances
In this section, whether the measurement distance has an impact on accuracy was studied and a total of three measurement distances (2.50 m, 2.75 m, 3.00 m) were selected. In addition, the measurement was repeated 30 times at each measurement distance. In the experiment, the arrangement of control points and the other settings were the same as in Section 3.2, but the focal length was slightly adjusted with the distance changes. The measured values obtained by the proposed method were compared with the true values of P3HPS, and the average errors and standard deviations of the rotational DOF and the movement DOF of each distance were calculated, as shown in Table 7. The maximum measurement average error of the rotational DOF reached 1.0737° and the minimum measurement average error reached 0.7578°, which appeared at the measurement distances of 3.00 m and 2.50 m, respectively. The maximum and minimum values of the standard deviations appeared at the measurement distances of 3.00 m and 2.75 m, respectively. The maximum and minimum measurement average error of the movement DOF were 0.0067 m and 0.0039 m, respectively, which appeared at the measurement distances of 3.00 m and 2.50 m, and the maximum and minimum values of standard deviations were also found in these two measurement distances. On the whole, the errors of the measurement results of the 2.50 m measurement distance and the degree of dispersion of the data are better than the results of the other measurement distances. Moreover, in the three measurement distances, the accuracy and stability of the measurement results increase as the measurement distance decreases.  ( , , , , , ), the line diagrams shown in Figure 9 can be obtained. From the lines in Figure 9a-c, the measurement errors comparison of the three measurement distances can be seen as follows: measurement errors of 2.50 m < measurement errors of 2.75 m < measurement errors of 3.00 m. It can also be seen from the volatility of the lines that the standard deviation of the measurement error of 2.50 m was the smallest, and that of 3.00 m was the largest. This is consistent with the results presented in Table 7. In addition, Figure 9c shows the measurement errors of the rotational DOF and the movement DOF in the z-axis direction. Compared with the x-axis measurement error lines in Figure 9a and the y-axis measurement error lines in Figure 9b, the measurement errors in the z-axis direction and the degree of dispersion were slightly larger than those in the x and y axes directions; this again verified the experimental results in Section 3.2.
(a)  It can be seen from Table 7 and Figure 9 that, among the three measurement distances, the measurement results at 2.50 m were more accurate and more stable than the results of the other two measurement distances, but more experiments need to be carried out to find the optimal measurement distance. The reasons for this phenomenon can be summarized in three aspects: pixel coordinates reading, lens distortion, and camera calibration. (1) In terms of pixel coordinates reading, on the basis of the pixel coordinates, it is calculated by Equations (14) and (15) after reading the pixel coordinates of the control points. According to the calculation equation and Figure 4, it is obvious to know that, when there is a certain error in the pixel coordinates, the error of the calculation result of Equation (14) increases as the measurement distance increases. As a result, it may cause the error of the 6-DOF measurement to increase. (2) In terms of lens distortion, the lens assembly will inevitably cause errors, which will cause lens distortion. The distortion of the lens will cause some pixels in the image to shift, so that the pixel coordinates of the control points in the image are inconsistent with the actual situation. As a result, even without reading errors, the pixel coordinates are still inaccurate, and the impact of inaccurate pixel coordinates is the same as the point of view in the previous part.
( 3) In terms of camera calibration, because the calibration data obtained by camera calibration are used throughout the entire measurement process, the impact it produces is conceivable.

Conclusions
In this paper, a multi-camera universal measurement method for 6-DOF of rigid bodies in the world coordinate system is proposed. This method only needs to use at least two cameras to achieve measurement, which is made available for most morphological rigid bodies. First of all, on the basis of Zhang Zhengyou's calibration method, multi-camera calibration is introduced. The pose relationship between the camera coordinate system and the world coordinate system is obtained, which is different from other calibrations. Meanwhile, the intrinsic and extrinsic parameters of the camera are estimated. Secondly, on the basis of the pinhole camera model, the 6-DOF solution model of the proposed method is gradually analyzed by the matrix analysis method. The proposed method uses the control points on the measured rigid body to achieve the calculation of the 6-DOF by least squares methods. Finally, P3HPS (Phantom 3D high-speed photogrammetry system) with an accuracy of 0.1 mm/m was used to evaluate the performance of proposed method. The experiment results show that the average error of the rotational DOF is less than 1.1 deg, and the average error of the movement DOF is less than 0.007 m.  It can be seen from Table 7 and Figure 9 that, among the three measurement distances, the measurement results at 2.50 m were more accurate and more stable than the results of the other two measurement distances, but more experiments need to be carried out to find the optimal measurement distance. The reasons for this phenomenon can be summarized in three aspects: pixel coordinates reading, lens distortion, and camera calibration. (1) In terms of pixel coordinates reading, on the basis of the pixel coordinates, it is calculated by Equations (14) and (15) after reading the pixel coordinates of the control points. According to the calculation equation and Figure 4, it is obvious to know that, when there is a certain error in the pixel coordinates, the error of the calculation result of Equation (14) increases as the measurement distance increases. As a result, it may cause the error of the 6-DOF measurement to increase. (2) In terms of lens distortion, the lens assembly will inevitably cause errors, which will cause lens distortion. The distortion of the lens will cause some pixels in the image to shift, so that the pixel coordinates of the control points in the image are inconsistent with the actual situation. As a result, even without reading errors, the pixel coordinates are still inaccurate, and the impact of inaccurate pixel coordinates is the same as the point of view in the previous part.
( 3) In terms of camera calibration, because the calibration data obtained by camera calibration are used throughout the entire measurement process, the impact it produces is conceivable.

Conclusions
In this paper, a multi-camera universal measurement method for 6-DOF of rigid bodies in the world coordinate system is proposed. This method only needs to use at least two cameras to achieve measurement, which is made available for most morphological rigid bodies. First of all, on the basis of Zhang Zhengyou's calibration method, multi-camera calibration is introduced. The pose relationship between the camera coordinate system and the world coordinate system is obtained, which is different from other calibrations. Meanwhile, the intrinsic and extrinsic parameters of the camera are estimated. Secondly, on the basis of the pinhole camera model, the 6-DOF solution model of the proposed method is gradually analyzed by the matrix analysis method. The proposed method uses the control points on the measured rigid body to achieve the calculation of the 6-DOF by least squares methods. Finally, P3HPS (Phantom 3D high-speed photogrammetry system) with an accuracy of 0.1 mm/m was used to evaluate the performance of proposed method. The experiment results show that the average error of the rotational DOF is less than 1.1 deg, and the average error of the movement DOF is less than 0.007 m.
The proposed method does not need expensive and professional instruments. The measurement process is simple and the principle is not complicated. As the final measurement results of the proposed method are the 6-DOF between the world coordinate system and the measured rigid body coordinate system, the measurement is reproducible and the measurement results of the 6-DOF of the rigid body are not restricted by the movement of the camera, which is different from other vision measurement methods. Certainly, the proposed method still has limitations. On the one hand, for a measured rigid body with a too large moving range, it may exceed the camera's field of view, resulting in an inability to measure. On the other hand, there are more dynamic measurement problems to be solved [48][49][50], which will be the key problem of our research in the future.