## 2 SLAM Framework Based on Graph Optimization  
A graph-based SLAM framework could be divided into two parts: Front-end and back-end, as shown in Figure 1 below. The front-end is mainly used to estimate the position and pose of the robot by sensor data. However, the observed data contain varying degrees of noise whether in images or in laser. Especially for low-cost LiDAR and cameras. The noise will lead to cumulative errors in pose estimation, and such errors will increase with time. Through filtering or optimization algorithms, the back-end part can eliminate the cumulative errors and improve the positioning and map accuracy. In this paper, graph optimization is used as the back-end, and the error is minimized by searching the descending gradient through nonlinear optimization. Graph optimization simply describes an optimization problem in the form of a graph. The node of the graph represents the position and attitude, and the edge represents the constraint relationship between the position and the attitude and the observation. The sketch map of graph optimization is shown in Figure 2.  
Figure 1: A general graph-based light detection and ranging (SLAM) framework. In Figure 2, **X** represents the position and pose of the robot, and **Z** represents the observation. In this paper, the robot observes the external environment through both LiDAR and camera. As a result, **Z** could be a combination of three-dimensional spatial feature points detected by camera or obstacle points detected by LiDAR. Matching result and errors of visual feature points and scan data are an essential part to generate the nodes and constrain for graph optimization. For the matching of visual feature points, the error is usually represented by re-projection error. The calculation of re-projection error needs to give two cameras corresponding to adjacent frames, two-dimensional coordinates of matched feature points in two images and three-dimensional coordinates of corresponding three-dimensional space points. As shown in Figure 3, for the feature point pairs \(p_{1}\) and \(p_{2}\) in adjacent frames, a real-world spatial point \(P\) corresponding to \(p_{1}\) could be localized at first, and then \(P\) is re-projected onto the latter frame to form the feature point position \(\hat{p}_{2}\) of the image. Due to the error of position and pose estimation, the existence of \(\hat{p}_{2}\) and \(p_{2}\) is not a coincidence. The distance between these two points could be denoted as a re-projection error. Pure vision SLAM usually extracts and matches feature points, approaches like EPnP [29] can be applied to estimate the pose transformation \((R,t)\) of adjacent frames. Where \(R\) represents the rotation matrix, and \(t\) represents the transformation matrix. According to Figure 3, the re-projection errors could be calculated as follows:  
Firstly, calculate the 3D position \(\left[X^{\prime},Y^{\prime},Z^{\prime}\right]\) of \(P^{\prime}\) (corresponding point of \(P\)) in camera coordinate, through the pose and transformation relation [**R**,**t**]:  
\[P^{\prime}=RP+t=[X^{\prime},Y^{\prime},Z^{\prime}]^{T}. \tag{1}\]  
Then, get the normalized coordinate \(\textbf{P}_{c}\):  
\[\textbf{P}_{c}=[u_{c},v_{c},1]^{T}=[X^{\prime}/Z^{\prime},Y^{\prime}/Z^{\prime},1]^{T}. \tag{2}\]  
Based on camera parameters \((f_{x},f_{y},c_{x},c_{y})\), the re-projected pixel coordinate \(\hat{p}_{2}=(u_{s},v_{s})\) could be calculated as:  
\[\left\{ \begin{array}{l}u_{s}=f_{x}u_{c}+c_{x}\\ v_{s}=f_{y}v_{c}+c_{y}\end{array} .\right. \tag{3}\]  
Figure 3: The re-projection error. Figure 2: Sketch map of graph optimization. Finally, if there are \(n\) feature points matched between two frames, the re-projection error is:  
\[e_{1}=\frac{1}{2}\sum_{i=1}^{n}\left\|p_{i}-\hat{p}_{i}^{\prime}\right\|_{2}^{2}. \tag{4}\]  
Compared with the visual error, the laser scan matching error is much simpler to obtain. LiDAR-SLAM usually needs scan matching to realize the estimation of pose transformation of adjacent frames.