## 3 Materials and Methods
### Accurate Camera Pose Determination Using ICP with LIDAR and RGB-D Point Cloud Data
#### 3.3.2 Accurate Pose Determination  
Owing to complexity of the indoor scene, obtaining a precise camera pose from the LiDAR data is challenging. Generally, an indoor environment satisfies the Manhattan world hypothesis that is mainly composed of cubical objects, which have abundant geometric structured information in LiDAR data. Therefore, the point-to-plane ICP iterative algorithm initialized by the pose of the visual tracking thread achieves fast and robust convergence to the global optimal, and can obtain precise camera pose control information, although the point-to-plane ICP iterative algorithm is not innovative. Equation (1) illustrates the point-to-plane ICP distance error function:  
\[\begin{array}{ll}&p_{i}\in P,\ q_{i}\in Q\\ \mathrm{E}(\alpha,\beta,\gamma,t_{x},t_{y},t_{z})^{k}&=\sum\limits_{i}^{|D|} \sum\limits_{j}^{|M|}w_{ij}d_{s}^{2}(T^{k}p_{i},S_{j}^{k})\\ &=\sum\limits_{i}^{|D|}\sum\limits_{j}^{|M|}w_{ij}(\frac{\left[\left[p_{i}(x )\cdot t_{x}+p_{i}(y)\cdot B_{j}+p_{i}(z)\cdot C_{j}+D\right]\right]}{\sqrt{A_{ j}{}^{2}+B_{j}^{2}+C_{j}^{2}}})\end{array} \tag{1}\]  
where,  
\(P\) is the RGB-D point cloud data generated by the corrected keyframe (Figure 9); \(Q\) is the local LiDAR point cloud data; \(A_{j},B_{j},C_{j},\) and \(D_{j}\) are the plane parameters of the tangent plane \(S_{j}^{k}\); \(w_{ij}\) is the weight of the distance from the point \(p_{i}\) to the plane \(S_{j}^{k}\) (usually set to one); \(\alpha,\beta,\ \text{and}\ \gamma\) are  
Figure 8: Dynamic object culling based on optical flow method. (**a**) Original RGB image, (**b**) dynamic object culling in RGB image, (**c**) corresponding point clouds generated by original depth data, and (**d**) dynamic object culling in depth data.  
rotation angles; and \(t_{x},t_{y},\text{ and }t_{z}\) are translation parameters. The registration algorithm finds a final transformation \(T\) which minimizes the above-mentioned error function \(E(\alpha,\beta,\gamma,t_{x},t_{y},t_{z})^{k}\) by iteratively using the least squares method; the iterative termination condition is that the number of iterations is greater than the maximum number of iterations, or that the registration error defined in Equation (2) is less than a given threshold.  
\[\sigma=\frac{\|E(\alpha,\beta,\gamma,t_{x},t_{y},t_{z})^{k}-E(\alpha,\beta, \gamma,t_{x},t_{y},t_{z})^{k-1}\|}{N}\leq\epsilon_{e}\left(\epsilon_{e}>0\right) \tag{2}\]  
where \(\epsilon_{e}\) is the error threshold and \(N\) is the number of used points \(p_{i}\).  
Using the above-mentioned ICP algorithm, the camera pose is corrected and the accurate pose is obtained. However, because the effect of single LiDAR corrected keyframe camera pose in the map is small, the accurate pose needs to be propagated to the local map using keyframe covisibility graph to expand the control range of the accurate pose. The pose transfer to the local map is described in Equation (3).  
\[\left\{\begin{array}{c}K_{f}=\left\{k_{f}^{i}\lor k_{f}^{i}\cap k_{f}!=0,k_{ f}^{i}\in Map\right\}\\ T_{\epsilon_{i}e}=T_{\epsilon_{i}m}T_{mc}\\ T_{\epsilon_{i}m}=T_{\epsilon_{i}c}T_{cw}\\ p_{iw}^{j}=T_{wc_{i}}p_{ic}^{j}\end{array}\right. \tag{3}\]  
where, \(K_{f}\) is a keyframe set is shared with the corrected keyframe \(k_{f}\), \(T_{\epsilon_{i}c}\) is the relative pose between the common view keyframe and the corrected keyframe, \(T_{\epsilon_{i}w}\) is the corrected pose of the common view keyframe, \(p_{iw}^{j}\) is the corrected 3D coordinates of the j-th map point observed by the common view keyframe, and \(p_{ic}^{j}\) is the coordinate of the map point in the keyframe camera coordinate system. After the above steps are performed, the accuracy of the local map around the corrected keyframe is improved, and the control range of the accurate pose information in the map is expanded.