# Fast 3D Semantic Mapping in Road Scenes +
## 4 3D Semantic Mapping
### Incremental Fusion  
There might be a large amount of inconsistent 2D semantic labels between consecutive frames, due to the noise of sensors, the complexity of environments in the real world and the failure of scene parsing model. Incremental fusion of semantic label from the stacked keyframes allows associating probabilistic label in a Bayesian way, when combining with the inverse depth map propagation between keyframes in the LSD-SLAM. We give the details about the incremental semantic fusion as follows. The camera projection transformation function \(\pi(\cdot):\mathbb{R}^{3}\rightarrow\mathbb{R}^{2}\) is defined as  
\[\mathbf{p}=\pi(\mathbf{P})=[\alpha\frac{x}{z}+c_{x},\beta\frac{y}{z}+c_{y}]^{T}, \tag{2}\]  
which maps a point \(\mathbf{P}=[x,y,z]^{T}\) in 3D space into a 2D point \(\mathbf{p}=[x^{\prime},y^{\prime}]^{T}\) on the digital image plane \(I_{i}\) in the camera coordinate system. Since this projection function is nonlinear, for the computation efficiency, the transformation should be augmented into the homogeneous coordinate system, which is defined as  
\[\mathbf{p}_{h}=\left[ \begin{array}{c}x^{\prime}_{h}\\ y^{\prime}_{h}\\ z^{\prime}_{h}\end{array} \right]=\left[ \begin{array}{ccc}\alpha&0&c_{x}&0 \\ 0&\beta&c_{y}&0\\ 0&0&1&0\end{array} \right]\left[ \begin{array}{c}x\\ y\\ z\\ 1\end{array} \right]=\mathbf{K}[\mathbf{I}\ 0]\mathbf{P}_{h}, \tag{3}\]  
where \(\mathbf{K}\) is referred to as the camera matrix. Given a 3D point \(\mathbf{P}_{W}\) in the world reference system, the mapping to image plane \(I_{i}\) in the homogeneous reference system is calculated as  
\[\mathbf{p}_{h}=\mathbf{K}\mathbf{T}^{i}_{W}\mathbf{P}_{Wh}, \tag{4}\]  
where \(\mathbf{T}^{i}_{W}\) is the pose of the camera in the world reference system. Then, we get Euclidean coordinates \(\mathbf{p}=[x^{\prime}_{h}/z^{\prime}_{h},y^{\prime}_{h}/z^{\prime}_{h}]^{T}\) from the homogeneous coordinates. From this point on, any point \(\mathbf{p}\) and \(\mathbf{P}\) is assumed to be in homogeneous coordinates and thus we drop the \(h\) index, unless stated otherwise. Correspondingly, given the inverse depth estimation \(\hat{d}\) for a pixel \(\mathbf{p}=[x^{\prime},y^{\prime}]^{T}\) in the image \(I_{i}\) of the keyframe \(\mathcal{K}_{i}\), we also have an inverse projection function from 2D pixel point into the 3D point in the current camera coordinate system as:  
\[\mathbf{P}=\pi^{-1}(\mathbf{p},\hat{d})=[\frac{x^{\prime}/\hat{d}-c_{x}/\hat {d}}{\alpha},\frac{y^{\prime}/\hat{d}-c_{y}/\hat{d}}{\beta},\frac{1}{\hat{d}} ]^{T}, \tag{5}\]  
where \(\hat{d}=D_{i}(\mathbf{p})\sim\mathcal{N}(\mu,V_{i}(\mathbf{p}))\) corresponds to the inverse depth of the point \(\mathbf{p}\), which is normally distributed. The inverse depth estimation of each existing keyframe is continuously refined using its following frames until new keyframe is selected. In reference to Equations (4) and (5), we can derive the normally distributed 3D points in the world reference system as follows:  
\[\mathbf{\hat{P}}_{W}={T^{i}_{W}}^{-1}\pi^{-1}(\mathbf{p},D_{i}(\mathbf{p}),V _{i}(\mathbf{p})), \tag{6}\]  
where the homogeneous transformation matrix has the property: \({T^{i}_{W}}^{-1}=T^{W}_{i}\). Once a new frame is chosen to become a keyframe \(\mathcal{K}_{j}\), its inverse depth map \(D_{j}\) is initialized by projecting points from previous keyframe into it. The information of existing, close-by keyframes is propagated to new keyframe for its initialization and semantic probabilistic refinement.