Fastlivo2

误差迭代卡尔曼滤波器（esikf）

“智能纠错员”和“迭代优化大师”

在一个slam系统中，对于机器人的位姿会有一个估计值，但是这个估计值会有误差，同时传感器也有测量位置，但是传感器也会有误差。

esikf作用就时把不太准确的预估值和带噪声的测量值结合起来算出一个个表格靠谱的真实位置。

与其他的卡尔曼滤波的不同是，普通卡尔曼滤波计算的是机器人的真实位置，这里的卡尔曼滤波计算的是误差，意思就是它假设这个机器人有一个标称状态，然后只去估计整个标称状态和真实状态之间的误差。

1.数据同步：扫描重组

激光雷达的点是顺序连续输出的，但是相机是离散拍摄的这里的作用就是把从tk-1到tk之间的雷达点都统一到tk时刻用于和tk时刻的相机同步。

imu前向传播：

在tk-1到tk时刻新的激光雷达和项技术数据未到达，需要持续估计状态，利用imu数据和运动学模型，预测书tk时刻的状态x和协方差矩阵P，作为后续的激光列的和视觉更新的初始估计。

反向传播，运动畸变补偿：

得到tk时刻的优化状态估计（imu前向传播得到的）后反向传播到开始时刻，重新计算每个点对应的位姿将所有的点统一矫正到tk时刻的坐标系下。

2.顺序更新

先用 LiDAR 搭好 “几何骨架”，再用视觉填 “纹理细节

第一步雷达更新：

先进行点云投射把反向传播得到的雷达点通过当前的迭代状态投影到全局坐标系，得到全局坐标。找每个点的匹配的平面，计算点到平面的残差（就是点到平面的距离），迭代优化对残差求导得到雅可比矩阵，然后通过卡尔曼增益公式计算最优的更新量，然后通过这个更新量更新下一次的状态。

第二部视觉更新：

先选可用的地图点，从体素地图中依傲轩能用的地图点（原则是先选雷达点所在的体素，去掉遮挡的点，去掉相邻点深度查太大的点），然后进行多尺度迭代优化这里它构建了一个三层金字塔从粗到细进行优化。对于每一层计算光度误差，每个视觉地图点都带有一个参考补丁（一个像素块），把这个补丁投影到相机图像上计算相机图像和这个补丁中的像素灰度值的差，后面还是对这个差求雅可比矩阵进而求卡尔曼增益来更新状态。

最后输出最后的状态当两部更新完成之后，输出最终预测，留作下次imu前向传播的初始状态，和更新体素地图的视觉信息。

按需光线投射，主要是用来填补感知空白的关键机制。只在当前图像中确实缺少世界地图点的区域才进行光线投射查询，以寻找可能被遗漏的可见地图点。

主要用于，雷达近距离盲区，相机与雷达视野差异，单一平面场景（比如隧道、长走廊），LiDAR 只能检测到少量平面点。

先将之前雷达选出的地图点投影到当前相机图像上并按照30*30的像素划分图像为均匀网格，再检查每个网格如果没有已经选视觉地图点，说明这里雷达覆盖不到

从相机像素反向发射光线，就是从当前像素点平行光轴的一条曲线，然后在这条线上按照相应深度设置采样点，然后对每个采样点查询他们在体素地图中对应的体素，如果这个体素存在之前雷达构建的平面上，说明这个采样点是有效的。就借用这个点来作为当前的新的地图点。这里还要进行处理深度差异。