# Navigation Map Construction Based on Semantic Segmentation and Multi-Submap Integration
## 4 Methods
### Multi-Submap Fusion Mapping Method for Outdoor Environment
#### 4.2.1 Map Update Strategy for a Single Submap
This value can then be substituted into the fitted ground plane equation to obtain the following:  
\[\left(n_{wx}\quad n_{wy}\quad n_{wz}\right) \begin{pmatrix}X_{SubMap}\\ Y_{SubMap}\\ Z_{SubMap}\end{pmatrix} -d_{w}=0 \tag{16}\]  
where \(\left(n_{wx},n_{wy},n_{wz}\right)\) is equivalent to \(\mathbf{n}_{w}^{\text{T}}\). By rearranging the equation, \(Y_{SubMap}\) can be computed, which allows for the determination of the world coordinates of the submap center as follows:  
\[Y_{SubMap}=\frac{d_{w}-n_{wx}X_{SubMap}-n_{wz}Z_{SubMap}}{n_{wy}} \tag{17}\]  
Let the center coordinates of the submap, denoted as \(\left(X_{SubMap},Y_{SubMap},Z_{SubMap}\right)^{\text{T}}\), be represented by \(\mathbf{p}_{submap}\), and the coordinates of the submap center in the CGCS2000 coordinate system, denoted as \(\mathbf{p}_{CGCS}\), where \(\mathbf{p}_{CGCS}=(X_{CGCS},Y_{CGCS},Z_{CGCS})^{\text{T}}\). Let the initial orientation of the vehicle relative to the longitudinal axis of the CGCS2000 coordinate system be denoted as \(\theta\). The following equation holds:  
\[ \begin{bmatrix}X_{CGCS}\\ Y_{CGCS}\\ 1\end{bmatrix} = \begin{bmatrix}\cos\theta&-\sin\theta&x_{0}\\ \sin\theta&\cos\theta&y_{0}\\ 0&0&1\end{bmatrix}\begin{bmatrix}X_{SubMap}\\ Y_{SubMap}\\ 1\end{bmatrix}  \tag{18}\]  
By using Equation (18), we align the map center coordinates between the relative coordinate system and the CGCS2000 coordinate system, thereby determining the position of each grid cell in the CGCS2000 coordinate system. On the xOz-plane of the submap coordinate system, we sample a series of points at 0.1 m intervals across the entire plane. The resulting array of points represents the centers of each grid cell in the submap. The coordinates of these points in the submap coordinate system, denoted as \(\mathbf{p}_{ij}\), can be expressed as:  
\[\mathbf{p}_{ij}=(0.1i,0,0.1j)^{\text{T}} \tag{19}\]  
where the variables \(i\) and \(j\) are both integers ranging from \(-50\) to \(50\). When the AGV operates within the current submap's range and new keyframes with their corresponding semantic masks are provided as input observations, we do not update all the grid cells of the submap simultaneously. To ensure precise updates of the grid cells' occupancy status, we adopt a strategy of multiple small-scale updates. If the Chebyshev distance between the center of a grid cell and the keyframe in the xOz-plane of the submap coordinate system is less than 3 m, the grid cell is projected onto the semantic mask via the transformation matrix between the submap and camera coordinate systems. The specific projection process is illustrated in Figure 3.  
First, the center of the grid cell is transformed from the submap coordinate system into the camera coordinate system. Using the camera's intrinsic parameters, the pixel coordinates of the grid cell's center are computed. The corresponding position on the semantic mask is then identified based on these pixel coordinates, thereby projecting the grid cell center onto the semantic mask. If the projected point falls within the mask region labeled as a road, the probability of the grid cell being classified as a \"road cell\" increases; otherwise, it decreases. As more keyframes are input, this probability is updated accordingly. Once the probability exceeds a predefined threshold, the grid cell is classified as a road cell. The detailed steps of the entire map update process are outlined in Algorithm 2. ```
1:KeyFrame \(pKF\), Map Transform \(Tum\), Cell Length \(sfCellLength\), Cell Radius \(CellNearKFRadius\), Map Size \((snWidth,snHeight)\). 2:Updated Submap Cells \(mvCells\). 3:Obtain the current camera pose \(\mathbf{T}_{cw}\) from the reference keyframe. 4:Calculate the transformation matrix \(\mathbf{T}_{cm}=\mathbf{T}_{cw}\cdot\mathbf{T}_{wm}\) from the submap to the reference keyframe camera coordinate system. 5:Invert the transformation matrix \(\mathbf{T}_{cm}\) to obtain the transformation matrix \(\mathbf{T}_{mc}\) from the camera coordinate system to the submap coordinate system.