【PLIO学习总结】laserMapping中的时间戳与状态更新逻辑

本文仅用于个人学习总结记录。如有错误，请批评指正。

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 

作者注释了“大问题”，可能是一些功能增删造成的混乱吧。但应该不影响上面部分的大逻辑。