大家好,欢迎来到IT知识分享网。
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数据集下载链接
写在最后
博客中有相关错误,希望大佬们指出。(感谢感谢!!)
免责声明:本站所有文章内容,图片,视频等均是来源于用户投稿和互联网及文摘转载整编而成,不代表本站观点,不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益,请在线联系站长,一经查实,本站将立刻删除。 本文来自网络,若有侵权,请联系删除,如若转载,请注明出处:https://yundeesoft.com/14178.html