## 3 The SLAM Framework of Low-Cost LiDAR and Vision Fusion
### Pose Graph Optimization  
Due to the errors and noise of sensor measurements, the best matching of two adjacent poses may not be the best for the whole map. By unitizing a series of relative poses, the pose graph optimization could be applied for minimizing the errors during the SLAM process. In our approach, the robot pose set \(x\) is regarded as the variable to be optimized, and the observations are regarded as the constraints between the poses.  
The robot pose set \(x\) is:  
\[x=[x_{1},x_{2},x_{3},\dots,x_{k}]^{T}. \tag{10}\]  
Based on the Lie Algebraic transformation set \(\xi\), robot pose set \(x\) is related to transformation set of \(\xi\) by:  
\[x=h(\xi). \tag{11}\]  
Which also could be written as:  
\[\xi=h^{-1}(x). \tag{12}\]  
As a result, the total error of \(K\) pairs of related poses could be rewritten as:  
\[E=\sum_{i=1}^{K}e_{i}=\sum_{i=1}^{K}F_{i}(\xi_{i})=f(x). \tag{13}\]The target of pose graph optimization is to find a proper set \(x\), so that the error \(f(x)\) is minimized. Correspondingly, \(\Delta x\) is the increment of the \(x\) of the global independent variable. Therefore, when the increment is added, the objective function is:  
\[\frac{1}{2}\big{\|}f(x+\Delta x)\big{\|}^{2}\approx\frac{1}{2}\sum_{t=1}^{k} \big{\|}f(x_{t})+J_{t}\Delta x_{t}\big{\|}^{2}, \tag{14}\]  
where \(J\) represents the Jacobian matrix, which is the partial derivative of the cost function to the variables, \(k\) is the number of poses to be optimized, and the number of postures between the current frame and the loop frame is the global optimization.  
Position and pose optimization can be regarded as the least square problem. The commonly used methods to solve the least square problem are Gradient Descent method, Gauss Newton method [30] and Levenberg-Marquardt (L-M) method [31]. The L-M method is a combination of Gadient Descent method and Gauss Newton method. This paper uses the L-M method to solve the least square problem.  
According to L-M method and formula (14), in each iteration step, we need to find:  
\[\min_{\Delta x_{i}}\frac{1}{2}\big{\|}f(x_{i})+J(x_{i})\Delta x_{i}\big{\|}^{2 }\text{ s.t. }\big{\|}I\Delta x_{i}\big{\|}^{2}\leq\mu, \tag{15}\]  
where \(\mu\) is the radius of the searching area (here we judge the area is a space sphere) in each step. The problem could be changed to solve the following incremental equation:  
\[(J(x)^{T}J(x)+\lambda I)\Delta x=-J(x)^{T}f(x), \tag{16}\]  
where \(J\) represents the Jacobian matrix, and \(\lambda\) is the Lagrange multiplier. As in each iteration step, \(x\) is known, let \(\mathbf{H}=J(x)^{T}J(x)\) and \(\mathbf{g}=-J(x)^{T}f(x)\), it could be rewritten as:  
\[(\mathbf{H}+\lambda\mathbf{I})\Delta x=\mathbf{g}. \tag{17}\]  
As L-M method could be regarded as a combination of Gadient Descent method and Gauss Newton method in the formula (17), it could be more robust to get a reliable \(\Delta x\) even if \(\mathbf{H}\) is not invertible. By solving the above linear equation, we can obtain \(\Delta x\), and then iterate \(\Delta x\) to obtain \(x\) corresponding to the minimum objective function, that is robot pose. Generally speaking, the dimension of \(\mathbf{H}\) is very large, and the complexity of matrix inversion is \(O(n^{3})\). However, because \(\mathbf{H}\) contains the constraints between poses, and only the adjacent posed have direct constraints, most of the elements are 0. \(\mathbf{H}\) is sparse and shaped like an arrow. The calculation of (17) could be accelerated.  
In practice, the pose graph optimization problem could be solved through tools like g2o [32] or cares [33], with given error function and initial value. In our work, we apply g2o to solve this problem. The error function is formula (8), and the initial value of \(x\) is obtained mainly through scan matching.