LOAM简介

LOAM简介LOAMLOAM系统流程  整体思想:在点云中提取边缘特征(如树干、墙角等采用线模型描述)和平面特征(如地面、墙面等,采用面模型描述),匹配时以点到线距离和点到面的距离为残差优化位姿。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\inS,j\neqi}\lef

大家好,欢迎来到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=SX(k,i)L1jS,j=i(X(k,i)LX(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)LXˉ(k,l)L(X~(k+1,i)LXˉ(k,j)L)×(X~(k+1,i)LXˉ(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)LXˉ(k,l)L)×(Xˉ(k,j)LXˉ(k,m)L)(X~(k+1,i)LXˉ(k,j)L)((X~(k,j)LXˉ(k,l)L)×(Xˉ(k,j)LXˉ(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=1Nεdεi+i=1NHdHi=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+sysxSzcxszcysxszczsyczsysxcyszcxczcyczsx+syszcxsysxcycx
{ 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+1LTk+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+1LF(X(k+1,i)L,Tk+1L)=Tk+1LD(G(X(k+1,i)L,Tk+1L))=X~(k+1,i)LD(X~(k+1,i)L)Tk+1LG(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)LD(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)LD(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

(0)

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用 * 标注

关注微信