LOAM
文章目录
LOAM系统流程
整体思想: 在点云中提取边缘特征(如树干、墙角等采用线模型描述)和平面特征(如地面、墙面等,采用面模型描述),匹配时以点到线距离和点到面的距离为残差优化位姿。
1、特征提取
(1)按线数分割
这里的线数指的是激光雷达的线程数。
(2)计算曲率
c
=
1
∣
S
∣
⋅
∥
X
(
k
,
i
)
L
∥
∑
j
∈
S
,
j
≠
i
(
X
(
k
,
i
)
L
−
X
(
k
,
j
)
L
)
∥
c=\frac{1}{|S| \cdot \| X_{(k, i)}^{L}}\left\|\sum_{j \in S, j \neq i}\left(X_{(k, i)}^{L}-X_{(k, j)}^{L}\right)\right\|
c=∣S∣⋅∥X(k,i)L1∥∥∥∥∥∥j∈S,j=i∑(X(k,i)L−X(k,j)L)∥∥∥∥∥∥
该公式计算点
i
i
i的曲率,其中
S
S
S代表在周围,并且和
i
i
i再同一条扫描线上的点,
X
(
k
,
i
)
L
X_{(k, i)}^{L}
X(k,i)L和
X
(
k
,
j
)
L
X_{(k, j)}^{L}
X(k,j)L分别代表点
i
i
i和
j
j
j的坐标。
(3)删除异常点
(4)按曲率大小筛选特征点
主要分为以下四类:
a.曲率特别大的点(sharp)
b.曲率大的点(less_sharp)
c.曲率特别小的点(flat)
d.曲率小的点(less_flat)
2、帧间匹配
(1)特征关联与损失函数计算
a.线特征 b.面特征
点到线的距离
d
ε
=
∣
(
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
∣
(
1
)
d_{\varepsilon}=\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ε=∣∣∣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)∣∣∣(1)
点到面的距离
d
ε
=
∣
(
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
)
∣
(
2
)
d_{\varepsilon}=\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)
dε=∣∣∣(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))∣∣∣∣∣∣(2)
把(1)和(2)放在一个模型里,得到总的损失函数为:
loss
=
∑
i
=
1
N
ε
d
ε
i
+
∑
i
=
1
N
H
d
H
i
=
D
(
X
~
(
k
+
1
,
i
)
L
)
(
3
)
\operatorname{loss}=\sum_{i=1}^{N_{\varepsilon}} d_{\varepsilon i}+\sum_{i=1}^{N_{H}} d_{H i}=D\left(\tilde{X}_{(k+1, i)}^{L}\right)(3)
loss=i=1∑Nεdεi+i=1∑NHdHi=D(X~(k+1,i)L)(3)
定义
t
k
+
1
t_{k+1}
tk+1时刻的位姿为
T
k
+
1
L
=
[
t
x
,
t
y
,
t
z
,
θ
x
,
θ
y
,
θ
z
]
T
T_{k+1}^{L}=\left[t_{x}, t_{y}, t_{z}, \theta_{x}, \theta_{y}, \theta_{z}\right]^{T}
Tk+1L=[tx,ty,tz,θx,θy,θz]T
特征点从当前雷达坐标系投影到目标坐标系
X
~
(
k
+
1
,
i
)
L
=
R
X
(
k
+
1
,
i
)
L
+
t
=
G
(
X
(
k
+
1
,
i
)
L
,
T
k
+
1
L
)
(
4
)
\begin{aligned} \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{aligned} (4)
X~(k+1,i)L=RX(k+1,i)L+t=G(X(k+1,i)L,Tk+1L)(4)
其中
t
=
[
t
x
,
t
y
,
t
z
]
T
t=\left[t_{x}, t_{y}, t_{z}\right]^{T}
t=[tx,ty,tz]T
R
=
R
x
R
y
R
z
=
[
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
]
\begin{aligned} 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{aligned}
R=RxRyRz=⎣⎡cycz+sysxSzcxszcysxsz−czsyczsysx−cyszcxczcyczsx+syszcxsy−sxcycx⎦⎤
又
{
c
x
=
cos
(
θ
x
)
s
x
=
sin
(
θ
x
)
c
y
=
cos
(
θ
y
)
s
y
=
sin
(
θ
y
)
c
z
=
cos
(
θ
z
)
s
z
=
sin
(
θ
z
)
\left\{\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}\right.
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧cx=cos(θx)sx=sin(θx)cy=cos(θy)sy=sin(θy)cz=cos(θz)sz=sin(θz)合并(3)和(4)得到
loss
=
F
(
X
(
k
+
1
,
i
)
L
,
T
k
+
1
L
)
=
D
(
G
(
X
(
k
+
1
,
i
)
L
,
T
k
+
1
L
)
)
\operatorname{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)
loss=F(X(k+1,i)L,Tk+1L)=D(G(X(k+1,i)L,Tk+1L))
(2)LM迭代优化
T
k
+
1
L
←
T
k
+
1
L
−
(
J
T
J
+
λ
diag
(
J
T
J
)
)
−
1
J
T
d
T_{k+1}^{L} \leftarrow T_{k+1}^{L}-\left(J^{T} J+\lambda \operatorname{diag}\left(J^{T} J\right)\right)^{-1} J^{T} d
Tk+1L←Tk+1L−(JTJ+λdiag(JTJ))−1JTd
其中
J
=
∂
F
(
X
(
k
+
1
,
i
)
L
,
T
k
+
1
L
)
∂
T
k
+
1
L
=
∂
D
(
G
(
X
(
k
+
1
,
i
)
L
,
T
k
+
1
L
)
)
∂
T
k
+
1
L
=
∂
D
(
X
~
(
k
+
1
,
i
)
L
)
∂
X
~
(
k
+
1
,
i
)
L
∂
G
(
X
(
k
+
1
,
i
)
L
,
T
k
+
1
L
)
∂
T
k
+
1
L
\begin{aligned} 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{aligned}
J=∂Tk+1L∂F(X(k+1,i)L,Tk+1L)=∂Tk+1L∂D(G(X(k+1,i)L,Tk+1L))=∂X~(k+1,i)L∂D(X~(k+1,i)L)∂Tk+1L∂G(X(k+1,i)L,Tk+1L)
对于线特征,梯度方向为通过特征点的垂直于直线的方向
∂
D
(
X
~
(
k
+
1
,
i
)
L
)
∂
X
~
(
k
+
1
,
i
)
L
=
[
a
ε
,
b
ε
,
c
ε
]
T
\frac{\partial D\left(\tilde{X}_{(k+1, i)}^{L}\right)}{\partial \tilde{X}_{(k+1, i)}^{L}}=\left[a_{\varepsilon}, b_{\varepsilon}, c_{\varepsilon}\right]^{T}
∂X~(k+1,i)L∂D(X~(k+1,i)L)=[aε,bε,cε]T
对于面特征,梯度方向为通过特征点的垂直于平面的方向
∂
D
(
X
~
(
k
+
1
,
i
)
L
)
∂
X
~
(
k
+
1
,
i
)
L
=
[
a
H
,
b
H
,
c
H
]
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}
∂X~(k+1,i)L∂D(X~(k+1,i)L)=[aH,bH,cH]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
③运行结果
LOAM-KITTI数据集
下载链接
这里给大家提供本文中所使用的kitti数据测试下载链接,欢迎下载
测试用到的KITTI数据集下载链接
写在最后
博客中有相关错误,希望大佬们指出。(感谢感谢!!)