这篇文章将向大家简要介绍FAST-LIVO2开源系统中的VIO部分实现的思路，具体的较为细节的内容在参考引文的链接里，这里对相关思路做一下汇总说明。VIO的实现采用直接法视觉里程计（Direct Visual Odometry）的核心概念，用光度残差衡量同一3D点在当前帧与参考帧的像素亮度差异。
在该系统的VIO算法中，基于ESKF（error state kalman filter）的误差测量方程的相关公式定义和基于光度残差和状态参数（误差状态，如基于旋转的李代数扰动量，平移噪声等）的雅可比矩阵的计算及推导参见[1]，其中链式计算中的图像梯度的具体计算方法参考[2]。ESKF的状态更新公式及相关代码说明参考[3]。其中需要说明的是每一个可视点的测量光度误差的计算时，其参考图像帧可能不属于同一个参考帧（每个点云点根据光度一致性，几何约束或其他等条件选择最优的参考帧），而ESKF的每一次迭代都是基于当前的所有的用来优化位姿的地图点集合的光度误差总和进行的。具体的可以参考代码[4]中的详细实现。
函数retrieveFromVisualSparseMap基于当前处理逻辑点云帧（经时间对齐在原始点云帧基础上处理过的点云集合）中的点云集合信息动态提取当前帧可见的特征点，并构建高质量的局部子地图(visual_submap)，其中稀疏地图中的可视点会基于点的多个观测，选择最优的patch参考（不同点可能会选择不同的参考图像帧），并根据姿态变换对图像做出对应的放射变换以便后续的光度残差的计算，ESKF基于子地图的可视点的观测状态（光度残差）和IMU的位姿误差运动方程紧耦合优化位姿；
问题：vio中关于地图的构建思路，和lidar的体素地图的关系？
回答：局部地图visual_submap主要用于优化位姿，在函数retrieveFromVisualSparseMap中构建局部稀疏地图的开始时进行重置，而全局地图feat_map为基于体素索引值和视觉点（VisualPoint）集合的hash表结构(unordered_map)。
References
- [1]、基于光度残差的误差测量状态方程的相关量推导过程： documents/phtometric_errors_jacobian.md
- [2]、图像梯度的计算方法： documents/dudv_calc.md
- [3]、VIO误差测量的定义： documents/VIO.md
- [4]、ESKF的优化代码实现：https://gitee.com/kindlytree/fast-livo2-comments/blob/main/src/vio.cpp#L1618
Leave a Reply