## 3 Experimental Test Procedure
### RGB+D Fusion
#### 3.3.2 Image Data Processing  
The described process in this subchapter is unique to the use of Trimble MS instrument. The process is repeatable using any other MS from other manufacturers, but it would involve more steps and calculations. The camera is modeled as a pinhole camera with known intrinsic parameters from the manufacturer represented in a calibration matrix \(K\):  
\[K= \begin{bmatrix}f&&x_{0}\\ &f&y_{0}\\ &&1\end{bmatrix} ,\]  
where \(f\) denotes the focal length and \((x_{0},y_{0})\) the image coordinates of the principal point. The coordinates of the camera center in the used coordinate system were calculated based on the telescope theodolite angles and the nominal offsets of the camera center from the instrument center. Nominal offsets values were known from the mechanical design blueprint of the instrument data, provided by the manufacturer. The mathematical model that defined the coordinates of the camera center based on the telescope theodolite angles was created. The camera center circled around the instrument center on a sphere, as depicted in Figure 6. Angle differences between the collimation axis and vertical axis and the 3D distance were constants. For the given telescope theodolite angles (\(Hz\) and \(V\)), camera center theodolite angles in the used coordinate system are defined as:  
\[ \begin{array}{c}Hz_{c}=Hz+Hz_{diff},\\ V_{c}=V+V_{diff}.\end{array}  \tag{1}\]  
Coordinates of the camera center are defined as:  
\[ \begin{array}{c}Cam_{cow}=INS_{cow}+\begin{bmatrix}\Delta_{x}\\ \Delta_{y}\\ \Delta_{h}\end{bmatrix}\ \rightarrow\ \begin{bmatrix}x_{cc}\\ y_{cc}\\ h_{cc}\end{bmatrix}=\ \begin{bmatrix}x_{INS}\\ y_{INS}\\ h_{INS}\end{bmatrix}+\ \begin{bmatrix}d_{3D}*\sin V*\sin Hz\\ d_{3D}*\sin V*\cos Hz\\ d_{3D}*\cos V\end{bmatrix},\end{array}  \tag{2}\]  
where \(INS_{cow}\) are the coordinates of the instrument center from the instrument setup. Exterior orientation parameters of the camera are calculated based on a Bivector that defines the relation between the camera and telescope axis. Bivectors for each image are stored in the MS job file. Bivector from the camera to the telescope defines the 3D rotation of the camera around the theodolite axis as a combination of the general main rotation, collimation error, temperature dependent part, and any user calibration [32]. The use ofBivectors replaces the standardly used transformation from 3D world coordinates into image pixel positions and backward transformation using rotation matrices and translation vectors. This step calculates the transformation parameters from the telescope coordinate system to the camera coordinate system. The exact formulas and algorithms used for calculating the extrinsic camera parameters cannot be shown in this paper, as it is Trimble Geospatial proprietary code, and the authors were instructed not to distribute it further. The use of self-calibrated data from Bivector simplifies the calculation process and mathematical models, from the manual version, where users would need to calculate everything themselves. This step calculates the theodolite angles (\(Hz\) and \(V\)) of the camera principal point in the used coordinate system. Within the Trimble MS job file, calibrated values of angle difference per pixel are stored for each image. Theodolite angles for each pixel can be defined in a simplified way as:  
\[ \begin{array}{l}Hz_{pix}=Hz+(x-x_{0})*Pix_{angle},\\ V_{pix}=V+(y-y_{0})*Pix_{angle},\end{array}  \tag{3}\]  
where \((x_{0},y_{0})\) are the image coordinates of the principal point, \((x,y)\) are the image coordinates of the pixel and \(Pix_{angle}\) is calibrated value for pixel angle difference. Approximately \(Pix_{angle}\) equals to 18.3\({}^{\prime\prime}\) for the primary camera of SX10. Because the image features are roughly on the same part in each image, effect of any residual camera calibration errors are negligible. All image data is stored within a 5-dimensional matrix that have the same dimension as the base image.