SLAM总结（五）-视觉后端

最新推荐文章于 2024-05-13 14:53:33 发布

loui robot

最新推荐文章于 2024-05-13 14:53:33 发布

阅读量744

点赞数

分类专栏： SLAM 文章标签： slam 计算机视觉机器学习

本文链接：https://blog.csdn.net/weixin_41245988/article/details/115877771

版权

SLAM 专栏收录该内容

11 篇文章 51 订阅

订阅专栏

SLAM总结（五）-视觉后端

1.三角化
2.非线性优化
3.优化库

后端的主要功能是三角化产生新的路标点和进行非线性优化。还要删除冗余关键帧和不好的3D点的操作。非线性优化的重点是设计目标函数。首先，需要确定目标函数中的观测量（接近真实值的），建立待求变量（一般是世界坐标系下机体的位姿或点的3D坐标）与观测量的关系，建立残差项（约束）。优化时需用到残差项对待求变量导数。为了均很计算量和精度，长时间大范围运行时，一种方法是使用滑窗优化滑窗内所有相机位姿和路标点，滑窗之外固定；第二种方法是路标点优化几次后固定，采用位姿图优化方法优化所有相机位姿。第三种方法就是混合使用两种方法，比如ORB-SLAM2中，使用与当前帧的共视关系构建局部窗口，滑窗内与当前帧共视帧超过一个阈值的一部分优化路标点和相机位姿，同时考虑与该窗口内有共视关系的帧的重投影误差，但不优化其相机位姿。
十四讲p255，可考虑用栅格图显示矩阵样式。

1.三角化

与初始化时三角化不同的是，此时的匹配点对应帧的位姿是已知的。

2.非线性优化

通过贝叶斯网络推导出概率模型，再通过将高斯模型取负对数，将问题转换成非线性最小二乘问题。该方程不方便直接（导数为0、解析解）求解，改用迭代的方法求解非线性最小二乘解，关键帧的位姿初值（在世界坐标系）一般由前端给出，路标点位置（在世界坐标系）初值一般在后端通过三角化得到，求解增量的方法由有一阶和二阶梯度法、高斯牛顿法、列文伯格-马夸尔（LM）。BA和增量式优化的目标函数和求解方法相同，区别在于增量式优化通过不同更新策略避免了冗余的计算，获得了与BA相当的精度。

BA（Bundle Adjustment）

迭代步骤如下图所示，迭代方法重要的两个值是初值和每步迭代的增量。迭代的退出条件一般是Δx足够小，但实际中，为了保证实时性，一般会设置最大迭代次数。前端会给出机体位姿的初值，三角化得到路标点的初值。这些初值一般比较准确，优化问题更容易进入全局最优。增量一般使用LM方法求得。一阶梯度法（最速下降）过于贪心，容易走出锯齿路线，反而难收敛，二阶的海塞矩阵H较难求得。
摘自g2o论文
上图中（H+λI）Δx=b的求解过程如下图。设残差项为f(x)。
在这里插入图片描述

其中，H=^T（x_k)J（x_k)，b=-J^T（x_k)f(x_k)，每次迭代都得代入x_k，求得在当前迭代点的J（x_k)和-J^T（x_k)f(x_k)，求取雅可比矩阵可称为线性化。然后便是求解线性方程，可使用SLAM总结（二）提到线性方程求解方法求解。
旋转矩阵处理方法总结：增量加法和求导

目标函数一般由以下残差项（约束）相加构成。首先是构造残差项f（x），然后是求取f(x)对变量x的导数J(x)，最后求解线性方程的方法，根据H矩阵的稀疏性和特点需选择合适的方法。
1)重投影残差项：变量是从世界坐标系到第i帧关键帧的6个自由度位姿T_iw和第k个路标点在世界坐标系3个自由度位置P_k[X,Y,Z]^T。观测是第k个路标点在第i帧和第j帧的像素坐标z_ik和z_jk。在假设共有n帧关键帧位姿和m个路标点位置待优化。变量表示如下，是6n+3m维列向量：
x=[T_1w,T_2w … T_nw,P₁，P₂ … P_m]^T
a）使用三维坐标表示位置时，由把所有有投影关系的路标点和关键帧构成残差项，第k个路标点在第i帧
e_ik=z_ik-KT_iwP_k
求导

f(x)最高是2nm维的列向量，但是由于每个路标点只会被一部分关键帧观测到，没有投影关系的残差项可以省略，故维度远小于2nm
f(x)=[e₁₁,e₁₂，… e_1m,e₂₁,e₂₂,… e_2m,…e_nm]^T
目标函数为：
min||f(x)||²=Σⁿ_i=1Σ^m_k=1||e_ik||²(只包含有投影关系）
f(x)对x求导得,J(x)行数与f(x)一样，列数为6n+3m：
J(x)=[J₁₁,J₁₂，… J_1m,J₂₁,J₂₂,… J_2m,…J_nm]^T
在这里插入图片描述
求系数矩阵H=^T（x_k)J（x_k)，H矩阵中B为J相机列转置与相机列两两相乘的结果，不同列相同行至少有一个不为0，故结果为0。H矩阵中C为J路标列转置与路标列两两相乘的结果，不同列相同行至少有一个不为0，故结果为0。E为相机列转置与路标列相乘的结果，当某个路标被相机观测到时对应位置不为0
在这里插入图片描述
由SLAM总结（二）shur消元（边缘化方法还有cholesky分解等）只知主要计算量在：
（B-EC^-1E^T)Δx=v-EC^-1w，B-EC^-1E^T是否为对称矩阵等价于C^-1是否为对称矩阵？
EC^-1E^T的非零块与EE^T的非零块一样，则B-EC^-1E^T与EE^T一样如下图所示。非零块表示两个相机有至少一个共视路标。另外，B-EC^-1E^T矩阵的条件数小于H矩阵，即H矩阵比B-EC^-1E^T更病态。
在这里插入图片描述

b）逆深度表示位姿时

2)相对位姿残差项（位姿图优化）：随着时间增长路标点会不断增多，优化变量会快速增多。比如每新增一帧关键帧会增加100个新的特征点，那么路标点维数未3*100，是一个6自由度的相机位姿的50倍。因为路标点优化几次后趋向于收敛，继续优化精度提高不大，反而会大大增加计算量，所以可以固定路标点，只优化位姿。但这里不再使用重投影误差，而是使用相对位姿残差。首先待优化的是相机在世界坐标系的位姿T_wci，长时间的误差累积它是漂移的，但短时间相对位姿T_cicj可认为是准确的，可构成残差项：
e(ij)=T_wcjT_wci^-1T_cicj,理想情况下，e(ij)等于单位矩阵I
位姿图边一般分为里程计（相邻）边和回环边。如果都是里程计边，H矩阵是稀疏的，是对角块矩阵；但若回环边较多，则H矩阵不再稀疏，且无规律可循，但有回环边能更快更好地减少累积误差，更容易收敛。只有里程计边时能否将有噪声的结果拉回正确值，可承受多大噪声。可先得到正确轨迹，然后加上带累积漂移的噪声试试。
3)光度残差项：

因子图

因子图地节点是变量节点，与贝叶斯相比，不包含给定的控制量u和观测量z。因子是指变量之间的关系，与某个因子有关系的变量节点会连接到该因子上，有关系即变量直接或间接被包含在因子的式子中，可以通过边缘化等一些手段改变因子来改变连接关系。因子图优化就是优化各变量使得所有因子的乘积最大化。和贝叶斯一样，通过高斯乘积通过取负对数最小化可以将概率问题转换成最小二乘问题，最后使用LM等非线性迭代方法求解。