# A Lightweight High Definition Mapping Method Based on Multi-Source Data Fusion Perception
## 4 Fusion Mapping on Cloud Server
### Semantic Alignment Algorithm
Semantic GICP algorithm builds the loop-closure pose constraint between multiple dependent trajectories, that is, using \(\mathbf{C}=\left\{\dot{\mathbf{z}}_{i_{m}}^{t}\right\}\) to represent the observation value of loop-closure constraint between multiple trajectories, \(i,j\in\{\mathbf{1},\mathbf{2},\cdots,\mathbf{R}\},\ \mathbf{i}\
eq j\).where \(\mathbf{R}\) is the number of all trajectories, and \(\dot{\mathbf{c}}_{i_{m}}^{t_{n}}\) represents the loopback constraint between the m-th pose node of the \(i\)-th trajectory and the n-th pose node of the \(j\)-th trajectory. InterRPC builds constraints between different trajectories. When the relative constraint between adjacent pose nodes within each trajectory is considered at the same time, each pose node of multiple trajectories can be constructed \(\mathbf{X}=\{\mathbf{x}_{i}^{t}\}_{i=\mathbf{1},\cdots,\mathbf{N}}^{\mathbf{r}=\mathbf{1},\cdots,\mathbf{R }},\mathbf{X}_{i}^{t}\) represents the ith pose node of the r-th trajectory, \(\mathbf{\chi}=(\mathbf{R},\mathbf{t})\in\mathbf{SE}(\mathbf{3})\times\mathbb{R}^{\times\mathbf{4}}\). Let the observation value of adjacent pose constraints and prior constraints in each trajectory \(\mathbf{Z}=\left\{\mathbf{u}_{i}^{t}\right\}_{i=\mathbf{1},\cdots,\mathbf{N}}^{\mathbf{r}=\mathbf{1}, \cdots,\mathbf{R}}\cup\{\mathbf{p}_{i}^{t}\}_{i=\mathbf{1},\cdots,\mathbf{N}}^{\mathbf{r}=\mathbf{1}, \cdots,\mathbf{R}}\). The optimization problem can be described using probabilistic model as:  
\[\mathbf{P}(X|\mathbf{Z},\mathbf{C})\propto\prod\limits_{r=1}^{\mathbf{R}}\left(\prod\limits_{ i=1}^{\mathbf{N}}\mathbf{P}(\mathbf{X}_{i}^{t}|\mathbf{p}_{i}^{t})\prod\limits_{i=1}^{\mathbf{N}}\mathbf{P}( \mathbf{X}_{i+1}^{r}|\mathbf{\chi}_{i}^{r},\mathbf{u}_{i}^{t})\right)\prod\limits_{ij}^{ \mathbf{R}}\mathbf{P}\Big{(}\mathbf{\chi}_{i}^{m}|\mathbf{\chi}_{j}^{n},\dot{c}_{i_{m}}^{t_{n}} \Big{)} \tag{8}\]  
We assume that the measurement model follows the Gaussian distribution, and the relative pose constraint probability model in the trajectory is given by  
\[\mathbf{P}(\mathbf{\chi}_{i+1}^{r}|\mathbf{\chi}_{i}^{r})=\mathbf{N}(f_{i}(\mathbf{\chi}_{i}^{r}, \mathbf{u}_{i}^{t}),\mathbf{\Lambda}_{i}^{r}) \tag{9}\]  
The relative pose constraint probability model between different trajectories is given by  
\[\mathbf{P}(\mathbf{\chi}_{i}^{m}|\mathbf{\chi}_{j}^{n})=\mathbf{N}\Big{(}h_{i}\Big{(}\mathbf{\chi} _{j}^{n},\dot{c}_{i_{m}}^{t_{n}}\Big{)},\mathbf{\Gamma}_{i_{m}}^{t_{n}}\Big{)} \tag{10}\]  
\(\mathbf{X}^{*}\) under the maximum a posteriori probability is the optimal solution we obtained. You can take the negative logarithm of Formula (8) and combine Formula (9) and (10) to convert it into a nonlinear least squares problem  
\[ \begin{split}\mathbf{X}^{*}=\underset{\mathbf{X}}{\text{argmax}}\mathbf{P}( \mathbf{X}|\mathbf{Z},\mathbf{C})&=\underset{\mathbf{X}}{\text{argmin}}-\text{ log}\mathbf{P}(\mathbf{X}|\mathbf{Z},\mathbf{C})\\ &=\underset{\mathbf{X}}{\text{argmin}}\Bigg{\{}\sum\limits_{\mathbf{r}=0} ^{R}\left(\sum\limits_{i=1}^{\mathbf{N}}\|\mathbf{p}_{i}^{r}-\mathbf{\chi}_{i}^{r}\|_{\mathbf{ \Sigma}}^{2}+\sum\limits_{i=1}^{\mathbf{N}}\|\mathbf{f}_{i}(\mathbf{\chi}_{i}^{r},\mathbf{u}_{ i}^{t})-\mathbf{\chi}_{i+1}^{r}\|_{\mathbf{\Lambda}_{i}^{r}}^{2}\right)+\sum\limits_{ij}^{M} \rho\left(\|\mathbf{h}_{i}\Big{(}\mathbf{\chi}_{j}^{n},\dot{c}_{i_{m}}^{t_{n}}\Big{)}- \mathbf{\chi}_{i}^{m}\|_{\mathbf{\Gamma}_{i_{m}}^{t_{n}}}^{2}\right)\Bigg{\}}\end{split}  \tag{11}\]  
Considering the robustness of the algorithm, we add a filter function \(\rho\) (.) of the pose edge in loop-closure pose in the Formula (11) Pairwise Consistency Measurement (PCM) [30] based on maximum clique is used to filter out abnormal or low precision loopback pose edges. The optimized pose can be obtained The optimized pose \(\mathbf{\chi}=(\mathbf{R},\mathbf{t})\), by solving the aforementioned nonlinear least square problem.