# A Lightweight High Definition Mapping Method Based on Multi-Source Data Fusion Perception
## 4 Fusion Mapping on Cloud Server
### Semantic Alignment Algorithm
The position error of semantic elements caused by trajectory errors can be corrected by using the formula \(\mathbf{P}^{\prime}=\mathbf{R}\mathbf{P}+\mathbf{t}\) thus achieving alignment of semantic elements observed by multiple trajectories. To perform iterative optimization, we use the Levenberg Marquard algorithm [36]. The algorithm is implemented using the C++three-way toolkit GTSAM [37]. In the second stage, we use the pose calculated in the first stage as the initial value, and add the spatial constraints of semantic elements to construct a global Boundary Adjustment optimization. This method, called GBA_SGICP (Global Bundle Adjustment based on Semantic GICP), optimize both the relative error between multiple trajectories and the overall absolute error. This approach further improves the semantic alignment accuracy, as shown in Figure 5b. Unlike the traditional 2D-3D Boundary Adjustment [38], we use Direct Bundle Adjustment [39] to optimize the spatial position of corresponding corresponding 3D-3D semantic elements directly. For the semantic elements of line, such as lane line and stop line, the error function \(e_{l}\) is based on the distance between the point and the line, expressed as:  
\[e_{l}(x_{s},x_{t})=\sum_{k=1}^{N_{L}}\left\|\left(I-n_{l}^{k}n_{l}^{k}\right) \left(x_{s}p_{s}^{k}-x_{t}p_{t}^{k}\right)\right\|_{2}^{2} \tag{12}\]  
where \(x_{s}\) and \(x_{t}\) denotes the pose node on the trajectories \(s\) and \(t\) respectively, \(p_{s}^{k}\) and \(p_{t}^{k}\) denotes the \(k\)-th semantic element matched pair observed at the pose nodes \(x_{s}\) and \(x_{t}\); \(N_{L}\) is the set of all semantic features belonging to a point-line matching relationship; \(n_{l}^{k}\) denotes the unit normal vector of the line where the position \(p_{t}^{k}\) is located. Similar to the semantic elements of a plane, such as ground arrows and traffic signs, the loss function, \(e_{p}\), based on the distance between the point and the plane can be defined by  
\[e_{p}(x_{s},x_{t})=\sum_{k=1}^{N_{P}}\left\|n_{p}^{k}\right\|^{k}_{p}\left(x_{ s}p_{s}^{k}-x_{t}p_{t}^{k}\right)\right\|_{2}^{2} \tag{13}\]  
where \(N_{P}\) is the set of all semantic elements belonging to the point-plane matching relationship and \(n_{p}^{k}\) denotes the unit normal vector of the plane where the position \(p_{t}^{k}\) is located. Let \(\chi=\{x_{1},x_{2},\cdots,x_{n}\}\) be the set of all pose nodes of multiple trajectories, and \(x_{s},x_{t}\in\chi\). All matching pairs and initial pose values in (12) and (13) are derived from the optimization in the first step. The trajectory pose optimization equation in the second step can be obtained by using (12) and (13)  
\[\chi^{*}=\underset{\chi}{\text{argmin}}\sum_{m,n}^{R}\sum_{i\in m,j\in n} \left[\rho\left(e_{l}\left(x_{i},x_{j}\right)\right)+\rho\left(e_{l}\left(x_{ i},x_{j}\right)\right)\right] \tag{14}\]  
where \(R\) denotes the number of tracks participating in fusion optimization, \(m,n\in R,m\
eq n\) represents the \(m\)-th and \(n\)-th tracks, \(x_{i},x_{j}\) denotes the \(i\)-th pose node of the \(m\)-th pass and the \(j\)-th pose node of the \(n\)-th pass, respectively. The Huber loss function, \(\rho\), can be defined \(\rho(s)=\left\{ \begin{array}{c}\textbf{s},\ \textbf{s}\leq 1\\ \textbf{2}\sqrt{s}-\textbf{1},\ \textbf{s}>1\end{array} \right..\)  
By employing the aforementioned optimization, we can obtain more precise trajectory poses, resulting in the improved alignment of semantic elements observed by multiple trajectories, as depicted in Figure 4(c)-4(c).