LOAM学习-代码解析（七）融合信息 transformMaintenance

前言

一、初始化

二、位姿转换 transformAssociateToMap

三、接收信息 Handler

四、主函数 main

结语

一些碎碎念

---

前言

在进行完LOAM学习-代码解析（五）地图构建 laserMapping和LOAM学习-代码解析（六）地图构建 laserMapping之后，终于到了LOAM代码解析的尾声，本文将进行最后的transformMaintenance的代码进行解析。

LOAM代码(带中文注释)的地址：https://github.com/cuitaixiang/LOAM_NOTED

LOAM代码(带中文注释)的百度网盘链接：https://pan.baidu.com/s/1tVSNBxNQrxKJyd5c9mWFWw 提取码: wwxr

LOAM论文的百度网盘链接： https://pan.baidu.com/s/10ahqg8O3G2-xOt9QZ1GuEQ 提取码: hnri

LOAM流程：

一、初始化

创建里程计计算的转移矩阵、平移增量、世界坐标系位姿、优化前位姿、优化后位姿态

//odometry计算的转移矩阵(实时高频量)
float transformSum[6] = {0};
//平移增量
float transformIncre[6] = {0};
//经过mapping矫正过后的最终的世界坐标系下的位姿
float transformMapped[6] = {0};
//mapping传递过来的优化前的位姿
float transformBefMapped[6] = {0};
//mapping传递过来的优化后的位姿
float transformAftMapped[6] = {0};

//ros发布、坐标系、里程计、位姿转换
ros::Publisher *pubLaserOdometry2Pointer = NULL;
tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;
nav_msgs::Odometry laserOdometry2;
tf::StampedTransform laserOdometryTrans2;

二、位姿转换 transformAssociateToMap

位姿转换主要是将odometry的运动估计和mapping矫正量融合，主要步骤如下

步骤1：计算两次激光里程计的平移增量transformIncre。由于是基于匀速运动模型的假设，所以运动增量为transformBefMapped[3] - transformSum[3]。由于这两个数组中的位姿态都是基于世界坐标系（/camera_init）下的，所以需要将点云从世界坐标系转换到当前时刻的imu坐标系下，变换矩阵为

步骤2：计算地图map与世界坐标系（/camera_init）的矩阵

最终的旋转矩阵