本文仅用于个人学习总结记录。如有错误，请批评指正。
0、PLIO简要思路
从PLIO的论文中，可以知道，完整的PLIO算法采用IMU和LiDAR数据同时作为“输入”，维护状态变量包括加速度和角速度。
同时，PLIO是一种distortion-free的方法，即不需要进行去畸变。
之所以是不需要去畸变，是因为对每个雷达点都进行状态更新，然后将lidar点投回世界系作为地图点。
有了这个基本概念，我们就开始看laserMapping中main函数中的处理流程。
本文按照“IMU作为output”，即IMU不仅用于预测，也具有观测模型。此时，use_imu_as_input为false，imu_en为true。
1、不同时刻的更新逻辑
对于一个LIO系统，同时存在LiDAR和IMU数据，下图画出了对齐以后的两个时间轴。
可以看出，一次lidar的scan之间有多个绿色的雷达点，同时也有多个IMU测量。
两个IMU的测量间，可能有多个LiDAR pint；
两个LiDAR point间，也可能有多个IMU（虽然我不知道为啥，但代码的逻辑是这么处理的。实测发现，基本上两个Point之间只会出现“1个”IMU）
在代码的sync_packages时，把LiDAR和IMU数据进行了打包，保证了在后续处理时，永远是一个完整的scan中LiDAR Point
和IMU数据。
那么，在收到一个IMU或一个LiDAR时，理论上都进行状态更新。
但是，在代码实现中，永远是在每个LiDAR Point到来时进行更新。只不过，如果这个point到来时，buffer中下一个IMU数据如果是这个point之前，则需要用IMU的数据进行更新；否则，只更新LiDAR Point这部分。
1.1 无IMU数据
这里我们把这种情况叫做 Case 1，具体的示意图如下：
即当前时间是一个lidar point对应的时刻，前一次状态更新时刻time_predict_last_const和当前时刻time_current之间没有插入IMU数据。
此时，需要做的事情为：
- 根据上一次IMU的输入，进行预测操作，预测状态量，和对应的协方差；
- 根据LiDAR的观测和残差，更新状态量和协方差。
1.2 有IMU的数据
这里我们把这种情况叫做 Case 2，具体的示意图如下：
这就稍微有一些复杂了，因为上一个“状态完整更新”时刻应该是上一个Point，但当前的Point和前一个Point之间，有了多个IMU数据（一般是1个）。
此时，做的事情包括：
- 对第1个IMU时刻，计算上一个Point到这个IMU的时间间隔dt，然后预测新的状态，和协方差；
- 利用第1个IMU的数据，观测模型，计算残差，并进行更新全部状态和协方差；
- 如果有多个IMU，则重复上面两个步骤。注意，重复时，时间间隔为距离上一个IMU测量的时间间隔，而不是到前一个LiDAR Point的；
- 完成所有IMU时刻的预测和更新之后，处理新的LiDAR Point的数据，此时再执行1.1里面的两步：计算当前Point到前面最近IMU的时间间隔dt，预测状态和协方差；根据LiDAR的数据进行更新。
1.3 完整的时间戳示意图
2、代码
捋清楚上面的过程，就可以看代码了。这里，只保留代码的框架。
// IMU作为状态量进行更新
if (!use_imu_as_input){
// 对所有（时间压缩后的）雷达点进行更新
for (k = 0; k < time_seq.size(); k++)
{
PointType &point_body = feats_down_body->points[idx+time_seq[k]];
time_current = point_body.curvature / 1000.0 + pcl_beg_time; // 获取当前雷达点时间戳
if(imu_en)
{
bool imu_comes = time_current > imu_next.header.stamp.toSec();
// 是否有了新的IMU数据？如果有，执行下面while，即case2中所说的内容；如果没有，跳过这部分，只是简单的case1
while (imu_comes)
{
imu_next = *(imu_deque.front());
// 获得buffer中的IMU，这个IMU的时间戳处于“上一次更新”和“当前雷达点”时间戳之间
double dt = imu_last.header.stamp.toSec() - time_predict_last_const;
kf_output.predict(dt, Q_output, input_in, true, false); // case2中，预测到当前IMU时刻时，状态的预测；
double dt_cov = imu_last.header.stamp.toSec() - time_update_last; // 问题：
if (dt_cov > 0.0)
{
kf_output.predict(dt_cov, Q_output, input_in, false, true); // case2中，预测到当前IMU时刻时，协方差的状态；
kf_output.update_iterated_dyn_share_IMU(); // case2中，IMU进行update
}
}
}
double dt = time_current - time_predict_last_const;
kf_output.predict(dt, Q_output, input_in, true, false); // case1，lidar点到前一个预测时刻，状态的预测
time_predict_last_const = time_current;
if (!kf_output.update_iterated_dyn_share_modified()) // case1，lidar进行update
{
idx = idx+time_seq[k]; // 获取下一个 Lidar Point的索引
continue;
}
}
}
3、遗留问题
现在还遗留了一个问题，就是代码中有两个变量，记录上一次XXX的时间：
time_predict_last_const 和 time_update_last。
从名字上看，前者是“上一次predict的时刻”，后者是“上一次update的时刻”。二者什么区别么？
在代码中，二者的区别体现在
if(!prop_at_freq_of_imu)
{
double dt_cov = time_current - time_update_last;
if (dt_cov > 0.0)
{
kf_output.predict(dt_cov, Q_output, input_in, false, true);
time_update_last = time_current; // 这里更新了“上一次更新时间戳”
}
}
kf_output.predict(dt, Q_output, input_in, true, false);
time_predict_last_const = time_current; // 这里更新了“上一次预测时间戳”
即，如果prop_at_freq_of_imu是false的，两者不一样；
但是，目前版本的代码中，prop_at_freq_of_imu一直是true，因此，这里时间戳有一些的混乱。
所以，在作者的代码中，有这么一行：
time_predict_last_const = imu_last.header.stamp.toSec(); // big problem
作者注释了“大问题”，可能是一些功能增删造成的混乱吧。但应该不影响上面部分的大逻辑。
