LOAM

LOAM系统流程

  整体思想： 在点云中提取边缘特征（如树干、墙角等采用线模型描述）和平面特征（如地面、墙面等，采用面模型描述），匹配时以点到线距离和点到面的距离为残差优化位姿。

1、特征提取

（1）按线数分割

  这里的线数指的是激光雷达的线程数。

（2）计算曲率

$$c=\frac{1}{|S|\cdot \Vert X_{(k,i)}^L}\Vert \sum _{j\in S,j\ne i}\left(X_{(k,i)}^L-X_{(k,j)}^L\right)\Vert$$
  该公式计算点$i$的曲率，其中$S$代表在周围，并且和$i$再同一条扫描线上的点,$X_{(k,i)}^L$和$X_{(k,j)}^L$分别代表点$i$和$j$的坐标。

（3）删除异常点

（4）按曲率大小筛选特征点

  主要分为以下四类：
     a.曲率特别大的点（sharp）
     b.曲率大的点（less_sharp）
     c.曲率特别小的点（flat）
     d.曲率小的点（less_flat）

2、帧间匹配

（1）特征关联与损失函数计算

      a.线特征               b.面特征

  点到线的距离
$$d_{\epsilon }=\frac{\left|\left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L\right)\times \left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,l)}^L\right)\right|}{\left|{\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right|}（1）$$
  点到面的距离
$$d_{\epsilon }=\frac{\left|\begin{array}{c}\left({\tilde{X}}_{(k+1,i)}^L-{\bar{X}}_{(k,j)}^L\right)\\ \left(\left({\tilde{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right)\times \left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L\right)\right)\end{array}\right|}{\left|\left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,l)}^L\right)\times \left({\bar{X}}_{(k,j)}^L-{\bar{X}}_{(k,m)}^L\right)\right|}（2）$$
  把（1）和（2）放在一个模型里，得到总的损失函数为：
$$\mathrm{loss}=\sum _{i=1}^{N_{\epsilon }}{d}_{\epsilon i}+\sum _{i=1}^{N_H}{d}_{Hi}=D\left({\tilde{X}}_{(k+1,i)}^L\right)(3)$$
  定义$t_{k+1}$时刻的位姿为
$$T_{k+1}^L={\left[t_x,t_y,t_z,{\theta }_x,{\theta }_y,{\theta }_z\right]}^T$$
  特征点从当前雷达坐标系投影到目标坐标系
$$\begin{array}{rl}{\tilde{X}}_{(k+1,i)}^L& =R{X}_{(k+1,i)}^L+t\\ & =G\left(X_{(k+1,i)}^L,T_{k+1}^L\right)\end{array}(4)$$
其中$$t={\left[t_x,t_y,t_z\right]}^T$$
$$\begin{array}{rl}R& =R_x{R}_y{R}_z\\ & =\left[\begin{array}{ccc}{c}_y{c}_z+s_y{s}_x{S}_z& c_z{s}_y{s}_x-c_y{s}_z& c_x{s}_y\\ c_x{s}_z& c_x{c}_z& -s_x\\ c_y{s}_x{s}_z-c_z{s}_y& c_y{c}_z{s}_x+s_y{s}_z& c_y{c}_x\end{array}\right]\end{array}$$
又$\{\begin{array}{l}{c}_x=\cos \left({\theta }_x\right)\\ s_x=\sin \left({\theta }_x\right)\\ c_y=\cos \left({\theta }_y\right)\\ s_y=\sin \left({\theta }_y\right)\\ c_z=\cos \left({\theta }_z\right)\\ s_z=\sin \left({\theta }_z\right)\end{array}$合并（3）和（4）得到
$$\mathrm{loss}=F\left(X_{(k+1,i)}^L,T_{k+1}^L\right)=D\left(G\left(X_{(k+1,i)}^L,T_{k+1}^L\right)\right)$$

（2）LM迭代优化

$$T_{k+1}^L\leftarrow T_{k+1}^L-{\left(J^{T}J+\lambda \mathrm{diag}\left(J^{T}J\right)\right)}^{-1}{J}^{T}d$$
其中$$\begin{array}{rl}J& =\frac{\partial F\left(X_{(k+1,i)}^L,T_{k+1}^L\right)}{\partial T_{k+1}^L}\\ & =\frac{\partial D\left(G\left(X_{(k+1,i)}^L,T_{k+1}^L\right)\right)}{\partial T_{k+1}^L}\\ & =\frac{\partial D\left({\tilde{X}}_{(k+1,i)}^L\right)}{\partial {\tilde{X}}_{(k+1,i)}^L}\frac{\partial G\left(X_{(k+1,i)}^L,T_{k+1}^L\right)}{\partial T_{k+1}^L}\end{array}$$
对于线特征，梯度方向为通过特征点的垂直于直线的方向
$$\frac{\partial D\left({\tilde{X}}_{(k+1,i)}^L\right)}{\partial {\tilde{X}}_{(k+1,i)}^L}={\left[a_{\epsilon },b_{\epsilon },c_{\epsilon }\right]}^T$$
对于面特征，梯度方向为通过特征点的垂直于平面的方向
$$\frac{\partial D\left({\tilde{X}}_{(k+1,i)}^L\right)}{\partial {\tilde{X}}_{(k+1,i)}^L}={\left[a_H,b_H,c_H\right]}^T$$

3、构建地图

（1）合并地图点

   ①把关键帧的特征点按照位姿转到地图坐标系中
   ②按照位置和cube尺寸划分到对应的cube中

（2）位姿优化与里程计优化的方法相同

4、基于数据集实现

（1）Kitti数据集简介

   硬件组成：
   ①一个64 线程激光雷达，在车顶中心；
   ②两个彩色摄像头和两个黑白摄像头，在雷达两侧；
   ③一个组合导航系统，在雷达左后方。特可以输出RTK/IMU组合导航结果，包括维度和姿态，同时也输出IMU原始数据。

（2）里程计工程框架实现

   核心思想：
   ①通过类的封装，实现模块化；
   ②把ROS流程与C++内部实现分开，使流程清晰；
   ③基于C++多态，实现搞可扩展性。

（3）里程计精度评价

   以组合导航的结果为真值，使用evo工具进行里程计精度评价

（4）数据集的使用

①制作bag文件

  升级 numpy：

sudo pip install -U numpy

  安装kitti2bag：

sudo pip install kitti2bag

  按照如下目录存放文件

生成bag

kitti2bag -t 2011_10_03 -r 0027 raw_synced

②测试bag

  输入以下指令：

roscore
rviz
rosbag play kitti_2011_10_03_drive_0027_synced.bag

③运行结果

下载链接

  这里给大家提供本文中所使用的kitti数据测试下载链接，欢迎下载
测试用到的KITTI数据集下载链接

写在最后

  博客中有相关错误，希望大佬们指出。（感谢感谢！！）