前端流程(scanRegistration.cpp)

多线激光雷达即有多个激光发射器同时工作，如常见的 Velodyne16,就是共有 16 个激光发射器，一般这些发射器竖排排列，然后一起水平旋转。
激光雷达在一定的时间内旋转一圈，即一帧的点云数据。值得注意的是，在一帧时间内激光雷达的载体也会运动，因此，一帧数据内会包括不同时间的点云，这也就是激光雷达的运动畸变。
下图是kitti数据集的坐标系表示

loam论文中的框图

1.scanRegistration.cpp

main函数：

建立scanRegistration节点
设置雷达线束(64)，最小范围(5m)
订阅雷达原始点云信息，发布的信息包括有效点云、大曲率角点、稍微大曲率角点、小曲率面点，稍微小曲率面点，主要处理过程在原始点云信息的回调函数

laserCloudHandler回调函数

ros格式转pcl点云，移除空点及车体范围内的点
角度补偿，计算起始角度和结束角度，确保相差2 PAI
遍历点云，根据俯仰角计算对应的扫描线，根据水平角计算当前角度距离起始角度的差值，根据差值计算时间戳，用于后面进行运动补偿，将点云存储进对应的扫描线。这里注意intensity属性整数部分存储的是扫描线的索引，小数部分是相对于起始时刻的时间
提取特征并均匀化。首先计算所有点云曲率，将每个扫描线分为6等分，将角点曲率从小到大排序，每段取最大的两个作为大曲率角点，次大的20个作为稍微大曲率的角点，取曲率最小的4个作为面点，对面点进行下采样滤波
发布点云信息，包括包括有效点云、大曲率角点、稍微大曲率角点、小曲率面点，稍微小曲率面点

2.异常点筛选

这一步其实是需要放在特征提取之前做的
但是这块A-loam中没有，借助lio-sam中featureExtraction.cpp讲解

首先遍历所有点云，取出相邻两点的距离信息，计算两个点的列数id差。由于在筛选时会去除空点，因此防止两个有效点云虽然相邻但是间隔很远。
根据深度信息筛选下图中b的情况，根据相邻点的距离差，筛选下图中a的情况。

3.激光运动畸变及补偿

激光雷达的一帧数据是过去一段时间而非某个时刻的数据，在这一帧时
间内的激光雷达或者其载体通常会发生运动，因此，这一帧数据的原点都不一致，运动补偿的目的就是把所有的点云补偿到某一个时刻，这样就可以把本身在过去100ms 内收集的点云统一到一个时间点上去
Pstart=Tstartcurrent∗PcurrentP_{start}=T_{start_current} * P_{current} Pstart=Tstartcurrent∗Pcurrent
运动补偿需要知道每个点该时刻对应的位姿$ T_{start_current $，通常有几种做法：

如果有高频里程记，可以比较方便的获取每个点相对起始扫描时刻的位姿
如果有 imu，可以方便的求出每个点相对起始点的旋转
如果没有其他传感器，可以使用匀速模型假设，使用上一个帧间里程记的结果作为当前两帧之间的运动，同时假设当前帧也是匀速运动，也可以估计出每个点相对起始时刻的位姿

代码对应laserOdometry.cpp
去畸变
TransformToStart()函数将输入的点云坐标转到起始时刻

输入点云信息
对于旋转，使用Eigen的四元数插值得到当前时刻的四元数
对于平移，使用根据时间占当前周期的比例直接相乘
由于kitti里面是补偿过的，因此这里比例都设置为1
输出去畸变后的点云

TransformToEnd()函数将输入的点云坐标转到结束时刻

先调用TransformToStart()将输入点云转到起始时刻
根据帧到起始时刻到结束时刻的位姿信息，将点云转到结束时刻

主函数

定义节点laserOdometry
设置参数
订阅scanRegistration发布的角点、面点、有效点云5个topic，每个回调函数只负责把输入存入数组
发布5个topic，分别是
while循环实现激光里程计

while循环

触发一次回调函数（spinOnce）
ros消息转为pcl格式
第一帧的点云信息不处理，只加入kd tree。对第二帧开始构建约束求解，对于点线约束，构建当前点和上一帧两点的距离约束，两个点分别是kd tree中最近的点和不同扫描线上较近的点；对于点面约束，找的三个面点分别是kd tree最近的点、相同扫描线较近的点、不同扫描线较近的点。
根据约束构建ceres优化器，迭代4次进行优化
递推更新相对世界系的位姿
将当前帧点云加入kd tree,发布雷达里程计信息、路径、将较平坦的面点、曲率较大的角点、有效角点转为ros发布，用于后端

后端流程

laserMapping.cpp

main()函数

建立laserMapping节点，加载体素滤波参数，订阅前端点云和里程计信息，回调函数就是把信息存进队列。将后端数组复位，建立process线程进行处理

process()函数

里程计、点云、面点队列的首个元素时间戳都和角点队列的首个元素时间戳对齐，然后将首个元素都转为ros格式并弹出，清空角点数组以保证每次处理的实时性
将前端得到的位姿转到后端地图下Tmap_cur=Tmap_odom∗Todom_curT_{map\_cur}=T_{map\_odom} * T_{odom\_cur} Tmap_cur=Tmap_odom∗Todom_cur
得到当前帧在地图中的初始估计
根据初始估计确定在栅格中的索引，每个栅格是505050的正方体，宽21个(x)、高21个(y)、深11个(z)，一共4851个。如果当前点的栅格太靠左，就将整体右移，将第i-1个栅格点云放到第i个，靠近右边界就左移，其他两个轴也这样操作，这样移动完后会空出一列。经过这个步骤保证当前的位姿步骤栅格地图的边缘。
取出当前栅格附近 250250150的局部小地图做当前帧的配准，取出小地图的栅格id以及点云信息。
对当前帧的角、面特征下采样。下采样后如果还有足够的特征，进行优化。对于点线约束，先将角点点云投影到当前坐标系下，使用kd tree从小地图找到5个最近的角点，最远的必须小于1m，然后计算这个五个点的协方差矩阵，使用eigen进行特征值分解，如果最大特征值大于次大特征值的3倍，就认为是线特征，可以添加到ceres优化器。对于点面约束，同样kd tree搜索5个最近面点，计算其法向量，然后判断每个点到平面的距离，都小于0.2则将当前点和法向量送入优化器，开始优化。
进行位姿更新
Tmap_odom=Tmap_cur∗Todom_cur−1T_{map\_odom}=T_{map\_cur} * T_{odom\_cur}^{-1} Tmap_odom=Tmap_cur∗Todom_cur−1
将当前帧优化后的点云加到局部地图，将特征投影到地图坐标系，计算其在栅格中的位置，得到索引后加入一维数组。
对局部地图进行体素滤波，然后发布位姿、轨迹、tf、点云信息。

loam 框架流程描述相关推荐

基于LOAM框架的激光SLAM开源程序集合
一.前言 LOAM, 即Lidar Odometry and Mapping,是 Ji Zhang 博士于2014年提出的使用激光雷达完成定位与三维建图的算法.其算法流程如下: LOAM算法中主要包含 ...
Flask 框架流程
Flask 框架流程本人使用软件是 PyCharm 什么是Flask: Flask诞生于2010年, 使用python语言基于Werkzeug工具箱编写的轻量级Web开发框架 Flask本身相当于一 ...
python中基本程序结构_关于Python 程序格式框架的描述,正确的是( )
[判断题]元组的元素是可读的,可以对元组进行更新.增加.删除操作. [多选题]哪些选项关于循环结构的描述是正确的( ) [单选题]以下可以终结一个循环的是 . [判断题]对于大量列表的连接,exten ...
4G与5G会话建立流程描述以及对比
4G与5G会话建立流程描述以及对比 1. 用于会话建立流程的EPC网元与5GC网元 1.1 EPC架构 1.2 5GC架构 1.3 有关会话建立流程中网络功能分离的描述 2. EPC与5GC会话建立信 ...
SpringMVC工作流程描述（简易版本）
1.SpringMVC工作流程图: 2.SpringMVC工作流程描述: (1)用户端将请求统一提交到DispatcherServlet: (2)DispatcherServlet会将请求交给Hand ...
python语言不采用严格的缩进来表示程序的格式框架_关于 Python 程序格式框架的描述，以下选项中错误的是?????????????????????...
关于 Python 程序格式框架的描述,以下选项中错误的是????????????????????????????????????????????????????????????????( ?) . ...
ssm框架简单框架流程
ssm框架简单框架流程 1.框架搭建步骤一:mybatis部分 1.建实体类(pojo层):起名类名与表名一致,但类开头大写:如数据库某表books,类名Books 2.写接口进行一些业务操作(da ...
[源码] Eoxplayer主框架流程
Eoxplayer主框架流程核心流程 playing流程 AudioTrack调用核心流程主要流程: 采用1个线程Loop实现音/视频的解码渲染.(通常做法是把音视频各用1个线程去解码渲染,各不 ...
loam框架的注意点，公式推导细节，lego-loam对其的改进及自身不足，
目录前端流程及注意细节: 总体结构: 特征提取部分: 特征关联匹配部分: 最小二乘优化部分: 后端部分: lego-loam对其的改进: 前端流程及注意细节: 总体结构: 1.激光slam框架loa ...

loam 框架流程描述