## 4 The 3D Point Cloud Reconstruction
### The 3D Space Point Cloud Loop Closure Correction  
Since the pose estimation of multi-sensor fusion is a recursive process, the accumulated error will cause the overall deviation of the 3D reconstruction. This section adopts loop closure detection and optimization based on the visual key frame to improve the accuracy of the mapping. In the process of 3D reconstruction, it is necessary to continuously detect whether the current frame is similar to a historical frame in order to incorporate a loop. Due to the huge number of key frames in large-scale forest scenes, it is impossible to compare the current  
Figure 3: The 3D point cloud densification model. frame with all previous key frames one by one. Therefore, this paper adopts the DBoW2 model with bags of binary words [34] to form a KD-Tree of feature points. If the current frame \(K_{cur}\) meets the consistency detection with a past frame, the past frame is regarded as a closed-loop frame \(K_{loop}\). After the closed-loop frame is detected, the accurately matched feature point pair is established between the current frame \(K_{cur}\) and the closed-loop frame \(K_{loop}\). Meanwhile, we obtain the poses of \(K_{cur}\) and \(K_{loop}\). According to the pose optimization proposed in this paper in Section 3.3, we use the reprojection error of feature point pairs and the IMU pre-integration error between \(K_{cur}\) and \(K_{loop}\) as constraints in Equation (5). The correction matrix \(T_{cur}^{loop}\) can be obtained by tightly coupled optimization. Therefore, the 3D point cloud data at the current frame \(K_{cur}\) are corrected to  
\[\left( \begin{array}{c}X_{cur(cor)}^{i(W)}\\ Y_{cur(cor)}^{i(W)}\\ Z_{cur(cor)}^{i(W)}\end{array} \right)=T_{cur}^{loop}\left( \begin{array}{c}X _{cur}^{i(W)}\\ Y_{cur}^{i(W)}\\ Z_{cur}^{i(W)}\end{array} \right) \tag{15}\]  
The above process only corrects the point cloud information of the current frame \(K_{cur}\), but the point cloud information between \(K_{loop}\) and \(K_{cur}\) still has a cumulative error. We define one frame \(K_{j}\) between \(K_{loop}\) and \(K_{cur}\), assuming that we know the pose of \(K_{j}\) through the pose estimation of visual-IMU fusion. This paper establishes the residual function of the pose error. It includes two parts. The first one is caused by the translation and yaw angle between the \(K_{j}\)th key frame and the adjacent \(K_{j+1}\)th key frame; the other one is caused by \(T_{cur}^{loop}\):  
\[r_{j}^{j+1}=\left[ \begin{array}{c}R(\hat{\phi}_{j},\hat{\theta}_{j},\psi_{j })^{-1}(t_{j+1}-t_{j})-\hat{t}_{j}^{j+1}\\ \psi_{j+1}-\psi_{j}-\hat{\psi}_{j}^{j+1}\end{array} \right] \tag{16}\]  
\[r_{cur}^{loop}=\left[ \begin{array}{c}t_{cur}-t_{loop}-t_{cur}^{loop}\\ \psi_{cur}-\psi_{loop}-\hat{\psi}_{cur}^{loop}\end{array} \right] \tag{17}\]  
In Equations (16) and (17), \(R(\hat{\phi}_{j},\hat{\theta}_{j},\psi_{j})\) represents the rotation matrix that only optimizes the yaw angle, and \(t\) represents the translation matrix; \(\phi\), \(\theta\), and \(\psi\) represent the roll angle, pitch angle, and yaw angle, respectively. Here, the superscript \(\wedge\) represents the accurate value, which does not participate in the optimization process; \(\hat{t}_{j}^{j+1}\) represents the translation vector obtained by the pose estimation of the multi-sensor fusion between the \(j\)th and \(j+1\)th frames, and \(\hat{\psi}_{j}^{j+1}\) is the yaw angle obtained from the rotation matrix in the pose estimation; and \(\hat{t}_{cur}^{loop}\) and \(\hat{\psi}_{cur}^{loop}\) are obtained by the correction matrix \(T_{cur}^{loop}\). This paper constructs the loss function of the loop closure optimization by using the translation matrix \(t\) and the yaw angle \(\psi\) as the optimization variables:  
\[\min_{i,\psi}\left\{\sum_{l,j+1\in S}\left\|r_{j}^{j+1}\right\|^{2}+\sum_{cur,loop\in L}\left\|r_{cur}^{loop}\right\|^{2}\right\} \tag{18}\]  
where \(S\) is the set of adjacent frames, which are taken as constraints, and \(L\) is the set of closed-loop frames, which are taken as constraints.