# Multi-Modal Sonar Mapping of Offshore Cable Lines with an Autonomous Surface Vehicle
## 3 Consistent Mapping
### MBES Mapping  
The procedure for terrain mapping includes the raw measurement of bathymetry using sonar beams, reconstruction of 3D point clouds of underwater terrains with the vehicle's pose information, and outlier rejection.  
Bathymetry measurements using the MBES are shown in Figure 6. Given the sonar scan width \(s\) and beam spacing \(b\), we can transform the sonar ranges into sonar point clouds \(\mathbf{p}_{k,t}^{\text{M}}\) represented in a MBES-sensor-fixed frame [M] as follows:  
\[\mathbf{p}_{k,t}^{\text{M}} = \begin{bmatrix}0\\ r_{k,t}\cos\left(\frac{\pi+s}{2}-kb\right)\\ r_{k,t}\sin\left(\frac{\pi+s}{2}-kb\right)\end{bmatrix} \tag{7}\]  
where \(r_{k,t}\) is the measurement range from the \(k\)th sonar beam at time step \(t\). \(\mathbf{p}_{k,t}^{\text{M}}\) can be represented in [I] as follows:  
\[\mathbf{p}_{k,t}^{\text{I}} = R_{\text{B}}^{\text{I}}(\mathbf{q}_{t}^{\text{I}})(\mathbf{R}_{\text{M} }^{\text{B}}\mathbf{p}_{k,t}^{\text{M}}+\mathbf{t}_{\text{M}}^{\text{B}})+\mathbf{r}_{t}^ {\text{I}} \tag{8}\]  
where \(R_{\text{B}}^{\text{I}}(\cdot)\) is the rotation matrix describing the transformation from {B} to {I}, \(R_{\text{M}}^{\text{B}}\) is the matrix describing the rotation from {M} to {B}, and \(t_{\text{M}}^{\text{B}}\) is a translation vector describing the sensor position from the center of the vehicle's motion.  
The initial terrain map \(\mathcal{T}\) comprises the reconstructed 3D terrain points as follows:  
\[\mathcal{T} = \{\mathbf{p}_{k,t}^{\text{I}}|k=1,\ldots,n_{b},t=1,\ldots,t_{\text{s }}\} \tag{9}\]  
where \(n_{b}\) is the number of sonar beams in a single scan, and \(t_{\text{s}}\) is the total time step taken for sonar mapping.  
The above \(\mathcal{T}\) represents the terrain map for an ideal sonar ranging. However, in a real situation, sonar ranging suffers from multipath or scattering effects. This generally occurs from the water surface, the vehicle itself, or the rough seafloor, causing missing data and outliers. The latter must be removed from the initial sonar point clouds for consistent mapping.  
Therefore, we generated a sub-map of the sonar point cloud from the aggregated sonar scans and applied the statistical outlier removal (SOR) method [21] to the generated sub-map. A single sonar scan only contains a stripe of seafloor information; thus, it is better to aggregate the information from the consecutive scans and subsequently apply the filter to it. We also modified the traditional SOR filter into a v-SOR to generate statistics on the vertical heights, as the seafloor point clouds are 2.5D. This approach provides us with two advantages over the conventional method. First, we can preserve the sparse sonar points generated during the vehicle's heading change or abrupt roll motion. The conventional SOR filter uses Euclidean distance as a criterion for determining the outliers. However, this approach can remove inlier sonar points that are sparse in the XY plane. Our approach verifies the height differences and preserves sonar points within a statistical bound in the Z-direction. The second advantage is that we can skip the spawning of K-d trees to calculate the distance, as we directly use the height of each point to measure discrepancies, enhancing computational efficiency. A more detailed explanation of the approach can be found below in the section of experimental results.