LOAM介绍
- 实现功能：loam实现了一种实时激光里程计并建图的算法，使用的硬件是一个三维空间中运动的两轴单线激光雷达
这种雷达是在水平和垂直方向的单线雷达在这两个方向上不断变换形成点云，并且每个点的时间戳不同。现在主流使用的velodyne的16线\32线则是在一个垂直方向上有16\32个点，且有相同的时间戳，在水平方向时间戳则是递增的。这个算法也适用16线的lidar。 - 实现难点：每帧点云不是同时间获得，同时存在运动畸变导致匹配也存在误差。目前效果一致性比较好的3d地图是通过离线获得，同时还会通过回环检测来消除累积误差。
- 优点：实现了低的漂移，计算复杂度低，并且不需要高精度的lidar和惯导。
- 算法介绍：由两个部分构成，有一个是执行高频率的里程计但是低精度的运动估计（定位），另一个则是比定位低一个数量级的频率来执行匹配和注册点云信息（建图和校正里程计）。
这两个过程中都需要特征点，就是经典的面点和角点，然后进行配准。在前端定位算法中，特征点的提取会用到快速计算的方法；后端算法中，相互关联的特征点是通过特征值和特征向量来获得的。
符号约定
- P k P_k Pk，第k帧的所有点
- X ( k , i ) L X^L_{(k,i)} X(k,i)L，lidar坐标系下，第k帧的第i个点
- X ( k , i ) W X^W_{(k,i)} X(k,i)W，世界坐标系下，第k帧的第i个点
- P ‾ k \overline{\mathcal{P}}_{k} Pk，第k帧的点，全部投影到k+1帧时刻的点集。
- E k + 1 \mathcal{E}_{k+1} Ek+1，在第k+1帧里面提取到的角点
- H k + 1 \mathcal{H}_{k+1} Hk+1，在第k+1帧里面提取到的面点
算法部分
前端里程计计算相邻帧的运动，计算的运动用于校正畸变，运行频率大概是10hz。
后端处理前端的输出，将运动补偿后的点云与地图进行配准，运行频率1hz。最后是10hz的输出。
1.特征点提取：角点和面点
判断点的方式：取i左右两边的点，计算曲率：
c
=
1
∣
S
∣
⋅
∥
X
(
k
,
i
)
L
∥
∥
∑
j
∈
S
,
j
≠
i
(
X
(
k
,
i
)
L
−
X
(
k
,
j
)
L
)
∥
c=\frac{1}{|\mathcal{S}| \cdot\left\|\boldsymbol{X}_{(k, i)}^{L}\right\|}\left\|\sum_{j \in \mathcal{S}, j \neq i}\left(\boldsymbol{X}_{(k, i)}^{L}-\boldsymbol{X}_{(k, j)}^{L}\right)\right\|
c=∣S∣⋅∥
∥X(k,i)L∥
∥1∥
∥∑j∈S,j=i(X(k,i)L−X(k,j)L)∥
∥
- 把一帧里面的每个点的曲率算出来进行排序。
- 选择曲率最大的几个点为边缘点，最小的点为平面点。为了使提取的特征点均匀，会把每一帧雷达分成四等份（对于velodye这种相当于一根线分成4等分）
- 在每个部分提取出2个边缘点和4个面点。（其中曲率值满足阈值范围才会被判断为是否是特征点，并且选择的数量是不超过设定的值的）
- 不可靠的点：
- 点所在平面和激光束平行；
- 所在平面被另一个在前的平面遮挡，而刚好自己是能出头被看见的那个点，这种会被误认为是边缘点；
- 离群点：某点与左右相邻的两个点的距离过大
2.特征点匹配
方法：时间段从k到k+1之间形成的点聚集成
P
k
\mathcal{P}_k
Pk，然后在第k+1帧开始时（即时间从k+1开始），先把
P
k
\mathcal{P}_k
Pk投影到k+1帧下得到
P
‾
k
\overline{\mathcal{P}}_{k}
Pk（存在一个KD-tree中）。
然后在接收第k+1帧的过程中就递归的迭代估计6自由度运动，每一次迭代中，都用当前估计的变换把
E
k
+
1
和
H
k
+
1
\mathcal{E}_{k+1}和\mathcal{H}_{k+1}
Ek+1和Hk+1投影到第k+1帧下成
E
‾
k
+
1
和
H
‾
k
+
1
\overline{\mathcal{E}}_{k+1}和\overline{\mathcal{H}}_{k+1}
Ek+1和Hk+1，对于这两个集合中的点，需要通过3d KD-tree的方法在
P
‾
k
\overline{\mathcal{P}}_{k}
Pk找到距离最近的点。
- 边缘点匹配
现在i是 E ‾ k + 1 \overline{\mathcal{E}}_{k+1} Ek+1中的一个edge点，现在要在 P ‾ k \overline{\mathcal{P}}_{k} Pk中找两个点（采用点到线的距离，所以需要两个点定一条直线）
第一个点j是 P ‾ k \overline{\mathcal{P}}_{k} Pk中离i最近的点，第二个点l是 P ‾ k \overline{\mathcal{P}}_{k} Pk和j不同线束上的离i最近的点，然后利用曲率确认j和l都是边缘点。
d E = ∣ ( X ~ ( k + 1 , i ) L − X ‾ ( k , j ) L ) × ( X ~ ( k + 1 , i ) L − X ‾ ( k , l ) L ) ∣ ∣ X ‾ ( k , j ) L − X ‾ ( k , l ) L ∣ d_{\mathcal{E}}=\frac{\left|\left(\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}-\overline{\boldsymbol{X}}_{(k, j)}^{L}\right) \times\left(\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right)\right|}{\left|\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right|} dE=∣ ∣X(k,j)L−X(k,l)L∣ ∣∣ ∣(X~(k+1,i)L−X(k,j)L)×(X~(k+1,i)L−X(k,l)L)∣ ∣
上式是用向量的方法求点到直线的距离，在后面的运动估计中会通过非线性优化的方式来使这个距离最小，从而估计出位姿变换。 - 面点匹配
现在i是 H ‾ k + 1 \overline{\mathcal{H}}_{k+1} Hk+1中的一个面点，现在要在 P ‾ k \overline{\mathcal{P}}_{k} Pk中找三个点（采用点到面的距离，所以需要两个点定一平面）
第一个点j是 P ‾ k \overline{\mathcal{P}}_{k} Pk中离i最近的点，然后在j的线束上再找到一个与i最近的第二个点l，再在不同线束上找i的最近邻点m，最后还是要通过曲率的方法确认三个点都是面点。
d H = ∣ ( X ~ ( k + 1 , i ) L − X ‾ ( k , j ) L ) ( ( X ‾ ( k , j ) L − X ‾ ( k , l ) L ) × ( X ‾ ( k , j ) L − X ‾ ( k , m ) L ) ) ∣ ∣ ( X ‾ ( k , j ) L − X ‾ ( k , l ) L ) × ( X ‾ ( k , j ) L − X ‾ ( k , m ) L ) ∣ d_{\mathcal{H}}=\frac{\left|\begin{array}{c} \left(\tilde{\boldsymbol{X}}_{(k+1, i)}^{L}-\overline{\boldsymbol{X}}_{(k, j)}^{L}\right) \\ \left(\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right) \times\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, m)}^{L}\right)\right) \end{array}\right|}{\left|\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, l)}^{L}\right) \times\left(\overline{\boldsymbol{X}}_{(k, j)}^{L}-\overline{\boldsymbol{X}}_{(k, m)}^{L}\right)\right|} dH=∣ ∣(X(k,j)L−X(k,l)L)×(X(k,j)L−X(k,m)L)∣ ∣∣ ∣(X~(k+1,i)L−X(k,j)L)((X(k,j)L−X(k,l)L)×(X(k,j)L−X(k,m)L))∣ ∣
具体看一下手写笔记：
- 运动估计
在每帧的过程中，lidar的运动被假设为是匀速运动，这就使得可以通过线性插值的方式赋值一帧里面每个点的位姿变换。
T ( k + 1 , i ) L = t i − t k + 1 t − t k + 1 T k + 1 L \boldsymbol{T}_{(k+1, i)}^{L}=\frac{t_{i}-t_{k+1}}{t-t_{k+1}} \boldsymbol{T}_{k+1}^{L} T(k+1,i)L=t−tk+1ti−tk+1Tk+1L
其中，t是当前时间， t k + 1 t_{k+1} tk+1是k+1帧开始的时间， T k + 1 L {T}_{k+1}^{L} Tk+1L是 t k + 1 t_{k+1} tk+1和t时刻lidar位姿的变换。那么对于 P k + 1 \mathcal{P}_{k+1} Pk+1中的一点i，其时间为 t i t_i ti，则 T ( k + 1 , i ) L {T}_{(k+1, i)}^{L} T(k+1,i)L就是其在 t k + 1 t_{k+1} tk+1时刻对应的位姿。
通过这个得到的位姿就可以把 E k + 1 和 H k + 1 \mathcal{E}_{k+1}和\mathcal{H}_{k+1} Ek+1和Hk+1投影到第k+1帧下成 E ‾ k + 1 和 H ‾ k + 1 \overline{\mathcal{E}}_{k+1}和\overline{\mathcal{H}}_{k+1} Ek+1和Hk+1，然后求出对应距离后通过LM方法求解。 - lidar建图
建图算法运行频率比里程计低，为1hz。在k+1帧的最后，前端得到了经过畸变矫正的点云 P ‾ k + 1 \overline{\mathcal{P}}_{k+1} Pk+1，并且同时得到了一个位姿变换 T K + 1 L T_{K+1}^L TK+1L，此时建图算法的目的就是将这些点云配准到世界坐标系中。
其中， Q k Q_k Qk是已经存在地图中的点云（前k帧的累积，黑色的两条折线）， T k W T^W_k TkW是第k次扫描得到的地图坐标系下的第k帧位姿（蓝色曲线是轨迹），橙色曲线是由前端里程计得到的 T k + 1 L T^L_{k+1} Tk+1L，然后使用 T k W 和 T k + 1 L T^W_k和T^L_{k+1} TkW和Tk+1L把前端输出的去畸变了的点云映射到地图中，标记为 Q ‾ k + 1 \overline{\mathcal{Q}}_{k+1} Qk+1（绿色部分），再跟 Q k Q_k Qk进行配准，这个过程中会优化lidar的新位姿 T k + 1 W T^W_{k+1} Tk+1W。
–具体实现：
-a.特征提取与前端一样，不过要多10倍的个数。
-b.在进行特征点配对上面有些许的不同，主要是因为地图的点很多，为了提高效率，地图点会按10m的立方体划分，在立方体中有点和 Q ‾ k + 1 \overline{\mathcal{Q}}_{k+1} Qk+1相交时，才会存入3D kd-tree中。
-c.找出 Q k Q_k Qk中特征点周围一定范围的点，对于边缘点，就把范围内边缘点作为一个点集，把面点作为一个点集，然后求这个点集的协方差矩阵，并计算特征值和特征向量。
-d.如果点集是边缘点，那么特征值会一个大，两个小；如果是面点，则特征值会一个小两个大，特征向量并且与面正交；边缘线和平面的位置就认为是在几何中心。
-e.最后求点到直线，点到平面的距离，然后通过LM非线性优化优化求出位姿，
-f.对整个地图进行一个体素滤波。
简单补充
- 3D kd-tree：有多种方式，具体可以网上搜搜，这里挂处一种方法，是在PCL中的一种方式。
先算数据的每个维度上的标准差，假设x维度上的标准差最大，则对应x坐标进行排序，得到一个中位数，把这个中位数作为二叉树根节点，然后所有数据依次进行比较存储。 - 体素滤波：体素滤波器是一种下采样的滤波器，它的作用是使用体素化方法减少点云数量，采用体素格中接近中心点的点替代体素内的所有点云，这种方法比直接使用中心点要慢，但是更加精确。这种方式即减少点云数据，并同时保存点云的形状特征，在提高配准，曲面重建，形状识别等算法速度中非常实用。
4374
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
