vins-mono后端优化（1）

陈于落雁

已于 2023-11-20 21:52:32 修改

阅读量554

点赞数 13

分类专栏： vins-mono详解文章标签：人工智能自动驾驶

于 2023-11-14 16:02:43 首次发布

本文链接：https://blog.csdn.net/weixin_44587828/article/details/134349268

版权

vins-mono详解专栏收录该内容

7 篇文章 4 订阅

订阅专栏

前言

在本专栏中已经详细介绍了vins-mono的前端光流和imu预积分，也系统梳理了vins在初始化做的工作。初始化的目的是为了给后端提供一个相对准确的初值，本文详细介绍在后端紧耦合优化中如何将所有的状态量进行求解更新，后端优化函数为void Estimator::optimization()。

在后端优化里程计的约束有视觉重投影误差约束、imu预积分约束和滑窗边缘化约束。此外，在后端优化中还包含时间戳延时TD的优化和回环检测约束。本文主要介绍预积分约束和视觉重投影约束，包括残差的构建和雅克比矩阵的计算。

优化详解

首先要明确后端优化都要优化哪些状态量：

void Estimator::vector2double()
{
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        para_Pose[i][0] = Ps[i].x();
        para_Pose[i][1] = Ps[i].y();
        para_Pose[i][2] = Ps[i].z();
        Quaterniond q{Rs[i]};
        para_Pose[i][3] = q.x();
        para_Pose[i][4] = q.y();
        para_Pose[i][5] = q.z();
        para_Pose[i][6] = q.w();

        para_SpeedBias[i][0] = Vs[i].x();
        para_SpeedBias[i][1] = Vs[i].y();
        para_SpeedBias[i][2] = Vs[i].z();

        para_SpeedBias[i][3] = Bas[i].x();
        para_SpeedBias[i][4] = Bas[i].y();
        para_SpeedBias[i][5] = Bas[i].z();

        para_SpeedBias[i][6] = Bgs[i].x();
        para_SpeedBias[i][7] = Bgs[i].y();
        para_SpeedBias[i][8] = Bgs[i].z();
    }
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        para_Ex_Pose[i][0] = tic[i].x();
        para_Ex_Pose[i][1] = tic[i].y();
        para_Ex_Pose[i][2] = tic[i].z();
        Quaterniond q{ric[i]};
        para_Ex_Pose[i][3] = q.x();
        para_Ex_Pose[i][4] = q.y();
        para_Ex_Pose[i][5] = q.z();
        para_Ex_Pose[i][6] = q.w();
    }

    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

在后端优化前，需要将所有待优化变量转成double形式，由上面函数可以看到，需要进行优化的变量有滑窗内的位姿（7维）、速度（3维）和偏置（6维），3d点的逆深度（1维）以及时间延时td（1维）。

准备工作

在正式进行优化之前，需要对特征点管理器中的没有进行三角化的点三角化，三角化的点以第一次观测到该点的图像帧所在的相机系为参考系。

构建优化

在优化问题中需要添加参数块和构建问题对象等操作，在之前的ceres进行优化详解中已经详细介绍了使用ceres优化的步骤，这里不对细节问题赘述，主要对残差函数构建和雅克比矩阵求解进行梳理。ceres使用可以参见下文：

vins-mono中的初始化详解（2）-CSDN博客文章浏览阅读16次。本文详细介绍了在vins-mono初始化阶段如何使用ceres solver求解工具对其中的状态量进行优化求解。https://blog.csdn.net/weixin_44587828/article/details/134154767

imu预积分约束

所谓预积分约束，主要来源于预积分的公式，如下：

在构建约束问题时，上式中的位置和姿态初值是由滑窗内位姿给出，而预积分量是imu计算出的，因为存在噪声，所以上式不会完全相等，将所有式子移到等式一边，也就可以构成以下约束。

误差维数是15维，涉及到的待优化的参数块有滑窗内两帧位姿（7维），滑窗内两帧的速度和偏置（9维）。后端涉及到的优化变量较多因此采用的是解析求导，使用解析求导需要定义一个factor的类，该类需要以公共方式继承ceres::SizedCostFunction函数或者ceres::CostFunction函数。前者是在编译阶段就明确残差维数和待优化参数块维数，后者不明确上述参数块维度。

在imu构建的残差块中，参数块维数和残差块维数都已知，因此使用ceres::SizedCostFunction进行继承，该中方式继承需要写出模板参数维度，分别是残差块维度和参数块维度，15，7，9，7，9。此外，在函数体内还需要重写Evaluate函数，在函数体内部进行残差和雅克比的计算。

残差的计算根据上图中的公式即可计算，雅克比矩阵计算推导如下：

在计算残差和雅克比过程中涉及到信息矩阵，在ceres中没有信息矩阵的接口，故在计算残差矩阵和雅克比矩阵时候就将信息矩阵融进里面。具体做法是，将信息矩阵进行LLT分解，由于最小二乘形式是残差矩阵转置和自身相乘，所以添加进信息矩阵形式如下：

$f=e^{T}RR^{T}e$

其中R是信息矩阵（预积分计算出的协方差矩阵）的分解，残差矩阵和雅克比矩阵都要乘以R的转置。

视觉重投影约束

在进行构建视觉重投影约束时，包含估计时间延时和不进行时间延时估计两种情况。本文主要介绍不进行时间延时估计的情况，关于时间延时之后会专门出一期进行讲解。

在构建视觉重投影误差中，涉及到的待优化参数有两帧图像对应的滑窗内位姿、相机和imu外参、地图点逆深度。所以可以确定残差维度和参数块维度：残差维度是2，第一帧图像位姿维度7，第二帧图像位姿维度7，外参维度7，地图点逆深度维度1，这里也看出使用逆深度的好处，否则地图点逆深度就多出2维，对于数目最多的地图点，降低两维对于减轻后端优化压力意义重大。

建立约束模型

根据投影模型和vins特点，可以构建如下数学模型：

$\begin{bmatrix} x_{cj}\\ y_{cj}\\ z_{cj}\\ 1\end{bmatrix}=T_{bc}^{-1}T_{wb_{j}}^{-1}T_{wb_{i}}T_{bc}\begin{bmatrix} \frac{1}{\lambda }u_{ci}\\ \frac{1}{\lambda }v_{ci}\\ \frac{1}{\lambda }\\ 1\end{bmatrix}$

上述过程：将第i帧相机系下的世界点先经过外参变换到imu坐标系下，然后经过imu到世界系的坐标变换变到世界系下，再转换到第j帧imu坐标系，最后转换到第j帧相机系下。

将上式中的T都用对应的R和P表示，展开即可得到关于待优化变量的表达式：

$\begin{bmatrix} x_{cj}\\ y_{cj}\\ z_{cj}\\ 1\end{bmatrix}=\begin{bmatrix} R_{bc}^{T} &-R_{bc}^{T}P_{bc} \\ 0& 1 \end{bmatrix}\begin{bmatrix} R_{wb_{j}}^{T} &-R_{wb_{j}}^{T}P_{wb_{j}} \\ 0& 1 \end{bmatrix}\begin{bmatrix} R_{wb_{i}} &-R_{wb_{i}}P_{wb_{i}} \\ 0& 1 \end{bmatrix}\begin{bmatrix} R_{bc} & -R_{bc}P_{bc} \\ 0& 1 \end{bmatrix}\begin{bmatrix} \frac{1}{\lambda }u_{ci}\\ \frac{1}{\lambda }v_{ci}\\ \frac{1}{\lambda }\\ 1\end{bmatrix}$

将上式打开可以得到关于旋转和平移的式子：

$P^{ci}=\begin{bmatrix} \frac{1}{\lambda }u_{ci}\\ \frac{1}{\lambda }v_{ci}\\ \frac{1}{\lambda }\\ 1\end{bmatrix}$

$\begin{bmatrix} x_{cj}\\ y_{cj}\\ z_{cj}\end{bmatrix}=R_{bc}^{T}R_{wb_{j}}^{T}R_{wb_{i}}R_{bc}P^{ci}+R_{bc}^{T}(R_{wb_{j}}^{T}((R_{wb_{i}}P_{bc}+P_{wb_{i}})-P_{wb_{j}})-P_{bc})$

对于残差的构建比较简单，使用光流追踪得到的像素坐标变换到相机归一化平面的点和经过上式变换得到的归一化坐标作差即可：

$r_{c}=\begin{bmatrix} \frac{x_{cj}}{z_{cj}}-u_{cj}\\ \frac{y_{cj}}{z_{cj}}-v_{cj}\end{bmatrix}$

而关于雅克比的计算需要用到和slam14讲里面类似的链式求导，使用第j帧观测到的3d点作为中间变量，先求残差关于此点的雅克比，再求此点关于其他位姿等状态量的雅克比，最后结果二者相乘即可。

整个过程如下：

在计算关于视觉重投影构成的残差项和雅克比时，信息矩阵是一个固定的值。可以理解为对于每一个像素点的提取置信度是一样的，设置置信度在设置参数函数里面有赋值：

ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();

使用虚拟焦距460除以1.5，这里也是使用的虚拟焦距，由于光流追踪得到的像素点点会根据虚拟焦距映射到归一化平面，所以这里置信度也无伤大雅。然而，直观上理解，质量好的相机和质量差的相机拍摄的特征点置信度一样，应该多少也会对系统产生一定的影响。

至此，关于预积分残差约束和视觉重投影的残差约束以及对应的雅克比矩阵已经求解完成，在添加残差块中最主要的两部分已经完成。在下篇博客中再对后端优化中的滑窗约束进行梳理讲解，敬请期待，感谢阅读！

陈于落雁

关注

13
点赞
踩
3

收藏

觉得还不错? 一键收藏
1
评论
vins-mono后端优化（1）

本文详细推导了vins-mono后端优化里imu预积分残差、视觉重投影的雅克比矩阵计算。
复制链接

扫一扫

专栏目录