# Navigation Map Construction Based on Semantic Segmentation and Multi-Submap Integration
## 4 Methods
### Incremental Ground Position Estimation  
First, the semantic mask of the keyframe is obtained using PanopticFCN. The regions labeled \"Road\" and \"Pavement\" in the semantic mask are selected as candidate ground areas. To ensure that the point cloud is assigned an accurate semantic information, the semantic mask is first aligned with the keyframe's scale. It is then assumed that a map point \(P\) exists in the point cloud, corresponding to a total of \(n_{k}\) features. The pixel coordinates of these \(n_{k}\) features are used to locate their positions in the semantic mask. If \(m_{k}\) of these features are located within the ground semantic region of the mask, and if \(n_{k}\geqslant 3\) and \(\frac{m_{k}}{n_{1}}>0.5\), the 3D coordinates of the map point \(P\) are calculated using the 2D pixel coordinates of the \(m_{k}\) features. The map point \(P\) is then assigned the semantic attribute of the ground. This process results in a sparse point cloud with semantic information. The map points with ground semantics are filtered out to form a ground point cloud set, denoted as \(S_{f}\), where \(S_{f}=\{\mathbf{p}_{i}\}(i=1,2,\ldots,m)\). If the number of map points in the ground point cloud does not exceed a given threshold, or if the points are too narrowly distributed, new keyframes and map points are generated. Otherwise, the initial estimate of the ground plane is made based on \(S_{f}\). The fitted plane is represented using the Hessian normal form:  
\[\mathbf{n}^{\mathsf{T}}\mathbf{p}-d=0 \tag{1}\]  
where \(\mathbf{p}\) is a 3D row vector representing the coordinates of a point on the plane, \(\mathbf{n}\) is the unit normal vector of the plane, pointing from the origin of the coordinate system  
Figure 2: Occupancy grid map construction algorithm flowchart. toward the plane, and \(d(d>0)\) is the distance from the origin to the plane. During the movement of the AGV, only a portion of the ground point cloud within the camera's field of view can be observed in the initial phase of system operation. This limited ground point cloud is insufficient for accurate plane fitting, making it difficult to construct a correct grid map. The key to achieving incremental online plane fitting is the algorithm's ability to effectively leverage results from previous computations. The complete steps are shown in Algorithm 1. Suppose that during a given observation, a certain number of new ground map points are observed. The set of these newly observed ground map points is denoted as \(S_{f}^{new}\), \(S_{f}^{new}=\left\{\mathbf{p}_{j}^{new}\right\}(j=1,2,\dots,n)\). Let the combined point set, containing both the original ground map points and the newly observed points, be denoted as \(S_{f}^{total}\), where \(S_{f}^{total}=S_{f}\cup S_{f}^{new}\). The midpoint \(\mathbf{p}_{c}^{new}\) and variance \(Var\Big{(}S_{f}^{new}\Big{)}\) of the point set \(S_{f}^{new}\) are calculated. The midpoint \(\mathbf{p}_{c}^{total}\) and variance \(Var\Big{(}S_{f}^{total}\Big{)}\) of the point set \(S_{f}^{total}\) can then be computed as follows:  
\[p_{c}^{total}=\frac{m}{m+n}  
```
1:The midpoint \(\mathbf{p}_{c}\) and variance \(Var\left(S_{f}\right)\) from the previous execution of Algorithm 1, and the newly observed ground map point set \(S_{f}^{new}\).
2:The normal vector \(\mathbf{n}\) of the fitted plane and the distance d from the plane to the origin.
3:Compute the midpoint \(\mathbf{p}_{c}^{new}\) of the point set \(S_{f}^{new}\): \[\mathbf{p}_{c}^{new}=\frac{1}{n}\sum_{j=1}^{n}\mathbf{p}_{j}^{new}\Big{(}\mathbf{p}_{j}^{ new}\in S_{f}^{n},j=1,2,\ldots,n\Big{)}\] (8)
4:Compute the midpoint \(\mathbf{p}_{c}^{total}\) and variance \(Var\left(S_{f}^{total}\right)\) of the point set \(S_{f}^{total}=S_{f}\cup S_{f}^{new}\) using Equations (2) and (7). 5:Perform Singular Value Decomposition (SVD) on the variance \(Var\left(S_{f}\right)\), and select the right singular vector corresponding to the smallest singular value as the normal vector \(\mathbf{n}\) of the fitted plane.