VI-SLAM的边缘化管理与问题修复

问题描述

在实现在线估计路标点的VI-SLAM时，遇到不一致性的问题，位姿的协方差估计偏低。

---

原因分析

在执行边缘化时，为了降低信息矩阵的稠密度，只剔除仅被第一帧观测的路标点以及第一帧，其余路标点均保留。具体而言，首先找出第一帧观测到的路标点，分为即将被边缘化的点和被保留的点。执行舒尔补操作后，被保留的点成为下一轮优化时的先验信息。

如图所示，$p_1$和$l_1$被边缘化得到新的因子图，$l_2$、$l_3$被保留在先验信息中。在下一轮构建先验信息矩阵时，由于前述的策略，只有$l_2$、$l_4$被选入了状态向量中，而$l_3$与其他变量在本轮先验信息中的关系则被忽略（$l_3$-$p_2$、$l_3$-$l_2$），这导致边缘化后只存在$l_4$与$p_3$的错误先验信息。

---

解决方案

1. 在构建边缘化需要的先验信息矩阵$\Lambda =\left[\begin{array}{cc}{\Lambda }_{mm}& {\Lambda }_{mr}\\ {\Lambda }_{rm}& {\Lambda }_{rr}\end{array}\right]$时，将带有先验信息的路标点和新观测到的路标点都要考虑在内。
2. 参考VINS-Mono构建先验信息矩阵的方案，逐个矩阵块地添加到整个的信息矩阵中。

...
Lambda = zeros(2 * stateDim + 3 * length(involvedLandmarks));
b = zeros(2 * stateDim + 3 * length(involvedLandmarks), 1);
% 添加IMU约束信息
...
% 添加Visual约束信息
for i = 1:length(involvedLandmarks)
   lmId = involvedLandmarks(i);
   if ~isempty(find(priorLandmarks == lmId, 1))
       [Error, H_pose, H_lm] = visualMeasurement(lmId, obs, state{1}, landmarks);
       colIdx = 2 * stateDim + 3 * lmId - 2;
       Tk = eye(2);
       Lambda(1:stateDim, 1:stateDim) = Lambda(1:stateDim, 1:stateDim) + H_pose' * (Tk \ H_pose);
       Lambda(1:stateDim, colIdx:colIdx+2) = Lambda(1:stateDim, colIdx:colIdx+2) + H_pose' * (Tk \ H_lm);
       Lambda(colIdx:colIdx+2, 1:stateDim) = Lambda(1:stateDim, colIdx:colIdx+2)';
       Lambda(colIdx:colIdx+2, colIdx:colIdx+2) = Lambda(colIdx:colIdx+2, colIdx:colIdx+2) + H_lm' * (Tk \ H_lm);
       b(1:stateDim, 1) = b(1:stateDim, 1) + H_pose' * (Tk \ Error);
       b(colIdx:colIdx+2, 1) = b(colIdx:colIdx+2, 1) + H_lm' * (Tk \ Error);
   end
end  
% 添加先验信息
...

NOTE: 更好的方式是参照OpenVINS使用位姿与地图点按照自然顺序排列，比如($p_1$, $l_1$, $l_2$, $l_3$, $p_2$, $l_4$, $p_3$)，每一个位姿后伴随着对应的新观测到的地图点，如此在进行路标点管理与边缘化时都更加方便。

---

仿真验证

选用Kaist_vio数据集的圆形轨迹，前100秒的运行场景如下：

位姿的NEES结果如下：