专栏首页科学计算VINS后端非线性优化目标函数

VINS后端非线性优化目标函数

VINS后端非线性优化目标函数

1. 状态变量

 vins在后端优化中,使用了滑动窗口,其状态向量包含窗口内的n+1个相机的状态(位置,旋转,速度,加速度计bias及陀螺仪bias)、相机到imu的外参、m+1个路标点的逆深度:

X=\left[x_{0}, x_{1}, \cdots x_{n}, x_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right]
x_{k}=\left[p_{b_{k}}^{w}, v_{b_{k}}^{w} q_{b_{k}}^{w}, b_{a}, b_{g}\right]
x_{c}^{b}=\left[p_{c}^{b}, q_{c}^{b}\right]

2.代价函数

 我们建立后端需要优化的代价函数:

\min _{X}\left\{\left\|r_{p}-H_{p} X\right\|^{2}+\sum_{k \in B}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)\right\|_{P_{k+1}^{b_{k}}}^{2}+\sum_{(l, j) \in C}\left\|r_{C}\left(\hat{z}_{l}^{C_{j}}, X\right)\right\|_{P_{l}}^{2} c_{j}\right\}

 代价函数中的3个残差项分别对应边缘化先验信息,IMU残差,视觉重投影残差,需要注意的是,三种残差都是使用马氏距离进行表示的(相比欧式距离,多了协方差矩阵)。

 我们将IMU残差使用最小二乘法进行求解,当优化变量产生增量后,则有以下公式:

\min _{\delta X}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X+\delta \mathrm{X}\right)\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}=\min _{\delta X}\left\|r_{B}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)+H_{b_{k+1}}^{b_{k}} \delta \mathrm{X}\right\|_{P_{b_{k+1}}^{b_{k}}}^{2}

 其中

H_{b_{k+1}}^{b_{k}}

为相应Jacobian表达式,我们使用马氏距离公式对上式进行展开,并且求导,令导数为0,则可以得到增量的表达式为:

H_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}}{ }^{-1} H_{b_{k+1}}^{b_{k}} \delta \mathrm{X}=-H_{b_{k+1}}^{b_{k}}{ }^{T} P_{b_{k+1}}^{b_{k}}{ }^{-1} r_{B}

 由此递推,我们可以得到代价函数的展开式:

\begin{aligned} \left(\Lambda_{p}\right.&\left.+\sum H_{b_{k+1}}^{b_{k}}{ }^{T} p_{b_{k+1}}^{b_{k}}{ }^{-1} H_{b_{k+1}}^{b_{k}}+\sum H_{l}^{c_{j} T} P_{l}^{c_{j}-1} H_{l}^{c_{j}}\right) \delta \mathrm{X} \\ &=b_{p}+\sum H_{b_{k+1}}^{b_{k}} P_{b_{k+1}}^{b_{k}}{ }^{-1} r_{B}+\sum{ }_{l}^{c_{j}^{T}} P_{l}^{c_{j}^{-1}} r_{C} \end{aligned}

 其中

P_{b_{k+1}}^{b_{k}}

为IMU预积分的协方差矩阵,

P_{l}^{c_{j}}

为视觉观测的协方差矩阵。当IMU的协方差矩阵越大时,其逆越小,说明此时IMU的数据越来越不可靠,我们应该相信视觉的数据。

 我们将上市简化,可以得到后端优化的增量方程:

\left(\Lambda_{p}+\Lambda_{B}+\Lambda_{C}\right) \delta \mathrm{X}=b_{p}+b_{B}+b_{C}

 其中,左侧全部为Hessian矩阵。

2.1 IMU残差

 使用估计值减去测量值,我们可以得到IMU的残差表达式:

r_{B}^{15 \times 1}\left(\hat{z}_{b_{k+1}}^{b_{k}}, X\right)=\left[\begin{array}{c} \delta \alpha_{b_{k+1}}^{b_{k}} \\ \delta \theta_{b_{k+1}}^{b_{k}} \\ \delta \beta_{b_{k+1}}^{b_{k}} \\ \delta b_{a} \\ \delta b_{g} \end{array}\right]= \left[\begin{array}{c} R_{w}^{b_{k}}\left(p_{b_{k+1}}^{w}-p_{b_{k}}^{w}-v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)-\alpha_{b_{k+1}}^{b_{k}} \\ 2\left[{\gamma_{b_{k+1}}^{b_{k}}}^{-1} \otimes {q_{b_{k}}^{w}}^{-1} \otimes q_{b_{k+1}}^{w}\right]_{x y z} \\ R_{w}^{b_{k}}\left(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w} \Delta t_{k}\right)-\beta_{b_{k+1}}^{b_{k}} \\ b_{a_{b_{k+1}}}-b_{a_{b_{k}}} \\ b_{\omega_{b_{k+1}}}-b_{\omega_{b_{k}}} \\ \end{array}\right]

 上述各增量对bias的Jacobian可从IMU预积分的大Jacobian公式相应位置获取。

 我们对下面的优化变量开始求导:

\left[\delta p_{b_{k}^{\prime}}^{w} \delta \theta_{b_{k}}^{w}\right],\left[\delta v_{b_{k}^{\prime}}^{w} \delta b_{a_{k^{\prime}}}, \delta b_{\omega_{k}}\right],\left[\delta p_{b_{k+1}}^{w}, \delta \theta_{b_{k+1}}^{w}\right],\left[\delta v_{b_{k+1}}^{w}, \delta b_{a_{k+1}}, \delta b_{\omega_{k+1}}\right]

 Jacobian具体推导过程比较简单,全部都是导数的基本性质,只有在对旋转求导时较为麻烦,需要通过导数的定义,四元数的定义,左右乘的性质进行推导:

J[0]^{15 \times 7}=\left[\frac{\partial r_{B}}{\partial \delta p_{b_{k}}^{w}}, \frac{\partial r_{B}}{\partial \delta \theta_{b_{k}}^{w}}\right]= \left[\begin{array}{ccc} -R_{w}^{b_{k}} & {\left[R_{w}^{b_{k}}\left(p_{b_{k+1}}^{w}-p_{b_{k}}^{w}-v_{b_{k}}^{w} \Delta t_{k}+\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)\right]^{\wedge}} \\ 0 & -\mathcal{L}\left[q_{b_{k+1}}^{w}-1 \otimes q_{b_{k}}^{w}\right] \mathcal{R}\left[\gamma_{b_{k+1}}^{b_{k}}\right] \\ 0 & {\left[R_{w}^{b_{k}}\left(v_{b_{k+1}}^{w}-v_{b_{k}}^{w}+g^{w} \Delta t_{k}\right)\right]^{\wedge}} \\ 0 & 0 \\ 0 & 0 \end{array}\right]
J[1]^{15 \times 9}=\left[\frac{\partial r_{B}}{\partial \delta v_{b_{k}}^{W}}, \frac{\partial r_{B}}{\partial \delta b_{a_{k}}}, \frac{\partial r_{B}}{\partial \delta b_{w_{k}}}\right]= \left[\begin{array}{ccc} -R_{w}^{b_{k}} \Delta t & -J_{b_{a}}^{\alpha} & -J_{b_{\omega}}^{\alpha} \\ 0 & 0 & -\mathcal{L}\left[q_{b_{k+1}}^{w}{ }^{-1} \otimes q_{b_{k}}^{w} \otimes \gamma_{b_{k+1}}^{b_{k}}]\right. J_{b_{\omega}}^{\gamma}\\ -R_{w}^{b_{k}} & -J_{b_{\omega}}^{\beta} & -J_{b_a}^{\beta}\\ 0 & -I & 0 \\ 0 & 0 & -I \\ \end{array}\right]
J[2]^{15 \times 7}=\left[\frac{\partial r_{B}}{\partial \delta p_{b_{k+1}}^{w}}, \frac{\partial r_{B}}{\partial \delta \theta_{b_{k+1}}^{w}}\right]=\left[\begin{array}{ccc} R_{w}^{b_{k}} & 0 \\ 0 & \mathcal{L}\left[\gamma_{b_{k+1}}^{b_{k}} \otimes q_{b_{k}}^{w-1} \otimes q_{b_{k+1}}^{w}\right] \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \end{array}\right]
J[3]^{15 \times 9}=\left[\frac{\partial r_{B}}{\partial \delta v_{b+1}^{W}}, \frac{\partial r_{B}}{\partial \delta b_{a_{k+1}}}, \frac{\partial r_{B}}{\partial \delta b_{w_{k+1}}}\right]=\left[\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ R_{W}^{b_{k}} & 0 & 0 \\ 0 & I & 0 \\ 0 & 0 & I \end{array}\right]

2.2 视觉残差

 视觉主要是利用重投影,建立残差,我们直接给出视觉残差公式,推导公式比较简单,利用前后两帧的坐标系关系即可建立等式,不再赘述:

P_{l}^{c_{j}}=R_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{w}\left(R_{c}^{b} \frac{1}{\lambda_{l}} \bar{P}_{l}^{c_{i}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]-p_{c}^{b}\right\}

 我们对以下变量进行求导:

\left[\delta p_{b_{i}}^{w}, \delta \theta_{b_{i}}^{w}\right],\left[\delta p_{b_{j}^{\prime}}^{w}, \delta \theta_{b_{j}}^{w}\right],\left[\delta p_{c}^{b}, \delta \theta_{c}^{b}\right], \lambda_{l}

 Jacobian具体推导过程不再给出,主要利用了四元数的右乘扰动性质,并且忽略高阶导数。具体表达式如下:

J[0]^{3 \times 6}=\left[\frac{\partial r_{C}}{\partial \delta p_{b_{i}}^{w}}, \frac{\partial r_{C}}{\partial \delta \theta_{b_{i}}^{w}}\right]=\left[R_{b}^{c} R_{w}^{b_{j}} \quad-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w}\left(R_{c}^{b} \frac{1}{\lambda_{l}} \bar{P}_{l}^{c_{i}}+p_{c}^{b}\right)^{\wedge}\right]
J[1]^{3 \times 6}=\left[\frac{\partial r_{C}}{\partial \delta p_{b_{j}}^{w}}, \frac{\partial r_{C}}{\partial \delta \theta_{b_{j}}^{w}}\right]=\left[-R_{b}^{c} R_{w}^{b_{j}} \quad R_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{w}\left(R_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]\right\}^{\wedge}\right]
J[2]^{3 \times 6}=\left[\frac{\partial r_{C}}{\partial \delta p_{c}^{b}}, \frac{\partial r_{C}}{\partial \delta \theta_{c}^{b}}\right]==\left[\begin{array}{c} R_{b}^{c}\left(R_{w}^{b_{j}} R_{b_{i}}^{w}-I_{3 \times 3}\right) \\ -R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b}\left(\frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}}\right)^{\wedge}+\left(R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}}\right)^{\wedge}+\left\{R_{b}^{c}\left[R_{w}^{b_{j}}\left(R_{b_{i}}^{w} p_{c}^{b}+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right)-p_{c}^{b}\right]\right\}^{\wedge} \end{array}\right]^{T}
J[3]^{3 \times 1}=\frac{\partial r_{C}}{\partial \lambda_{l}}=-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b} \frac{\bar{P}_{l}^{c_{i}}}{\lambda_{l}^{2}}

 视觉协方差公式:

\Omega_{\text {vis }}=\Sigma_{\text {vis }}^{-1}=\left(\frac{1.5}{f} I_{2 \times 2}\right)^{-1}=\frac{f}{1.5} I_{2 \times 2}

2.3 边缘化和舒尔补公式

 VINS中使用的边缘化为传统的边缘化策略,当有新的帧进来的时候,我们希望删除最老的帧或者次新帧,不希望对这一帧的位姿及路标点再次进行计算,减少计算量,但我们不能直接删除,否则会破坏约束关系,导致求解崩溃,因此我们通过舒尔补公式,保留需要marg那一帧的约束关系。我们通过公式进行说明,将非线性优化公式

H \delta x=b

改写为:

\left[\begin{array}{cc} \Lambda_{a} & \Lambda_{b} \\ \Lambda_{b}^{T} & \Lambda_{c} \end{array}\right]\left[\begin{array}{l} \delta x_{a} \\ \delta x_{b} \end{array}\right]=\left[\begin{array}{l} g_{a} \\ g_{b} \end{array}\right]

 其中,

x_a

x_b

分别为我们需要merg掉的变量与需要保留的变量,使用舒尔补进行消元:

\left[\begin{array}{cc} I & 0 \\ -\Lambda_{b}^{T} \Lambda_{a}^{-1} & \mathrm{I} \end{array}\right]\left[\begin{array}{cc} \Lambda_{a} & \Lambda_{b} \\ \Lambda_{b}^{T} & \Lambda_{c} \end{array}\right]\left[\begin{array}{l} \delta x_{a} \\ \delta x_{b} \end{array}\right]=\left[\begin{array}{cc} I & 0 \\ -\Lambda_{b}^{T} \Lambda_{a}^{-1} & \mathrm{I} \end{array}\right]\left[\begin{array}{l} g_{a} \\ g_{b} \end{array}\right]
\Leftrightarrow\left[\begin{array}{cc} \Lambda_{a} & \Lambda_{b} \\ 0 & \Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b} \end{array}\right]\left[\begin{array}{c} \delta x_{a} \\ \delta x_{b} \end{array}\right]=\left[\begin{array}{c} g_{a} \\ g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a} \end{array}\right]

 其中,

\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}

就是

\Lambda_{a}

\Lambda_{b}

中的舒尔补项,我们将上式展开得:

\left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right) \delta x_{b}=g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a}

 至此,后端非线性优化的代价函数就全部介绍完成,相应的求导,即Jacobian矩阵也全部求解完成,剩下的就需要合理的非线性优化算法根据求得的Jacobian对代价函数进行求解了。

3.边缘化例子

 这里使用一个实例对边缘化进行说明,并且从图表中可以清楚的看到矩阵的变化情况,稠密或稀疏。我从网上找了一张示意图,读者请务必理解下图的变化过程。

 假设有4个相机位姿,其看到了6个路标点,即特征点,相机与路标点之间表示观测,相机与相机之间表示IMU约束:

 下图为上图的详细表示,每一个方块都代表对应的舒尔补项:

 然后我们现在marg掉

x_{p_1}

,则上图变化如下:

 我们可以看到,与

x_{p_1}

相关的都从矩阵中移除了,到了左上方,留下的矩阵即

\Lambda_c

,我们计算

\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}

(黄色),然后得到

\left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right)

的示意图:

 我们将上图转成关系图后,如下,可以发现,约束关系变了。

 在此基础上,我们merg掉

x_{m_1}

后的矩阵如下:

 对应的关系为:

 从这里我们可以看出,如果不想让矩阵变得稠密,只需要merg掉那些不被观测的路标点即可。

本文分享自微信公众号 - 科学计算technomania(Quant_Times),作者:大亮

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2021-04-29

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • SLAM后端:非线性优化

     我们要求其最小值,当然是对目标函数进行求导,但通常目标函数是非线性的,因此我们需要通过以下步骤对目标函数进行求解:

    猫叔Rex
  • PL-VINS:实时基于点线的单目惯导SLAM系统

    标题:PL-VINS: Real-Time Monocular Visual-Inertial SLAM with Point and Line

    点云PCL博主
  • 计算机视觉方向简介 | 视觉惯性里程计(VIO)

    Visual-Inertial Odometry(VIO)即视觉惯性里程计,有时也叫视觉惯性系统(VINS,visual-inertial system),是融...

    3D视觉工坊
  • 论文简述 | PL-VINS:具有点和线特征的实时单目视觉惯性SLAM

    利用线特征来提高基于点的视觉惯性定位系统(VINS)的定位精度越来越受到关注,因为它们对场景结构提供了额外的约束.然而,在VINS整合线特征时的实时性尚未得到解...

    计算机视觉
  • 解放双手——相机与IMU外参的在线标定

    在SLAM的众多传感器解决方案中,相机与IMU的融合被认为具有很大的潜力实现低成本且高精度的定位与建图。这是因为这两个传感器之间具有互补性:相机在快速运动、光照...

    用户1150922
  • 【SLAM】开源 | 港科大--VINS-Mono:基于视觉和IMU的紧耦合SLAM算法,VIO领域必读!

    论文地址: https://arxiv.org/pdf/1708.03852.pdf

    CNNer
  • vSLAM开发指南:从技术框架、开源算法到硬件选型!

    出品 | 智东西公开课 讲师 | 小觅智能 CTO 杨瑞翾 编辑 | 王鑫

    小白学视觉
  • 一文详解回环检测与重定位

    标题:VINS-Mono代码解读—回环检测与重定位 pose graph loop closing

    3D视觉工坊
  • 一文详解非线性优化算法:保姆级教程-基础理论

    不论是刚入门SLAM的小白,还是导航相关专业的同学,都对“非线性优化”这个词不陌生,如果你说你没听过这个词,那“因子图”一词总该略有耳闻吧,如果还是不知道,那就...

    3D视觉工坊
  • 视觉/视觉惯性SLAM最新综述:领域进展、方法分类与实验对比

    Visual and Visual-Inertial SLAM: State of the Art, Classification,and Experiment...

    3D视觉工坊
  • 视觉/视觉惯性SLAM最新综述:领域进展、方法分类与实验对比

    Visual and Visual-Inertial SLAM: State of the Art, Classification,and Experiment...

    计算机视觉
  • 一文详解非线性优化算法:保姆级教程-基础理论

    不论是刚入门SLAM的小白,还是导航相关专业的同学,都对“非线性优化”这个词不陌生,如果你说你没听过这个词,那“因子图”一词总该略有耳闻吧,如果还是不知道,那就...

    计算机视觉
  • 【SLAM】开源 | PL-VINS:对VINS添加了直线特征的约束条件,在保证实时的前提下定位误差比VINS降低12%-16%

    论文地址: http://arxiv.org/pdf/2009.07462v2.pdf

    CNNer
  • 【VINS论文笔记】系列之回环检测与重定位

    本文仅做学术分享,已获得作者授权转载,未经允许请勿二次转载!欢迎各位加入免费知识星球,获取PDF文档,欢迎转发朋友圈,分享快乐。

    点云PCL博主
  • MSCKF理论推导与代码解析

    在SLAM后端中,主要有两种主流方法用于优化:基于滤波的方法和基于非线性的方法。基于滤波的方法主要有MSCKF、S-MSCKF、ROVIO等,基于非线性的方法主...

    计算机视觉
  • MSCKF理论推导与代码解析

    在SLAM后端中,主要有两种主流方法用于优化:基于滤波的方法和基于非线性的方法。基于滤波的方法主要有MSCKF、S-MSCKF、ROVIO等,基于非线性的方法主...

    3D视觉工坊
  • 一种查看Block中引用的所有外部对象的实现方法

    在我的前一篇文章:iOS调试Block引用对象无法被释放的一个小技巧 中有介绍一种显示某个block对象的实现函数的方法,以及从Debug Memory Gr...

    欧阳大哥2013
  • 一文详解SLAM的主要任务和开源框架

    SLAM是Simultaneous localization and mapping缩写,意为“同步定位与建图”.

    3D视觉工坊
  • 用于机器人导航辅助的6自由度姿态估计的平面辅助视觉惯性里程计

    Plane-Aided Visual-Inertial Odometry for 6-DOF Pose Estimation of a Robotic Navi...

    计算机视觉

扫码关注云+社区

领取腾讯云代金券