A Method of Measuring the Absolute Position and Attitude Parameters of a Moving Rigid Body Using a Monocular Camera

1. Introduction

Non-contact measurement technology for determining trajectory and attitude changes of rigid moving targets has become a focus of research with the rapid advancement of science and technology [1,2,3]. Visual technologies for measuring the pose of a target are widely used in obtaining motion parameters and mainly involve monocular, binocular, and multi-ocular measurements, with each technology having its advantages and disadvantages. However, whereas binocular and multi-ocular measurements suffer from a small field of view and difficulties in stereo matching, monocular measurements have a simple structure, large field of view, strong real-time performance, and good accuracy. Monocular systems are thus used widely to measure parameters of rigid-body motion [2,3,4,5].

There are two types of monocular visual measurement of pose according to the features selected, namely measurements of cooperative targets [9,10,11] and measurements of non-cooperative targets [12,13,14,15]. Among them, the spatial constraint relationship between target feature points in cooperative target pose measurement is controllable, which to some extent limits the application scope but reduces the difficulty of feature extraction, improves the extraction accuracy, and reduces the complexity of the pose calculation. At present, research on cooperative target measurement methods has focused mainly on cooperative target design, feature extraction, pose calculation methods, and pose calculation errors [16,17,18,19,20]. However, most of the results are based on calibrated camera parameters and there has been little research on the pose estimation problem of cameras having varying focal lengths. In some practical problems, such as the measurement of multiple rigid-body motion parameters addressed in this article, the internal parameters of the camera need to be frequently adjusted and changed, and there are many factors that affect the calibration accuracy of the internal parameters of a camera. It is difficult to accurately determine the internal parameters in the absence of standard equipment. Therefore, research on uncalibrated camera pose estimation technology is also important. The main methods of solving the pose measurement problem of uncalibrated cameras are the classic two-step method [21], direct linear transform (DLT) method [22], Zhengyou Zhang calibration method [23], P4P and P5P methods [24], and AFUFPnP method [25] proposed by Tsai and others. The two-step method is not suitable for dynamic measurement. P4P and P5P methods face noise sensitivity problems because they use fewer control points to estimate internal and external parameters. Zhengyou Zhang’s calibration method requires the installation of a planar chessboard on the measured object to ensure accuracy, which is inconvenient in the case of a moving rigid body. The AFUFPnP method is a pose estimation method based on the EPnP [26] and POSIT [27] algorithms. This method has high pose estimation accuracy and calculation efficiency but low execution efficiency.

Against the above background, this article combines the foundations of previous research on pose measurement to theoretically derive a method of measuring the absolute pose of a moving rigid body and presents a complete implementation of the method. The method is validated in terms of its feasibility and experimental repeatability for different positional accuracies using a high-speed camera having a variable focal length.

2. Theoretical derivation of methods

2.1. Principle of the absolute pose measurement for a rigid body

As shown in Figure 1, Body and World rigid bodies move relative to one another in space. For the convenience of expression, the body coordinate system O_B-X_BY_BZ_B, world coordinate system O_W-X_WY_WZ_W, and camera coordinate system O_C-X_CY_CZ_C are respectively denoted B, W, and C hereafter. We let P and Q denote physical points on the body and world rigid bodies, respectively. We use superscripts to distinguish the coordinate values of the physical points P and Q in the different coordinate systems and subscripts to distinguish different points on the rigid body, For example, $P_3^W$represents the coordinate values of P₃ point on the Body rigid body in the W system. We use minuscule letters to represent pixel points; e.g., p represents the pixel points projected by physical point P on the image. R and T represent conversion relationships between coordinate systems, where superscript is used to represent the target coordinate system in the conversion and subscript is used to represent the starting coordinate system; e.g., $R_C^W$is the rotation matrix for conversion from the C system to the W system, and $T_B^W$is the translation matrix for the conversion from the B system to the W system.

The aim of this article is to measure the motion pose of a rigid body in the B system relative to the W system, and the real-time spatial coordinate $P^W$ of the center of mass P of the B-system rigid body in the W system.

The imaging projection formulas are

$$P^W=T_C^W+m{R}_C^W{p}^C$$

(1)

$$P^B=T_C^B+m{R}_C^B{p}^C$$

(2)

Here, $m=\frac{‖P^C‖}{‖p^C‖}$ is the ratio of two vector modes, representing the spatial coordinates of the pixel point p in the C system.

It follows from (1) and (2) that

$$P^W=T_C^W+R_C^W{\left(R_C^B\right)}^T\left(P^B-T_C^B\right)$$

(3)

$$R_B^W=R_C^W{\left(R_C^B\right)}^T$$

(4)

$$T_B^W=T_C^W-R_C^W{\left(R_C^B\right)}^T{T}_C^B$$

(5)

The motion pose of the rigid body can be solved by $R_B^W$, if

$$R_B^W=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

According to reference [28], the attitude angle of a rigid body is

$$\left\{\begin{array}{c}{Q}_z=atan2\left(r_{21},r_{11}\right)\\ Q_y=atan2\left(-r_{31},\sqrt{r_{32}^2+r_{33}^2}\right)\\ Q_x=atan2\left(r_{32},r_{33}\right)\end{array}\right.$$

(6)

Here, $Q_z$, $Q_y$and $Q_x$are the rotation angles around the Z, Y, and X axes, with the order of rotation being Z, Y, and X.

It is seen from (3) and (4) that for $P^W$and $R_B^W$, $R_C^W$, $T_C^W$, $R_C^B$and $T_C^B$need to be determined. $R_C^W$ and $T_C^W$ are determined from the control points on the W-system rigid body whereas $R_C^B$ and $T_C^B$ are determined from the control points on the B-system rigid body. The absolute pose problem of a moving rigid body is thus transformed into a problem of solving the rotation and translation matrices.

2.2. Implementation steps of the measurement method

Figure 2 is a flowchart of the method of measuring the rigid-body pose derived in Section 2.1.

The measurement of the pose parameters of the rigid body involves the following steps. Groups of control points with each group having more than six non-coplanar points are first arranged in the W and B systems. R and T for the W–C and B–C conversions are then calibrated and calculated to calculate the coordinates of the B-system control points in the W system and to calculate the attitude changes of the B-system rigid body. Finally, the other parameters are measured.

2.3. Determination of the rotation and translation matrices

This paper considers a high-speed camera with a variable focal length. To facilitate repeated experiments, a DLT [29] camera calibration method providing simple operation and high calculation efficiency is adopted to obtain the camera rotation and translation matrices. The DLT method does not require an approximate initial value of the internal orientation element (i.e., it does not require the manual calculation of the initial value in contrast with other space rendezvous algorithms) and is suitable for the calibration processing of non-metric digital cameras [30].

As shown in Figure 1, N (N ≥ 6) control points are arranged on each of the B and W series. Taking the control points on the body rigid body as an example, if the spatial coordinates are, the corresponding pixel coordinates are. It follows from the collinearity equation of the DLT method that

$$\left\{\begin{array}{c}{u}_i=\frac{L_1{X}_i+L_2{Y}_i+L_3{Z}_i+L_4}{L_9{X}_i+L_{10}{Y}_i+L_{11}{Z}_i+1}\\ v_i=\frac{L_5{X}_i+L_6{Y}_i+L_7{Z}_i+L_8}{L_9{X}_i+L_{10}{Y}_i+L_{11}{Z}_i+1}\end{array}\right.$$

(7)

where $L_i\left(i=\mathrm{1,2},\cdots 11\right)$ denotes coefficients representing the camera’s internal and external parameters.

Formula (7) shows that for the 11 unknowns, at least six points are required to determine the 11 values. As the number of equations is greater than the number of unknowns, the least squares method is used to solve for L and thus obtain the external parameters of the camera.

The constraint conditions of the rotation matrix are not fully considered in obtaining the results using the proposed method, and the error in the results is thus bound to be large. To improve the accuracy, this paper uses Gauss–Newton iterative method to solve the 11 L equations and six constraint equations of the rotation matrix iteratively and then applies singular value decomposition to the obtained rotation matrix. If $R=UD{V}^T$, then $R_C^B={UV}^T$. Similarly, $R_C^W$ and $T_C^W$ can be determined. After this processing, the estimated attitude matrix strictly meets the inherent constraints of the rotation matrix, and the estimation error is effectively reduced.

3. Experiments and data analysis

The experimental platform used in this study mainly comprised a three-axis turntable and camera. The SGT320E three-axis turntable had an accuracy of 0.0001 degrees and a mechanical structure in which a U-shaped outer frame rotated around the azimuth axis, an O-shaped middle frame rotated around the pitch axis, and an O-shaped inner frame rotated around the transverse roller axis. The outer frame, middle frame, and inner frame can all be considered rigid bodies, as shown in Figure 3. The three axes simultaneously had speed, position, and sinusoidal oscillation modes and did not interfere with each other, enabling the high-precision measurement of pose. The Phantom M310 high-speed camera, produced by Vision Research in the United States, had adjustable resolution, shooting speed, and exposure time.

3.1. Establishment of the coordinate system and arrangement of control points

For the convenience of arranging control points, the inner frame of the turntable was locked and formed a body rigid body with the middle frame, whereas the outer frame was a world rigid body. The origins of the B and W systems were both set at the center of the turntable. According to the construction of the turntable, this point was also the center of mass of the body rigid body, as shown in Figure 4. Figure 5 shows the scene of the experiment, which truly reflects the layout of each marking point. Eight control points were arranged on the Body rigid body (i.e., the middle frame) and eight control points were arranged on the World rigid body (i.e., the outer frame).

As the geometric structure and dimensions of the three-dimensional turntable were fixed, the three-dimensional coordinates of the control and test points on the middle and outer frames could be measured easily.

3.2. Determination of pose in the turntable measurement

For the convenience of experimental verification, the world rigid body (i.e., the outer frame of the turntable) is set stationary. As only the central axis rotates, the real-time motion attitude angle of the turntable changes only with the rotation angle around the X-axis, with the other two angles being zero.

Denoting the angle of rotation of the frame of the turntable around the X-axis as, the real-time position coordinates of any point on the body rigid body in the W system are

$$P_1^W=P_1^B\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\theta }_x& -\sin {\theta }_x\\ 0& \sin {\theta }_x& \cos {\theta }_x\end{array}\right]$$

(8)

The center of mass of the body rigid body is at the origin of the B and W systems and can be inferred from equation (8).

3.3. Experiment on measuring the pose of a rigid body with a camera

We set the body rigid body motion mode to swing mode, with the swing parameters being an amplitude of 20 degrees, frequency of 0.5 Hz, and time of 10 s. To ensure that the high-speed camera recorded the entire motion of the turntable, the high-speed camera was operated at 500 frames/second, and 16.68 seconds of video was collected. The test scene is shown in Figure 5.

In verifying the ability to make repeated measurements adopting the proposed method, the camera was placed at three positions to measure the swing mode of the central axis of the turntable, with each position being 2.21 m from the rigid body, as shown in Figure 6.

3.3.1. Attitude measurement and analysis

We used the high-speed camera to record video frames of dynamic changes in the middle and outer frames of the turntable. To ensure that the pixel coordinate measurement did not affect the pose measurements, the pixel coordinates of the control points were determined using TEMA, an advanced motion analysis tool developed by Image Systems AB in Sweden. We adopted the DLT method mentioned earlier for measurements and then used formulas (4) and (6) to obtain the attitude of the turntable. Dynamic changes in the attitude of the turntable are shown in Figure 7.

Figure 7 shows the change in the measured attitude angle of the rigid body when the camera is at a distance of 2.21 m. Figure 7a–c show the real-time change in the camera’s attitude angle $Q_x$ of the middle frame of the turntable measured for different camera positions. Table 1 compares the mean absolute error and standard deviation of the camera’s attitude angle measured for the three camera positions. Figure 7a–c and Table 1 reveal that the minimum error in the attitude angle $Q_x$ measured by the camera at the second camera position. The mean absolute error difference of the attitude angle measured at three positions is not significant (not more than 0.2 degrees). Figure 7d reflects the error of the camera in measuring the attitude angle $Q_x$ at the first position. The error increases with the tilt of the rigid body relative to the camera lens. The mean absolute error is 0.8092 degrees, the standard deviation is 0.6623 degrees, and there are individual errors as large as 2.5 degrees. Figure 7e,f show that although only one attitude angle $Q_x$ is changing in theory, the angles $Q_y$ and $Q_z$ measured by the camera still include errors. The mean absolute error is 0.186 degrees and the standard deviation is 0.641 degrees for $Q_y$, and the mean absolute error is −0.032 degrees and the standard deviation is 0.222 degrees for $Q_z$.

3.3.2. Measurement and analysis of the position of the center of gravity of the rigid body

The DLT method mentioned earlier was used to measure $R_C^W$, $T_C^W$, $R_C^B$, and $T_C^B$. The centroid coordinates of the rigid body were $P^B={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$, and the real-time position changes were calculated using formula (3).

Figure 8, Figure 9 and Figure 10 present the three-dimensional coordinate changes of the body’s center of mass as measured by the monocular camera at the first position. It is seen that the error in the three-dimensional position of the center of mass of the rigid body increased with the tilt of the rigid body relative to the camera lens. This is because the constraint of the non-coplanar control points on the rigid body decreased and the imaging quality of the control points deteriorated when the tilt ratio increased. The error in the X-axis coordinate of the rigid body center was much smaller than the errors in the other two axis coordinates, because the middle-frame swinging motion of the rigid body involved rotation around the X-axis, and all control points had the best X-axis constraint during the motion.

Table 2 gives the errors in the camera measurements made at the three camera positions. The error in the coordinates of the center of mass was small for the three camera positions. The maximum deviation of the mean absolute error was 0.53 cm and the maximum deviation of the standard deviation was 0.66 cm, indicating that the measurement process was insensitive to the camera position and that repeated measurements could be made.

4. Conclusions

This article systematically derived a method of measuring the absolute pose of a moving rigid body using a monocular camera based on a camera imaging model. A turntable test was conducted to verify that the proposed method could accurately measure position and attitude in repeated measurements within the visual range of the camera. The mean absolute error in the attitude angle measured at different camera positions varied by no more than 0.2 degrees. The error in the attitude angle measured by the camera at the first camera position increased with the tilt angle of the rigid body relative to the camera lens. The mean absolute error was 0.8092 degrees, the standard deviation was 0.6623 degrees, and there were individual errors up to 2.5 degrees. An analysis of the error in the coordinates of the center of mass position showed that the deviation of the measurements across the three camera positions was small, the maximum deviation of the mean absolute error was 0.53 cm, and the maximum deviation of the standard deviation was 0.66 cm. The measurement was thus insensitive to the camera placement, and repeated measurements could be made. Although the proposed method requires the placement of two sets of marker points, which may be infeasible, it is suitable for non-professional and low-cost measurement applications.