LeGO_LOAM的IMU处理

置顶 bug大湿

已于 2022-10-18 19:36:27 修改

阅读量1.7k

点赞数 1

分类专栏： 0_1SLAM 文章标签：算法 c++ 自动驾驶

于 2022-10-18 17:17:45 首次发布

本文链接：https://blog.csdn.net/qq_42911741/article/details/127188419

版权

0_1SLAM 专栏收录该内容

30 篇文章 6 订阅

订阅专栏

第一部分，是在 featureAssociation.cpp里

1）预处理

Core函数：imuHandler、AccumulateIMUShiftAndRotation

1、imuHandler：记录每帧imu的下标-imuPointerLast和时间戳-imuTime；实现imu坐标系由x前y左z上 ==> z前x左y(为了与相机系统一)，线加速度解算，除去重力加速度影响；得到欧拉角-imuRoll等、加速度-imuAcc、角速度-imuAngularVelo；

参考：LOAM中消除IMU重力影响解析_大箱子123的博客-CSDN博客_imu 去除重力

2、AccumulateIMUShiftAndRotation：换算上一步最终得到的imu系下的加速度（zxy固定轴）到世界坐标系下，用于并且根据匀加速运动积分求解位移(imuShift)、线速度(imuVelo)、角度(imuAngularRotation)；

注意：imuShift位移数据好像后面没用到!

2）帧间匹配预备阶段

Core函数：adjustDistortion、updateInitialGuess

1、adjustDistortion：将分割后的点云segmentedCloud，计算每个点的扫描时刻，插值该点对应的得到imuRollCur等、imuVeloCur、imuShiftCur，对于每帧的第一个点初始化imuRollStart等、imuVeloStart、imuShiftStart、imuAngularRotationCur(两次差值的得到imuAngularFromStart)；

updateImuRollPitchYawStartSinCo：计算imuRollStart等3个角的sin/cos，用于函数VeloToStartIMU和TransformToStartIMU；

VeloToStartIMU：速度转换，yxz旋转，相对于该帧的起始时刻，输出imuVeloFromStartCur；

TransformToStartIMU：点云坐标系转换，zxy旋转过去，再yxz旋转回来；

ShiftToStartIMU：位移转换，yxz旋转，输出imuShiftFromStartCur；（貌似没有调用！！，但是后面又用到了imuShiftFromStartCur数据）；

==》处理完毕之后，重新保存进segmentedCloud；

2、updateInitialGuess：利用imuAngularFromStart-姿态、imuVeloFromStartCur-积分得位移，给予初始猜想值transformCur；

3）帧间匹配完成阶段

Core函数：integrateTransformation

integrateTransformation：得到上一步帧间匹配的输出transformCur，调用一下2个函数，输出前端里程计的最终结果transformSum；

AccumulateRotation：transformXX的0~2是旋转分量，3~5是位移分量。其中transformSum是在

checkSystemInitialization函数里初始化了[0][2]两个角度。该函数主要是计算当前帧终点相对于世界坐标系的欧拉角，即上一帧的位姿结合当前帧的运动量，累加计算出来当前帧的世界坐标系位姿，yxz旋转得到tx、ty、tz的位移输出。

PluginIMURotation：将上个函数计算的欧拉角rx、ry、rz，累加上imu的旋转变化量，完成最终的姿态更新；

这两个函数推导参考：LOAM中关于坐标转换与IMU融合_chengwei0019的博客-CSDN博客_imu去除重力加速度

第二部分在mapOptmization.cpp里面

4）scan2map部分

Core函数: transformUpdate

transformUpdate: 在函数scan2MapOptimization里调用，只使用imu的imuRoll和imuPitch。在后端优化完成后，插值得到对应时间的imu数据，采用一阶低通滤波器融合后端优化的输出结果transformTobeMapped与imu的imuRoll角和imuPitch角。

最后，几个变量的含义说一下：


map.cpp里transformAssociateToMap函数下的意义：
 * transformSum: k时刻激光里程计的位姿
 * transformBefMapped: k-1时刻激光里程计的位姿
 * transformAftMapped: k-1经过mapping优化的位姿(scan2map)
 * transformTobeMapped: 利用 激光里程计 估计的AfteredMapped的位姿, 估计值