# Digital-Twin-Based System for Foam Cleaning Robots in Spent Fuel Pools
## 3 Implementation of the Digital Twin System for the Foam Cleaning Robot in Spent Fuel Pools
### Construction of the Working Scenario for the Cleaning Robot
#### 3.3.2 Sensor Data Fusion  
The information obtained by the depth camera and the two-dimensional laser rangefinder differed in their representation. The mapping relationship between the point cloud and image data was obtained using the rotational and translational transformation relationship. This enabled data transformation into the same coordinate system for further processing. Figure 10 shows the coordinate relationship between laser radar and depth camera. In Figure 10, the detected object has coordinates \(m_{D}(x_{d},y_{d},z_{d})\) in the depth camera coordinate system, \(m_{L}(x_{l},y_{l},z_{l})\) in the laser rangefinder coordinate system, and projection coordinates \(m^{\prime}(u,v)\) in the image coordinate system. For the depth camera, the collected information of point \(m\) includes the projection coordinates \(m^{\prime}(u,v)\) and the corresponding depth information \(z_{c}\). The laser rangefinder scanned and obtained the distance \(\rho\) and angle \(\alpha\) between the radar and the object. Here, \(\alpha\) represents the angle between the detected object and the \(O_{L}Z_{L}\) axis. Based on the geometric relationship between these two coordinate systems, the transformation relationship of the detected object between the two coordinate systems could be derived as follows:  
\[ \begin{bmatrix}x_{c}\\ y_{c}\\ z_{c}\end{bmatrix} = \begin{bmatrix}R&T\\ 0^{T}&1\end{bmatrix}\begin{bmatrix}x_{l}\\ y_{l}\\ z_{l}\end{bmatrix}  \tag{10}\]  
In Equation (10), \(R\) represents the rotation matrix, and \(T\) represents the translation matrix. \(m^{\prime}\) is the projection of the detected object on the image plane. In the depth camera coordinate system, the relationship between the depth information and projection coordinates is  
\[z_{c} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} = \begin{bmatrix}f_{x}&0&q_{x}&0\\ 0&f_{y}&q_{y}&0\\ 0&0&1&0\end{bmatrix}\begin{bmatrix}x_{c}\\ y_{c}\\ z_{c}\end{bmatrix}  \tag{11}\]  
In Equation (11), \(f_{x}\) and \(f_{y}\) represent the focal lengths on the \(X\) and \(Y\) axes, while \(q_{x}\) and \(q_{y}\) represent the baseline points on the \(X\) and \(Y\) axes. \(f_{x}\), \(f_{y}\), \(q_{x}\), and \(q_{y}\) are intrinsic parameters of the camera. According to the above formula, the data of the detected object collected by the depth camera could be transformed into the laser rangefinder coordinate system. The transformation relationship between them is  
\[z_{c} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} = \begin{bmatrix}f_{x}&0&q_{x}&0\\ 0&f_{y}&q_{y}&0\\ 0&0&1&0\end{bmatrix}\begin{bmatrix}R&T\\ 0&1\end{bmatrix}\begin{bmatrix}x_{l}\\ y_{l}\\ z_{l}\end{bmatrix}  \tag{12}\]  
During the scanning process, the laser rangefinder adopted a polar coordinate expression. The relationship between the information collected by the laser rangefinder about the detected object in its coordinate system is  
\[ \begin{cases}x_{l}=\rho\sin\alpha\\ z_{l}=\rho\cos\alpha\end{cases}  \tag{13}\]  
Figure 9: Point cloud data from depth camera. It was ensured that the center points of the depth camera and laser rangefinder lay on the same axis, with the height difference between them denoted as \(\delta\). From the above formulas, the transformation relationship between the depth camera information and laser rangefinder information could be derived as:  
\[z_{c} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} = \begin{bmatrix}f_{x}&0&q_{x}&0\\ 0&f_{y}&q_{y}&0\\ 0&0&1&0\end{bmatrix}\begin{bmatrix}R&T\\ 0&1\end{bmatrix}\begin{bmatrix}\rho\sin\alpha\\ h\\ \rho\cos\alpha\end{bmatrix}  \tag{14}\]  
By substituting the measured depth camera data and laser rangefinder data into the Equation (14), the rotation matrix and translation matrix in the system of linear equations could be determined. This completes the calibration to transform depth camera data into pseudo-laser rangefinder data. After the joint calibration of the sensors, the point clouds from both sensors were fused in the laser rangefinder coordinate system.