## 3 Materials and Methods
### Segment-Based Sparse Map Optimization Using Double Window Approach  
In previous sections, though locally optimized poses were obtained, the map did not achieve the global optimal. Therefore, to achieve good real-time performance, as well as a globally optimal and consistent map, we adopted a multithreaded segment-based map double window optimization strategy that considered the first and last keyframes of the current map segment corrected by the ICP algorithm with LiDAR data as the true values and then applied the double window approach to optimize this map segment. In multithread segment-based map optimization, as shown in Figure 10, the visual tracking thread operates on current map segment whereas the LiDAR data optimization thread operates on the previous map segment. When the visual tracking thread starts tracking the next map segment from \(t_{1}\) to \(t_{2}\), the LiDAR data optimization thread simultaneously completes optimizing the previous map segment and starts optimizing the next segment; therefore, real-time optimization and tracking performance are ensured. Figure 10: Tracking threads and LiDAR data optimization threads operate on different segments of the map; small dots in red represent keyframes currently activated by the corresponding thread. (**a**) keyframes currently activated by the corresponding thread in \(t_{1}\), (**b**) keyframes currently activated by the corresponding thread in \(t_{2}\). In the double window approach, as shown in Figure 11, the optimization window is divided into a front window and a back window. The front window constraint ensures the accuracy of the new map and the back window constraint ensures consistency between front and rear map segment. The front window comprises 40 keyframes in the current segment and uses the pose-point constraint to control the precise construction of this local map segment. The back window consists of an optimized set of keyframes that observe the map points in the front window. The back window constraints include the pose-pose constraints between two keyframes with common visible map points in the back and front window, as well as the pose-point constraints between the keyframes in the back window and the map points in the front window. These two constraints help achieve global consistency of the map. To improve the optimization efficiency and memory consumption, the map points that were optimized multiple times and the keyframes in the back window that are far from the front window are removed from the current optimization. This research considers the map points that have been optimized more than 6 times to be sufficiently accurate, and removes them from current optimization; therefore, a set of front window map point optimization variable \(V_{p-front}\) is obtained. The set of front window keyframe optimization variable is then \(V_{kf-front}\). Keyframes with distance more than 4 m were removed. The set of the back window keyframe optimization variable is \(V_{kf-back}\). Error Equation Formation  
We constructed the total error equation in the current segment, which is given by  
\[ \begin{array}{ll}E=E_{1}+E_{2}+E_{3}&=\sum\limits_{i=1}^{m}E(kf_{i})+\sum \limits_{k=1}^{a}E(kf_{k})+\sum\limits_{k=1}^{a}\sum\limits_{i=1}^{m}E(T_{ki} )\\ &=\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|z_{ij}-h(\xi_{i},p_{ij})\|^{2}+ \sum\limits_{k=1}^{a}\sum\limits_{j=1}^{n}\|z_{kj}-h(\xi_{k},p_{kj})\|^{2}\\ &+\sum\limits_{k=1}^{m}\sum\limits_{i=1}^{m}{v_{ki}}^{T}\Lambda_{Ti}v_{ki}\\ \\ &p_{ij}\in V_{p-front}\\ &p_{kj}\in V_{p-front}\\ &kf_{k}\in V_{kf-back}\\ &kf_{i}\in V_{kf-front}\end{array}  \tag{4}\]  
where, \(E(kf_{i})\) is the error equation of pose-point constraint in the front window; \(E(kf_{k})\), \(E(T_{ki})\) are the error equations of pose-map point constraint and pose-pose constraint in back window, respectively. Figure 11: Front window and back window. The Jacobian matrix (\(J\)), Hessian matrix (\(H\)), and gradient vector (\(g\)) can be obtained as follows  
\[\left\{