# 3D Point Cloud Generation Based on Multi-Sensor Fusion
## 3 A Method of 3D Point Cloud Generation by Multi-Sensor Fusion
### Trajectory Correction Algorithm  
During the trajectory correction of the scanning trolley, first the coordinates of the track center point of the same section were calculated according to the target point cloud coordinates and IMU attitude angle in RC. The IMU attitude angle and odometer speed were used to calculate the track center coordinates of the next point from the previous point and compare the calculated coordinates of the target control point to obtain the estimated error as an observation update to correct the odometer speed and IMU attitude angle. Since the calculated track center point changed after correcting the attitude angle, the track center point was recalculated using the corrected attitude angle and compared with the original target to determine whether the error exceeds the threshold. If the error exceeded the threshold, a secondary filtering was performed to correct it, but it did not exceed the calculated trajectory. Figure 13 shows the trajectory correction process. Figure 12: Schematic diagram of inclination calibration. (1) Track center point calculation  
After extracting the target point cloud, according to the IMU attitude angle and car body parameters, we calculated the displacement vector (\(\Delta\)X, \(\Delta\)Y and \(\Delta\)Z) of each target and the center point of the track after the conversion. The body correction in RS was:  
\[dh=H_{car}cos\gamma+L_{car}sin\gamma dl=H_{car}sin\gamma-L_{car}cos\gamma \tag{17}\]  
where \(H_{car}\) is the vertical translation of the scanner center relative to the car body center, and \(L_{car}\) is the lateral translation of the scanner center relative to the car body center, and \(\gamma\) is the roll angle at this moment. The point cloud coordinates in the car body coordinate system were:  
\[ \begin{bmatrix}x_{1}\\ y_{1}\\ z_{1}\end{bmatrix} = \begin{bmatrix}x_{0}\\ y_{0}+dl\\ z_{0}+dh\end{bmatrix}  \tag{18}\]  
According to time alignment, we calculated the rotation parameters of each scanning section according to the IMU attitude angle and then obtained the displacement vector of the target relative to the track center at the same section, as shown in Figure 14:  
\[ \begin{bmatrix}x_{c_{t}}\\ y_{c_{t}}\\ z_{c_{t}}\end{bmatrix} =R_{X}R_{Y}R_{Z} \begin{bmatrix}x_{1}\\ y_{1}\\ z_{1}\end{bmatrix}  \tag{19}\]  
where \(R_{X},R_{Y},R_{Z}\) are calculated by the following equations:  
\[R_{X}= \begin{bmatrix}1&0&0\\ 0&cos\varphi&sin\varphi\\ 0&-sin\varphi&cos\varphi\end{bmatrix} R_{Y}= \begin{bmatrix}cos\theta&0&-sin \theta\\ 0&1&0\\ sin\theta&0&cos\theta\end{bmatrix} R_{Z}= \begin{bmatrix}cos\gamma&sin\gamma&0\\ -sin\gamma&cos\gamma&0\\ 0&0&1\end{bmatrix}  \tag{20}\]  
where \(\varphi,\theta,\gamma\) represent three Euler angles, which are heading, pitch and roll angles. Figure 13: Trajectory correction. According to the position of the control point, the track center point of the section \(sta_{abs}\) could be inversely calculated. This was used as a control point to compare the error with the trajectory calculated from the original acquisition data of the IMU and odometer. \[sta_{abs}= \begin{bmatrix}x_{tag}\\ y_{tag}\\ z_{tag}\end{bmatrix} - \begin{bmatrix}x_{c_{t}}\\ y_{c_{t}}\\ z_{c_{t}}\end{bmatrix}  \tag{21}\]  
where \(\left(x_{tag},y_{tag},z_{tag}\right)\) is the coordinate of the target center measured by the total station. 2.