# Fast 3D Semantic Mapping in Road Scenes +
## 4 3D Semantic Mapping
### Incremental Fusion
The corresponding point \(\mathbf{\hat{p}}\) in the image \(I_{j}\) of new keyframe is located by  
\[\mathbf{\hat{p}}=\mathbf{K}\mathbf{T}^{i}_{W}\mathbf{T}^{j}_{i}\mathbf{\hat{ P}}_{W}\in I_{j}. \tag{7}\]  
Here, since the estimation of the inverse depth map is normally distributed, we have a one-to-many transform between keyframes, which involves a couple of estimated 2D/3D points, regarded as \(\mathbf{p}\in I_{i}\rightarrow\mathbf{\hat{P}}_{W}\rightarrow\mathbf{\hat{p} }\subset I_{j}\), as shown in Figure 3.  
The class label corresponding to a couple of 3D points \(\hat{\mathbf{P}}_{W}\) in the world reference is denoted as \(X:\hat{\mathbf{P}}_{W}\to l\in\mathcal{L}\). Note that the label \(Sky\) is removed from \(\mathcal{L}\) for the 3D semantic mapping. Our target is to obtain the independent probability distribution of each 3D point over the class labels \(P(X|\mathcal{K}_{0}^{i})\) given a sequence of existing keyframes \(\mathcal{K}_{0}^{i}=\{\mathcal{K}_{0},\mathcal{K}_{1},\ldots,\mathcal{K}_{i}\}\) in the pose-graph \(\mathcal{G}\). We explore a recursive Bayesian fusion to refine the corresponding probability distribution of 3D points with new keyframe's update:  
\[P(X|\mathcal{K}_{0}^{i})=\frac{1}{Z_{i}}P(\mathcal{K}_{i}|\mathcal{K}_{0}^{i-1 },X)P(X|\mathcal{K}_{0}^{i-1}), \tag{8}\]  
with \(Z_{i}=P(\mathcal{K}_{i}|\mathcal{K}_{0}^{i-1})\). Applying the first-order Markov assumption to \(p(\mathcal{K}_{i}|\mathcal{K}_{0}^{i-1},X)\), then we have:  
\[P(X|\mathcal{K}_{0}^{i})=\frac{1}{Z_{i}}P(\mathcal{K}_{i}|X)P(X|\mathcal{K}_{ 0}^{i-1})=\frac{1}{Z_{i}}\frac{p(\mathcal{K}_{i})P(X|\mathcal{K}_{i})}{P(X)}P (X|\mathcal{K}_{0}^{i-1}). \tag{9}\]  
We assume that \(P(X)\) does not change over time and there is no need to calculate the normalization factor \(P(\mathcal{K}_{i})/Z_{i}\) explicitly. According to the formulations above, the semantic probability distribution of all given keyframes can be recursively updated as follows:  
\[P(X|\mathcal{K}_{0}^{i})\propto P(X|\mathcal{K}_{i})P(X|\mathcal{K}_{0}^{i-1}), \tag{10}\]  
where a couple of 2D pixels matching between \(\mathcal{K}_{0}^{i}\) can be calculated with the Equations (4) and (5). The semantic map in \(\mathcal{K}_{0}^{i}\) contributes to the accumulated probabilistic estimation of object class. For example, given a pixel \(\mathbf{p}\) in the image \(I_{i}\) of the keyframe \(\mathcal{K}_{i}\), its corresponding scores (probabilities) of object classes are \(S_{i}(\mathbf{p})=\{P(Road|\mathbf{p})=p_{1},P(\textit{Sidewalk}|\mathbf{p}) =p_{2},P(\textit{Building}|\mathbf{p})=p_{3},\ldots,P(\textit{Bicycle}| \mathbf{p})=p_{19}\}\) with \(\sum p_{i}=1\). Then, at each fusion step, the predicted labels of 3D point \(\hat{\mathbf{P}}_{W}\) is the label with maximum probabilities as  
\[\max_{k=1}^{N}S_{j}(\hat{\mathbf{p}}_{k})S_{i}(\mathbf{p}), \tag{11}\]  
where there are \(N\) possible projected 3D points and pixels \(\hat{\mathbf{p}}\) in the image \(I_{j}\) of the keyframe \(\mathcal{K}_{j}\). The incremental fusion can refine the semantic label of the points in the 3D space based on the pose-graph of keyframes. It could handle the inconsistent 2D semantic labels, even though its performance relies on the depth estimation. In addition, map geometry is another useful feature which could improve the performance of the 3D semantic mapping further. The following section describes how we use the dense CRF to regularize the 3D semantic map by exploring the map geometry, which could propagate semantic information between spatial neighbors.