LIO-SAM中mapOptmization.cpp中的updateInitialGuess()理解

可能是本人不太聪明的原因，在看到这个位姿初始化函数内容的时候，我卡了很久，总感觉乱七八糟的，不过看了很多大佬的博客以后再加上多花时间，最后终于对这个函数有了一个粗略的认知，在这里写一下个人理解，有错误望批评指正，谢谢大家。

正文：

首先我们知道，这个函数是在回调函数laserCloudInfoHandler()中的，订阅的是作者自定义的一个cloud_info数据类型的数据，这里面包含了imu的原始数据（cloudInfo.imuRollInit等）和imu里程计信息（cloudInfo.initialGuessX等），我们就通过这些信息来对lidar位姿进行初始化。

在讲这个函数之前，我们首先理解一下这个函数中经常出现的一些变量所代表的含义：1.transformTobeMapped：保存每一帧实时位姿的一个容器（XYZ,RPY）

2.lastImuTransformation：保存imu旋转角，主要用在前两帧

3.lastImuPreTransformation：保存imu里程计，主要用于第三帧数据之后

然后我们再来理清一个事实：第一帧cloud_info数据进来的时候，是没有imu里程计信息的，因为我们可以发现，cloud_info是来源于imageProjection的，而imageProjection的imu里程计信息是来源于imuPreintegration的，但是imuPreintegration想要得到高频的imu里程计信息，是需要来自mapOptmization的低频激光里程计的，绕了一圈又回到了mapOptimzation()中，所以，第一帧cloud_info数据进来的时候，是没有imu里程计信息的。

进入函数updateInitialGuess()：

  1、第一帧数据进来的时候，关键帧集合为空，就用cloud_info中imu原始数据对transformTobeMapped位姿容器进行初始化，然后用lastImuTransformation保存此时的（第一帧）imu原始旋转角，接着return

2、第二帧数据进来的时候（此时第二帧已经有了imu里程计数据），用transBack保存此时的imu里程计信息，然后将transBack赋值给lastImuPreTransformation（lastImuPreTransformation也保存了imu里程计信息），但是此时其实不管是transBack也好还是lastImuPreTransformation也罢，在第二帧数据传进来的过程中都没有用到（在处理第三帧的时候，它们作为第二帧（第三帧的上一帧）才用到）

 跳过else直接看后面：我们用transBack来保存第二帧的imu原始旋转角，还记不记得我们在处理第一帧数据的时候用lastImuTransformation保存第一帧imu原始旋转角，因此我们已知第一帧和第二帧旋转角，就可以得到第一帧到第二帧的旋转增量transIncre，然后将transIncre作用于第一帧的初始位姿transformTobeMapped上，得到第二帧的初始位姿矩阵transFinal，然后通过函数pcl::getTranslationAndEulerAngles提取得到第二帧的初始位姿transformTobeMapped（XYZ,RPY），然后依旧用lastImuTransformation保存第二帧的imu原始旋转角。

 3、第三帧数据进来我们用transBack来保存第三帧imu里程计（lastImuPreTransAvailable是一个静态变量，初始被设置为false,之后就变成了true且保持不变）

 还记不记得在第二帧数据进来以后，我们用lastImuPreTransformation也保存了第二帧imu里程计信息（不记得的可以回过头去看看），因此第二帧里程计信息和第三帧imu里程计信息我们都知道了，因此我们就可以得到第二帧到第三帧的imu里程计增量transIncre，然后将其作用于第二帧初始位姿transformTobeMapped上，就可以得到第三帧的位姿矩阵transFinal上，通过函数pcl::getTranslationAndEulerAngles提取得到第三帧的初始位姿transformTobeMapped（XYZ,RPY），然后依旧用lastImuTransformation保存第三帧的imu原始旋转角。

 第四帧之后就跟第三帧的步骤是一样的了。