aloam中增加回环模块—

aloam没有回环模块，参考lego-loam增加回环模块。

一、lego loam mapping模块分析

1、首先要明确几个重要转换矩阵的含义：

    float transformLast[6];  float transformSum[6];float transformIncre[6];float transformTobeMapped[6];float transformBefMapped[6];float transformAftMapped[6];

transformLast[6]与transformAftMapped[6]一样，记录的是某个点云帧到地图坐标系的转换关系；
transformTobeMapped：在LM优化中，优化的是这个transformTobeMapped，优化完后，会在 transformUpdate()函数中transformTobeMapped赋给transformAftMapped，同时也会把transformSum赋给transformBefMapped。
transformSum、transformBefMapped记录的是lidar到里程计坐标系的转换关系，Sum记录的是当前得到的lidar到里程计的转换关系，而transformBefMapped记录的是上一帧的。

2、数据的转换关系：

mapping模块的回调函数中，接收laserOdometry模块发出/laser_odom_to_init，将其记录在transformSum变量中。
在主循环中，第一步做的就是调用transformAssociateToMap函数，这个函数的基本思路就是利用前一帧lidar到里程计坐标系的转换关系（记录在transformBefMapped）以及当前到里程计坐标系的转换关系（记录在transformSum中），计算出相对变化量，然后利用上一次的lidar到map的转换关系（记录在transformAftMapped），结合这个变化量，估计出当前的lidar到地图的转换关系（transformTobeMapped）。
估计方式：
T_(tobe)=T_(wAft) * T_(wBef).inverse * T_(wsum)
R=R(y)R(x)R(z)
估计了当前帧对地图的转换关系后，就开始抽取附近帧的数据。抽取的时候，在不打开loopdetect时，用的是currentRobotPosPoint这个变量作为原点来搜索，这个变量在这个时候记录的其实是上一时刻的姿态。
抽取附近关键帧的方法是kdtree在所有pose3d中搜索，找到附近50米的pose。把最终整理到的pose对应的点云加在一起：

for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];*laserCloudSurfFromMap   += *surroundingSurfCloudKeyFrames[i];*laserCloudSurfFromMap   += *surroundingOutlierCloudKeyFrames[i];}

下一步是对当前得到的surf、corner点进行下采样
接着调用scan2MapOptimization执行优化了，这个是关键：

void scan2MapOptimization(){if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);for (int iterCount = 0; iterCount < 10; iterCount++) {laserCloudOri->clear();coeffSel->clear();cornerOptimization(iterCount);surfOptimization(iterCount);if (LMOptimization(iterCount) == true)break;              }transformUpdate();}}

其实就是利用得到的周围的点云（分了corner、surf的）和当前的点云，进行优化计算，这个过程可以和aloam一样，用ceres进行。
得到了优化后的lidar到map的转换关系transformTobeMapped，然后在调用transformUpdate，对transformBefMapped和transformAftMapped进行赋值。
这个transformUpdate方法用到了接收到的imu的信息。

优化已经做完了，接着调用saveKeyFramesAndFactor方法保存key frame数据和factor因子。
函数首先保存当前位置：

 currentRobotPosPoint.x = transformAftMapped[3];currentRobotPosPoint.y = transformAftMapped[4];currentRobotPosPoint.z = transformAftMapped[5];

然后判断当前帧是否作为一个关键帧处理，判断的条件是与上一个关键帧的位移大于0.3米，或者是不是第一帧，只有是第一帧或者判断为是关键帧，才往下处理，否则不再处理。
如果是，则进入下面的处理逻辑：
如果是第一帧，则把当前的位置（理论上就是无旋转、无平移）作为先验加入到graph中；
如果不是第一帧，则构建上一个关键帧与本关键帧的betwenFactor，加入到graph中；
然后进行isam的计算，这种计算会实时给出了整个图的姿态，当然包括当前的姿态，所以，会用最新估计的姿态赋值给transformAftMapped，这个就是图优化后的更新姿态。
当然也要把估计的最新姿态数据给到transformLast和transformTobeMapped。
有个问题：
这里为什么图优化是有效的？
像当前帧的姿态就是利用上一帧的姿态根据相对变化计算的，加入到这种图里，有用吗？
我的理解：
imu起的作用还未分析；
这个图构建的时候，不是每一帧都放入进去的，而是隔几帧才放入的，这就可以消除那种缓慢偏离。
如对于不同时刻：i=1,2,3,4,5,6,7,8
关键帧时刻选为1，5，8.
5的姿态是由4 <- 3 <- 2 <- 1 这样逐步推理得到的，如果在每一阶段都有一定的误差，毕竟不能保证观察是完全准确的，mapping优化刚好到位，就会积累误差。
那加入这个因子图，由于记录了1时刻的位姿，以及5时刻的位姿，8时刻的位姿，在没有回环的时候，给这些位姿之间加上一个不确定度，就可以进行估计了。（好像除了加了不确定度，还是不能很好的说明这个优化有用）
当遇到回环的时候，这个约束就很强了。后面再分析。

correctPoses
当执行了回环后，才进行：
上面优化了所有时刻的位姿后，重新把isam里估计的姿态数据给记录的cloudKeyPoses3D和cloudKeyPoses6D
publishTF
发布当前的地图位姿，和tf数据。
publishKeyPosesAndFrames
发布相关的姿态和点云数据。
clearCloud
清理缓存的用于中间处理的点云数据。
整体的主流程代码：

if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {timeLastProcessing = timeLaserOdometry;//把点云坐标均转换到世界坐标系下//围绕transformTobeMapped数组进行的变换，这就是从雷达坐标系转换到世界坐标系下的变换。transformAssociateToMap();//由于帧数的频率大于建图的频率，因此需要提取关键帧进行匹配extractSurroundingKeyFrames();//降采样匹配以及增加地图点云，回环检测downsampleCurrentScan();scan2MapOptimization();saveKeyFramesAndFactor();correctPoses();publishTF();publishKeyPosesAndFrames();clearCloud();}

二、几个问题
1、imu在lego-loam的mapping起到什么作用？

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);// Get the matrix represented as roll pitch and yaw about fixed axes XYZ. tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;}

imu的数据用到了pitch、roll，唯一用到的就是在：
transformUpdate这个函数里，这个函数的执行是在scan2MapOptimization函数中，优化执行完以后进行才调用的，具体代码：

void transformUpdate(){   //此时transformTobeMapped已经经过LM优化过.if (imuPointerLast >= 0) {float imuRollLast = 0, imuPitchLast = 0;//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront != imuPointerLast) {if (timeLaserOdometry + scanPeriod < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeLaserOdometry + scanPeriod > imuTime[imuPointerFront]) {imuRollLast = imuRoll[imuPointerFront];imuPitchLast = imuPitch[imuPointerFront];} else {int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeLaserOdometry + scanPeriod - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeLaserOdometry - scanPeriod) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuRollLast = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchLast = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;}transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;}for (int i = 0; i < 6; i++) {transformBefMapped[i] = transformSum[i];transformAftMapped[i] = transformTobeMapped[i];//Lm优化是优化transformTobeMapped，将优化后的值赋给transformAftMapped}}

也就是说，上面利用LM方法优化得到的transformTobeMapped这个变量，在这里会根据imu之间的相对变化进行调整。
基本就是按照这一帧雷达的前面时刻的imu数据、后面时刻的imu数据，线性插值出当前帧时刻对应的imuRoll（记录在imuRollLast）、imuPitch（记录在imuPitchLast），然后按照固定权重微调里面的roll、pitch角度

transformTobeMapped[0] = 0.998 * transformTobeMapped[0] + 0.002 * imuPitchLast;
transformTobeMapped[2] = 0.998 * transformTobeMapped[2] + 0.002 * imuRollLast;

这个权重设置是否与安装有关系？imu的坐标轴如果和lidar的不一致，imu的pitch不一定是lidar的pitch啊？
那没有imu其实也可以啊？
有了imu的这一步调整，对应后面的加入因子图就会有不同的约束了，因为当前帧的姿态数据不完全来源于点云的匹配（不管是前后帧还是帧与局部点云的匹配），就带来了更多的信息。

三、调整思路
1、地图保存方式调整
修改地图点云的保存为lego-loam的地图保存的方式，即采用每个pose关联对应该帧点云，不再像aloam那样，将不同时刻得到的点云保存在立方体中，分不出彼此，这样才能在姿态更新后，进行调整。

2、增加imu消息的接收

3、优化的方法还是保留原来的ceres的方式

4、增加因子图记录帧间的关系

aloam中增加回环模块——流程分析相关推荐

SC-A-LOAM：在A-LOAM中加入回环检测
Thanks to LOAM, A-LOAM, and LIO-SAM code authors. The major codes in this repository are borrowed fr ...
SylixOS中MTD调用底层接口流程分析
1. MTD设备与底层驱动的关系 MTD设备是一种特殊的抽象设备,它用于简化驱动开发.它是底层硬件和上层软件的桥梁,无论对Nand Flash或是Nor Flash,它都提供了统一的框架供上层文件 ...
【android系统】android系统升级流程分析（一）---recovery模式中进行update包升级流程分析
今天我们直接来看下android中具体的升级过程是如何的. 升级流程概述升级的流程图: 升级流程分析第一步:升级包获取升级获取可以通过远程下载,也可直接拷贝到指定目录即可. 第二步:准备升级然 ...
HTC VIVE SDK 中的例子 hellovr_opengl 程序流程分析
最近Vive的VR头盔设备很火,恰逢项目需求,所以对 SDK 中的例子 hellovr_opengl 做了比较细致的代码分析,先将流程图绘制如下,便于大家理解. 在ViVe头盔中实现立体效果的技术核心 ...
Oracle数据库中闪回恢复的详细分析
Oracle9i开始提供闪回查询,以便能在需要的时候查到过去某个时刻的一致性数据,这是通过Undo实现的.这个功能有很大的限制,就是相关事务的undo不能被覆盖,否则就无力回天了.oracle10g大 ...
android6.0中app crash流程分析
要根据这个流程分析一下如何在应用中截获系统的app crash弹框,然后做到更人性化基于Android 6.0的源码剖析, 分析Android应用Crash是如何处理的. /frameworks/b ...
Linphone android去电增加自定义SIP消息头的流程分析
一.首先看一下如何在发起去电的sip请求中添加自定义的消息头增加自定义头消息发方法,so已经提供了native方法, 发起呼叫的示例如下: LinphoneCallParams params = l ...
蓝牙BLE设备主机重启回连流程分析
本文出自:<蓝牙BLE设备主机重启回连流程分析> 如果一个BLE设备已经与蓝牙中心设备连接上,那么当中心设备的断电重启,其依然会和配对过的BLE设备连接上,而不需要重新走配对的流程,这个过 ...
Alian解读SpringBoot 2.6.0 源码（八）：启动流程分析之刷新应用上下文（中）
目录一.背景 1.1.刷新的整体调用流程 1.2.本文解读范围二.调用后处理器 2.1.调用在上下文中注册为beanFactory的后置处理器 2.2.invokeBeanFactoryPostP ...

aloam中增加回环模块——流程分析

aloam中增加回环模块——流程分析相关推荐

最新文章

热门文章