Development and calibration of an accurate 6-degree-of-freedom measurement system with total station

To meet the demand of high-accuracy, long-range and portable use in large-scale metrology for pose measurement, this paper develops a 6-degree-of-freedom (6-DOF) measurement system based on total station by utilizing its advantages of long range and relative high accuracy. The cooperative target sensor, which is mainly composed of a pinhole prism, an industrial lens, a camera and a biaxial inclinometer, is designed to be portable in use. Subsequently, a precise mathematical model is proposed from the input variables observed by total station, imaging system and inclinometer to the output six pose variables. The model must be calibrated in two levels: the intrinsic parameters of imaging system, and the rotation matrix between coordinate systems of the camera and the inclinometer. Then corresponding approaches are presented. For the first level, we introduce a precise two-axis rotary table as a calibration reference. And for the second level, we propose a calibration method by varying the pose of a rigid body with the target sensor and a reference prism on it. Finally, through simulations and various experiments, the feasibilities of the measurement model and calibration methods are validated, and the measurement accuracy of the system is evaluated.

Liu et al introduce a 6-DOF measurement method using multitargets with a single-station instrument similar to an indoor GPS [10]. For the principle is very like the PnP technique, their disadvantages remain to be solved. Besides, some 6-DOF probes based on laser trackers are developed by commercial companies like Leica and API, which have excellent accuracies [11]. The scenario from Leica uses a motorized camera on the tracker station to track the multi-points on the probe, while API invents a cooperative sensor with a pinhole prism whose vertex is cut to form a light channel and with a PSD in the back to calculate the incident angle of the tracker beams. However, laser trackers are extremely costly and not useful in outdoor environments. Stephen Kyle also discussed plenty of designs in 6-DOF probing cooperated with laser trackers or total stations [12], but he has not processed any further study. Furthermore, many combined measurement scenarios using cameras, laser sensors, structured lights and even motorized stages have also been proposed [13][14][15][16][17]. Despite meeting their own requirements, these methods are not good enough to perform accurately in long-range measurement.
As described above, most of the 6-DOF measurement systems are mainly composed of two parts: a base station (or multi-stations) which is always stationary and a cooperative target sensor which is placed on the target. Laser trackers, indoor GPS, cameras and total stations are competent to act as base measurement stations. Of all these traditional instruments in LSM, total station is a popular tool in both industries and outdoor engineering, showing its advantages in flexibility, efficiency in measurement, low on-site calibration requirements, long range (at least hundreds of meters) and relatively high accuracy (1 mm level). Therefore, based on the aforementioned studies, this paper develops a pose measurement system with total station, and the cooperative target sensor, which is mainly composed of a pinhole prism, an industrial lens, a camera and a biaxial inclinometer, is referred to as pose measurement target sensor (PMTS).
PMTS is a single-point 6-DOF measurement target, and is designed to be very small and light for convenient use. A precise mathematical model is established from the original data observed by the total station, the camera and the inclinometer to the final six pose variables. And calibration methods are presented to determine the unknown parameters in the model. With the help of total station, the precise model and calibration we proposed guarantee this system performs accurately in long-range measurement.
This paper is organized as follows. In section 2, the configuration of the target sensor PMTS is described, and the mathematical model for 6-DOF measurement is established in section 3. Section 4 introduces calibration methods of different levels. Then, several simulations and experimental validations are described in section 5. Finally, concluding remarks and a brief overview of further work are presented.

Sensor configuration
The target sensor PMTS is designed light and handy with 150 mm × 90 mm × 90 mm in size and 1.75 kg in weight. Its structure inside the aluminum casing is shown in figure 1. The pinhole prism is processed from a traditional cube-corner prism, and its vertex is cut in a small part to form a light channel. An industrial lens with optical filter, a camera, an inclinometer and processing circuits are integrated on a steel  support behind the pinhole prism. Outside the casing, the DC power supply and Ethernet communication with an upper computer are both through the cable. The upper computer on the one hand controls the total station and transfers its measurement data to PMTS, on the other hand, receives the result processed by the CPU of PMTS, displaying it and using it for subsequent applications.

Light path and imaging
During practical applications, the measurement beam (can be considered as parallel light) from a total station is incident on the pinhole prism. The light path diagram is illustrated in figure 2. For PMTS, most of the incident light is reflected back by the prism which acts as a 3D cooperative target as usual. Meanwhile, the other part passes through the pinhole and is recorded by the subsequent lens and camera. Since the pinhole of the cube-corner prism is triangular, the image behind it is also triangular and diffraction phenomena exists (figure 2(b)). By the effect of a circular aperture which clings to the pinhole, the image behind it is round (figure 2(c)). Finally, the converging lens which focuses at infinity force the spot on the imaging plane concentrated (figure 2(d)), and the center of gravity method [18] is used to find a sub-pixel centroid of the spot. In addition, the optical filter added in the light path makes sure that lights only in wave band of the measurement laser can reach to the camera, and tiny stray lights are easily eliminated according to their sizes. Although in most cases, good imaging is achievable, active light sources like spotlights which emit strong light in a large spectrum maybe disturb the main spot, so the use of PMTS should avoid these light sources.

Definition of coordinate systems
A total of three different coordinate systems are defined in the 6-DOF measurement: the total station frame (O T X T Y T Z T -coordinate, frame T) at the base, the sensor frame (O S X S Y S Z S -coordinate, frame S) and the camera frame (O C X C Y C Z C -coordinate, frame C) in the sensor, as shown in figure 3. A total station is a spherical coordinates measuring system, origin O T is the starting point of the ranging measurement, Z T -axis is vertical, and X T -axis and Y T -axis are respectively defined as the north and the east in geodesy. Since total station must be leveled during measurement, X T -Y T plane is parallel to the ground horizontal plane. We define the reflection center of the prism on PMTS as the origin of frame S, the directions of the inclinometer's two axes as the directions of X S -axis and Y S -axis, and Z S -axis is normal to the X S -Y S plane.
In frame C, taking all the components in the light path including the prism into account, the reflection center of the prism where each incident rays intersect is also defined as the origin O C . And X C -axis and Y C -axis are defined by the camera as traditional definition. As frame T is a left-hand coordinate system (inherent definition of a total station), all the coordinate systems in this paper are defined as left-hand system.
In order for an easily understanding, we list the naming rule of the variables in this paper in table 1.

Imaging model
Different from a traditional imaging system which maps the 3D points in a world coordinate system to 2D image points, The transformation is from frame S to frame T The marks both on the upper left and lower left corner of a matrix represent a transformation between two coordinate systems the imaging system in PMTS instead maps laser beams. This imaging system conforms to the pinhole model with lens distortion, and the reflection center of the prism is the optical center. As shown in the camera frame definition part of figure 3, Let unit vector v C denotes the vector of the laser beam in frame C, (u′, v′) denote the real (distorted) image pixel coordinates, and (u, v) denote ideal image pixel coordinates, according to the pinhole imaging model, the four-parameter model of a camera [9] is given by where (u 0 , v 0 ) denote the image coordinates of the camera's principal point, (x, y) are ideal normalized image coordinates, a x and a y are scale factors. Then optical lens distortion is added to the ideal coordinates in order to obtain a precise model, and in this paper, we only consider the first two terms of radial distortion [19].
where k 1 and k 2 are the coefficients of the radial distortion.
The imaging model has been established from laser beam vectors to the corresponding centroid of the spots. And in reverse, the laser beam vector v C can also be obtained from the corresponding centroid of the real spot (u′, v′) according to equations (1)-(3) with all the camera parameters calibrated.

Measurement model
The underlying principle of the 6-DOF pose measurement is to solve for six unknowns (three positions and three orientations) in the rotation matrix R T S and the translation vector t T S . With the spatial transformation, a point p S in the coordinate frame S could be mapped as the same point p T in the coordinate frame T as follow: Note that orientations are represented in X-Y-Z fixed angles, known as roll-pitch-yaw (γ, β, α) with respect to the reference frame, and they are illustrated in figure 3, so R T S can be expressed as follow: The translation vector t T S is directly observed from the total station measurement of the prism on PMTS. The calculation of roll and pitch angel is based on the assumption that the horizontal plane on PMTS and on total station is the same. Since earth curvature effect on a distance of 100 m is just about 3″, which is far less than the measurement uncertainty of the inclinometer (0.005°), we consider the horizontal plane as the same. Then roll angle γ and pitch angle β are obtained from the inclinometer through simple geometrical calcul ations according to the coordinate system definition of frame S. In detail, as shown in the sensor frame definition part of figure 3, observed data (η, θ) indicate the inclined angles between the axes of the inclinometer and the horizontal plane which is parallel to X T -Y T plane, then β and γ are calculated using (6), and the corresponding proof is presented in appendix A.
The last degree of freedom is the yaw angle α. As the origin of frame C and frame S is defined the same, the transformation between these two coordinate systems just needs three orientations, so we assume the rotation matrix from frame C to frame S as R S C . The unit vector of the measurement beam in Frame T is the normalization of translation vector t , then the transformation relationship between v C and v T is described as the following Substituting for R T S by (5) in (7), simplifying and results in: Only the first two rows of equation (8) include the unknown yaw angle α, after the roll and pitch angle are calculated by (6), yaw angle α is derived by and the proof of (10) is presented in appendix B. Note that there are two aspects in the whole model should be calibrated in order to achieve measurement: these are the unknown intrinsic parameters a a u v k k , , , , , x y 0 0 1 2 ( ) of the imaging model, and the transformation matrix R S C between Frame C and Frame S. The calibration methods for these two levels are expounded in section 4.

Calibration of the imaging system
Camera calibration is a necessary step in 3D computer vision and much work has been done [19][20][21]. But different to a traditional imaging system which maps 3D points in the world coordinate system to 2D image points, the vision system in PMTS instead maps laser beam vectors. An accurate known control field is always used to act as calibration reference: for a traditional imaging system, it is 3D point field, but for PMTS, it is space laser beam field. In order to construct incident laser beams from different positions, we introduce a twoaxis rotary table to rotate with PMTS in two dimensions in front of a stationary total station. As shown in figure 4, the rotary table has three main components: the fixed base, the external frame which rotates around the vertical-rotation axis with respect to the fixed base, and the internal frame which rotates around the horizontal-rotation axis with respect to the external frame. PMTS is mounted on the internal frame of the rotary table with the prism pointing towards the fixed total station. The reflection center of the prism should be adjusted to the rotary center of the rotary table where the vertical-rotation axis and the horizontal-rotation axis meet together in order to make sure that during rotating, the laser beam vector from the total station to the prism remains stationary.
The rotary table coordinate frame (O R X R Y R Z R -coordinate, frame R) is defined fixed on the internal frame, its origin is on the rotary center, and the X R -axis and Y R -axis are the same as the vertical-rotation and horizontal-rotation axes in the default position (figure 4). At this position the default rotation is at vertical angle ψ = 0° and horizontal angle ϕ = 0°. And at this position, take the unit vector of the laser beam from the total station in frame R as v R 0 . In a situation where the internal frame and PMTS is rotated by vertical angle ψ (i) and horizontal angle ϕ (i) (ith place), the relative attitude of frame R with respect to the fixed base and the stationary laser beam has changed, and the laser beam vector in frame R has been transformed as: where the corresponding 2D transformation matrix R Since the origin of frame R and frame C is the same, the transformation between these two coordinate systems just needs three orientations, so we assume the rotation matrix from frame R to frame C as R C R , then the laser beam vector in frame C is expressed: Equation (13) where M is a penalty factor which is set by a large value.
The rotation R C R can be parameterized by a vector of three Euler angles according to equation (5) in order for easy calculation. Minimizing (14) is a nonlinear minimization problem, which can be solved using optimization techniques such as Levenberg-Marquardt algorithm [22]. Note that reasonable initial values should be given for a global optimal solution.

Calibration of the rotation between the camera and the inclinometer
As described in section 3, the aim of calibrating the transformation relationship between coordinate systems of the camera and the inclinometer is to determine the rotation matrix R S C in the measurement model. In this paper, based on the characteristics of the 6-DOF measurement system, we present a method utilizing a prism as a reference target fixed on a rigid body as well as PMTS to calibrate the unknown relationship between these two coordinate systems.
The calibration layout and process is illustrated in figure 5. During calibration, several positions and orientations of the rigid body with respect to the stationary total station are placed. Let p S r denote the unknown coordinate of the reference target in frame S. Since the reference target and PMTS are fixed on a rigid body, the value of p S r will not vary as the rigid body moves. At ith place, the coordinate of the reference target in frame T is directly measured by the total station as p T i r( ) , and the transformation relationship between these two values is then: where R T S i ( ) and t T S i ( ) are the rotation and translation matrix relate frame S to frame T, and R T S i ( ) can be parameterized by Euler angles (γ (i) , β (i) , α (i) ) as defined in equation (5). Substituting for R T S i ( ) by the Euler angles in (15), and re-writing, results in: According to the measurement model in section 3, pitch angle β (i) and roll angle γ (i) are directly obtained from the inclinometer, and t T S i ( ) from the total station measurement. Therefore only p S r and yaw angle α (i) are unknowns in equation (16). Note that the third row of (16) does not include α (i) , which is picked as: Equation (18) can be simplified in matrix form as D p d S r ⋅ = . If n ⩾ 3, the solution is given by means of least-square: Then take the first two rows of (16), which can be rewritten as: where p p p cos sin sin sin cos 0 c os sin Equation (20) has the same form as described in appendix B. And the yaw angle α (i) is solved after substituting the calculated value p S r in (21). Therefore the transformation matrix R T S i ( ) is obtained according to the definition of equation (5). Now the objective rotation matrix R S C to be calculated is finally introduced according to (7), which is rewritten in a new form: where v C i ( ) and v T i ( ) are the same beam vector in frame C and frame T respectively at ith place. By stacking n such equations as (22) together, we have where V and U are right and left singular matrix of AB T .
The above solution is obtained by linear methods step-bystep and it is not fully optimal. We can refine it by minimizing the following function: . can be parameterized by a vector of three Euler angles according to equation (5) in order for easy calculation. This nonlinear minimization problem is solved by the Levenberg-Marquardt algorithm. The linear solution can be used as the initial iteration values in the nonlinear optimization procedure.

Simulations and experiments
The camera we use in PMTS has 1280 × 1024 pixels with each of them 5.3 µm × 5.3 µm in size, and the industrial lens has a 25 mm focal length, which determine the field of view about 15° × 12°. The inclinometer has a measurement range of ±15° in both axes, and its measurement uncertainty is 0.005°. After the camera and the inclinometer are installed in PMTS, X S -axis is nearly parallel to Z C -axis, and Y S -axis is nearly parallel to X C -axis. A Leica TS15 total station with angle measurement uncertainty of 1″, distance measurement uncertainty of 1 mm + 1.5 ppm and 3D coordinate resolution of 0.1 mm is used with PMTS for these experiments.

Measurement error analysis
According to the measurement model, the 3D translation accuracy of PMTS depends entirely on the measurement acc uracy of the cooperative total station, and the accuracy of pitch and roll angle entirely depends on the measurement accuracy of the biaxial inclinometer in PMTS. Whereas yaw angle error is due to the measurement error of both inclinometer and imaging system. Therefore, we mainly analyze the effect of these factors on the yaw angle accuracy. Since the calculation model of yaw angle is complicated, we have conducted Monte Carlo simulations to statistically evaluate the accuracy of the yaw angle.    , the yaw angle is set at α = 0°, the values of pitch and roll are set according to the demand of each simulation, and all the unknown parameters in the whole model which should be calibrated are set as ideal values. For the parameters of the imaging model, the scale factors are obtained from dividing the designed focal length by the pixel size, the principal point values are set to half of the pixel numbers, all distortion coefficients are set to zero, and these values are all listed in the second column of table 2. For rotation R S C , in order for simply expressing, we parameterize it by three Euler angles (θ x , θ y , θ z ) using the same order as equation (5)  (1) Effects of Inclinometer Error: In this simulation, the measurement uncertainty of inclinometer is 0.005°. We change the pitch and roll angles both from −15° to 15° with a 1° step, and ran 1000 simulations in each position. The uncertainty of yaw angle is illustrated in figure 6. The result has shown that within the scope of the inclinometer's range, the change of roll angle has little effect on yaw angle uncertainty, but the increasing of pitch angle enlarges it. The uncertainty of yaw angle is nearly 0° when pitch angle is 0°, and enlarges to 0.0014° when pitch angle is 15°. (2) Effects of Imaging System Accuracy: In this simulation, pitch and roll angle are both set as 0°. We add zero-mean Gaussian noise onto the image spot center, with the noise uncertainty vary from 0.1 to 1 pixel, and ran 1000 simulations in each position. It is observed that the uncertainty of yaw angle is almost proportional to the positioning uncertainty of the laser spot, and a spot uncertainty of 0.5 pixel corresponds to the yaw angle uncertainty of 0.006°, as seen in figure 7.

Camera calibration experiment
The rotary table used for calibration is manufactured with a high position precision of 3 seconds of arc for all the frames. At the beginning of the calibration, the relative position of the total station and the rotary table should be controlled to make the laser shoot on the center of the camera imaging plane roughly. Then from this beginning position, horizontal axis is rotated from −7° to 7°, and vertical axis from −6° to 6° respectively both with a 1° step, ensuring the laser spots almost visit the whole plane of the image sensor within the The standard deviations for all parameters are quite small, implying that the proposed method is quite stable. The mean RMS residual distance error of the laser spot center is 0.24 pixel, which corresponds to equivalent RMS angle error of 0.003° according to the simulation in figure 7. These results have verified the feasibility and effectiveness of the calibration approach.

Calibration of the rotation between the camera and the inclinometer
We used a square steel tube about 1.6 m long as the rigid body for calibration and verification, as shown in figure 8. A PMTS and a reference prism are fixed on the two ends of the steel tube respectively. A total of 125 different positions and orientations were conducted for calibration. We also made five sets of this calibration experiments. We also use three Euler angles (θ x , θ y , θ z ) as described in section 5.1 to express the rotation. The result values are listed in table 3, where the last two columns display the mean and standard deviation of the five sets of results. The standard deviations for all parameters are quite small, which implies that the proposed algorithm is quite stable.

Evaluation of the whole 6-DOF measurement accuracy and calibration performance
The accuracy evaluation for 6-DOF measurement is a challenging problem, partly because the measuring unit (PMTS) and the measuring base (total station) are separated, and partly because the relationships among these six parameters are orthogonal and coupled. The method utilizing a reference target and PMTS fixed on a rigid body as described in section 4.2 of section 4 is not only employed for calibration, but also for evaluating the whole 6-DOF measurement accuracy of the system. Their difference lies in that the calibration is the inverse process of the evaluation. After calibration, as the coordinate value of the reference target in frame S has been obtained, its value in total station coordinate system can be calculated by 6-DOF measuring according to equation (15). Compared with the coordinate value directly observed from the total station, the deviation distances reflect the whole accuracy of the system. This method is maybe the most reliable method firstly because the measuring base is the same total station, and then during testing, all the six parameters have joined in it and need not be decomposed.
A total of 10 different positions and orientations were conducted for evaluation. We use the measurement model with its parameters set by the mean values in tables 2 and 3 to process the experiment data. And the experiment data was also processed by the model using ideal values for comparison. We named the camera calibration as Calibration I, and the calibration of the rotation R S C as calibration II. The deviations in 10 positions and orientations of the verification experiment are plotted in figure 9, showing that the proposed method has achieved excellent performance in accuracy. By using the proposed method, the mean deviation of these 10 sets of results is 0.5 mm. As a comparison, the mean deviation processed by the model without calibration II is 10.2 mm, and without calibration I and II is 14.2 mm.
This experiment on the one hand demonstrates the necessity and validity of the calibration approach in order for an excellent measurement accuracy, on the other hand evaluate the whole 6-DOF measurement accuracy: for a reference point in the rigid body which is 1.6 m apart from the target sensor PMTS, the mean 6-DOF measurement accuracy of the system can reach to 0.5 mm.

Relative yaw angle evaluation
According to the measurement principle in section 3, a rotation of PMTS within the horizontal plane does not affect the value of pitch and roll, so the yaw angle can be individually evaluated. We introduced a horizontal multitooth dividing table with an accuracy of less than 1 s of arc to evaluate the yaw angle accuracy. The PMTS with a 6D cloud platform to change and adjust its posture is fixed on the multi-tooth dividing table as illustrated in figure 10. The multi-tooth dividing table has been leveled therefore no matter how the posture of the PMTS change, the measurement pitch and roll angle of PMTS would not change during the table rotating, and its rotation angle is the evaluation criteria of the relative measurement yaw angle of PMTS by directly comparison.
In the experiments, the distance between the PMTS and the total station is set to be about 8 m, 51 m, and 108 m respectively, and the pitch angle of PMTS is set to be about 0°, 5° and 10° in each distance. At each pitch of each distance, 14 sets of comparison data are acquired. The RMS errors of yaw angle are summarized in figure 11, showing that the increase of the pitch angle enlarges the errors of the measurement yaw angle which has verified the simulation results, and the standard deviation of all the results is up to 0.0045°. It is interesting to note that yaw angle errors decrease a little with the increase of measurement distance. It is mainly because the light spot on the image plane is more irregular in the edge when the total station near PMTS and its position locating accuracy decreases.

Conclusions
In this paper, a 6-DOF measurement system with a total station has been developed by taking advantages of total station about long range measurement, relatively high accuracy, etc. The system configuration of the 6-DOF target sensor has been described, which is mainly composed of a pinhole prism, an industrial lens, a camera and a biaxial inclinometer. The imaging property of the light path has been illustrated. The mathematical model of solving six degrees of freedom has been expounded in detail. In order to calibrate this model, this paper has also proposed approaches of camera calibration and the calibration between the coordinate systems of the sensor and the camera. Repetitive experiments of two levels have both verified the feasibilities and stabilities of these approaches, and evaluation experiment with a reference target on a rigid body has demonstrated the necessity and validity of the calibration approach in order to ensure excellent measurement accuracy. At the same time, the evaluation experiment has shown that the 6-DOF measurement accuracy of the whole system is up to 0.5 mm for a reference point which is 1.6 m apart from the target sensor. In addition, the accuracy of the yaw angle has been analyzed by Monte Carlo simulations, and evaluated in field experiments, where the measurement distance is up to 100 m. The experiments' results reveal that the RMS error of relative yaw angle is up to 0.0045°, and both the simulations and the experiments have verified that the increasing of pitch angle enlarges the measuring error of yaw angle.
Although having achieved high accuracy in static condition, the dynamic performance will be analyzed and validated in the future work. In addition, the imaging system has the ability of measuring two angles, but in this paper, we haven't studied the calculation of pitch angle from the camera information. By considering the double pitch estimation from both the camera and the inclinometer, we may increase the accuracy of the system. cos sin sin cos