SLAM——LIO-SAM
Lio-sam: https://github.com/TixiaoShan/LIO-SAM
Lio-sam has a similar code structure with A-LOAM (more similar to LeGO-LOAM, but without ground points). However, they are very different. A-LOAM doesn't have a pose graph optimization in back-end. It uses current scan to matching the global map, which can lead to drift over a long distance scanning. LIO-SAM uses a lidar-IMU odometry, and it adopts a factor graph for multi-sensor fusion and global optimization. If you want to fuse other sensors it would be much easier. Also, it supports loop closure detection.
From the demo (https://www.youtube.com/watch?v=A0H8CoORZJU), we could see that LIO-SAM is really robust to arbitrary rotation and has high localization accuracy. At any time, the robot state can be formulated as:
\(\mathbf R\) is the rotation matrix, \(\mathbf p\) is the position, \(\mathbf v\) is the speed and \(\mathbf b\) is the IMU bias.
\[ \mathbf{x}=\left[\mathbf{R}^{\mathbf{T}}, \mathbf{p}^{\mathbf{T}}, \mathbf{v}^{\mathbf{T}}, \mathbf{b}^{\mathbf{T}}\right]^{\mathbf{T}} \]
There are 4 types of factors in LIO-SAM: (a) IMU preintegration factors, (b) lidar odometry factors,(c) GPS factors, and (d) loop closure factors.
We denote the world frame as \(\mathbf W\) and the robot body frame as \(\mathbf B\). \(\mathbf T_t^{\mathbf {BW}}\) is the transformation from world
IMU Preintegration
IMU will provide angular velocity and acceleration:
\[ \begin{array}{l}\hat{\omega}_{t}=\omega_{t}+\mathbf{b}_{t}^{\omega}+\mathbf{n}_{t}^{\omega} \\\hat{\mathbf{a}}_{t}=\mathbf{R}_{t}^{\mathbf{B W}}\left(\mathbf{a}_{t}-\mathbf{g}\right)+\mathbf{b}_{t}^{\mathbf{a}}+\mathbf{n}_{t}^{\mathbf{a}}\end{array} \]
where \(\hat{\omega}_t\) (angular velocity) and \(\hat{\mathbf{a}}_{t}\) (acceleration) are the raw IMU measurements in \(\mathbf B\) at time \(t.\) \(\mathbf b_t\) and \(\mathbf n_t\)are bias and white noise respectively. \(\mathbf R_t^{\mathbf{BW}}\) is the rotation matrix from \(\mathbf W\) to \(\mathbf B\). \(\mathbf g\) is the constant gravity vector in \(\mathbf W\). And based on those measurements we could infer the motion of robot. The velocity, position and rotation of the robot at time \(t + \Delta t\) can be computed as follows:
\[ \begin{array}{l}\mathbf{v}_{t+\Delta t}=\mathbf{v}_{t}+\mathbf{g} \Delta t+\mathbf{R}_{t}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{t}^{\mathbf{a}}-\mathbf{n}_{t}^{\mathbf{a}}\right) \Delta t \\\mathbf{p}_{t+\Delta t}=\mathbf{p}_{t}+\mathbf{v}_{t} \Delta t+\frac{1}{2} \mathbf{g} \Delta t^{2}+\frac{1}{2} \mathbf{R}_{t}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{t}^{\mathbf{a}}-\mathbf{n}_{t}^{\mathbf{a}}\right) \Delta t^{2} \\\mathbf{R}_{t+\Delta t}=\mathbf{R}_{t} \exp \left(\left(\hat{\omega}_{t}-\mathbf{b}_{t}^{\omega}-\mathbf{n}_{t}^{\omega}\right) \Delta t\right)\end{array} \]
where \(\mathbf{R}_{t}=\mathbf{R}_{t}^{\mathbf{W B}}=\mathbf{R}_{t}^{\mathbf{B W}^{\top}}\). The IMU preintegrated measurements between time \(i\) and \(j\) can be computed as following (Apparently, \(\Delta \mathbf{v}_{i j}\) is not \(v_j - v_i\) ):
\[ \begin{aligned}\Delta \mathbf{v}_{i j} &=\mathbf{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right) \\\Delta \mathbf{p}_{i j} &=\mathbf{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right) \\\Delta \mathbf{R}_{i j} &=\mathbf{R}_{i}^{\top} \mathbf{R}_{j} .\end{aligned} \]
See https://arxiv.org/pdf/1512.02363.pdf for detailed derivation.
Lidar Odometry
The lidar odometry in LIO-SAM is similar to Lego-LOAM or A-LOAM. The difference is LIO-SAM adopt keyframe strategy. When a new keyframe \(\mathbb F_{i}\) is registered, it will associate with a new robot state node \(x_{i}\) in the factor graph. The lidar frames between two keyframes will be discarded (Note that the position and rotation change thresholds for adding a new keyframe in LIO-LOAM are \(1m\) and \(10\degree\) respectively). The generation of a lidar odometry factor is described in the following step:
- Sub-keyframes for voxel map. Organize \(n\) most recent keframes as a sub-keyframes and combines their scans as a voxel map \(\mathbf M_i = \left\{\mathbb{F}_{i-n}, \ldots, \mathbb{F}_{i}\right\}\), which includes edge feature voxel map \(\mathbf M^{e}_i\) and plane feature voxel map \(\mathbf M^{p}_i\). Note that \(\mathbf M_i\) will be down-sampled to remove duplicated features.
- Scan-matching. A new keyframe \(\mathbb F_{i+1}\) will be matched to \(\mathbf M_{i}\). The initial transformation \(\tilde{\mathbf{T}}_{i+1}\) is estimated through IMU. The matching process is similar to A-LOAM's lidar mapping module.
GPS
System will still suffer from drift during long-distance navigation tasks. GPS is useful for correcting this long-distance-error. In LIO-SAM, a GPS factor is only added when the estimated position covariance is larger that the received GPS position covariance.
Loop Closure
LIO-SAM use a naive but effective method for loop closure detection based on the Euclidean distance. When a new node \(x_{i+1}\) is registered, we search for the near nodes from the factor graph. For example, \(x_3\) is selected, and we match \(\mathbb F_{i+1}\) with the subkeframe voxel map \(\left\{\mathbb{F}_{3-m}, \ldots, \mathbb{F}_{3}, \ldots, \mathbb{F}_{3+m}\right\}\). If the matching succeed, a relative transformation \(\Delta \mathbf T_{3, i+1}\) will be added into fact graph as a loop closure factor. In LIO-SAM，\(m = 12\) and the search radius is \(15m\).