0. 简介

在VINS优化中，我们除了对IMU的P（位置）,Q（旋转角）,V（速度）,Ba（加速度偏转角）,Bg（陀螺仪偏转角）以外，我们还需要对相机本身的外参、逆深度、与时差进行优化。其中逆深度最为重要，但是网上的信息没有能够很好的对逆深度进行科学的阐释。所以本文根据自己的理解来讲述一下里程计。

1. 参数化

在了解逆深度之前，我们需要了解下为什么需要参数化。参数化作为点、线、面的slam表现形式，
这使得我们能够通过矩阵的形式表示出合适的优化输入。
比如我们经常用一个点的世界坐标x,y,z三个量来描述它，这是一种参数化形式。我们认为x,y,z三个量都是随机的，它们服从三维的高斯分布。然而，在极线搜索中使用了图像坐标u,v和深度值d来描述某个空间点（即稠密建图）。我们认为u,v不动，而d服从（一维的）高斯分布，这是另一种参数化形式。那么这两种参数化形式有什么不同吗？我们是否也能假设u,v服从高斯分布，从而形成另一种参数数化形式呢？

不同的参数化形式，实际都描述了同一个量，也就是某个三维空间点。考虑到我们在相机看到某个点时，它的图像坐标u,v是比较确定的（u，v的不确定性取决于图像的分辨率）而深度值d则是非常不确定的。此时，若用世界坐标x,y,z描述这个点，那么根据相机当前的位姿，x,y,z三个量之间可能存在明显的相关性。反映在协方差矩阵中，表现为非对角元素不为零。而如果用u,v,d参数化一个点，那么它的u,v和d至少是近似独立的，甚至我们还能认为u,v也是独立的——从而它的协方差矩阵近似值为对角阵，更为简洁。

2. 逆深度

逆深度（Inverse depth）是近年来SLAM研究中出现的一种广泛使用的参数化技巧。在极线搜索和块匹配中，我们假设深度值满足高斯分布。然而仔细想想会发现，在一些室外应用中，可能存在距离非常远，乃至无穷远处的点。我们难以通过高斯分布描述他们，

同时在Bundle Ajustment中，参数空间通常呈现出高维度、非线性的特点。其中对于3D特征点的优化占据了大量的运算量。然而当特征点的距离很远时，该特征点带来的误差就会很大，代价函数需要对该特征点进行较大幅度的调整才能进一步降低代价函数。

于是，逆深度应运而生。人们在仿真中发现，假设深度的倒数（也就是逆深度），为高斯分布是比较有效的。随后，在实际应用中，逆深度也具有更好的数值稳定性，从而逐渐成为一种通用的技巧。

3. 单目视觉逆深度推导

以VINS为代表，其前端的本质就是基于滑动窗口的紧耦合单目VIO。VINS系统会有三个坐标系统，分别为世界坐标系，Body/IMU坐标系和相机坐标系。
在这里插入图片描述

对于滑动窗口内，系统需要求解的状态向量如下所示：IMU状态（PVQ、加速度bias、陀螺仪bias）、IMU到Camera的外参、m+1个路标点逆深度。

在这里插入图片描述

第一个式子是滑动窗口内所有状态量，n是帧数，m是滑动窗口内特征点总数。特征点逆深度为了满足高斯系统。

第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。

第三个式子是相机外参。

这里我们着重介绍逆深度部分，在初始化(划窗优化)部分加入了逆深度部分的信息，这部分可以有效的表示出在无穷远处的X,Y,Z三维的信息。具体的步骤为：

  • 首先是判断当前特征点出现的次数是够大于两次，并且首次观测到当前特征点的图像帧是否在次最新帧之前。如果当前特征点的逆深度大于零（初始化为-1）时，则表示当前特征点已经三角化过，则跳过。
  • 计算首次观测到当前特征点的变换矩阵$R_0,t_0$，表示第i帧相对于世界坐标系的变换矩阵。
  • 遍历其他观测到当前特征点的图像帧，计算第j帧相对于世界坐标系的变换矩阵$R_1,t_1$，然后计算第$j$帧相对于第$i$帧的变换矩阵$R,t$，最后再取逆得到P，表示第i帧到第j帧的变换矩阵。
  • 然后根据上一小节推导的公式$AX=0$，构造矩阵A，最后进行SVD分解，即可得到当前特征点的深度信息。
for (auto &it_per_frame : it_per_id.feature_per_frame)
{
 // 得到该KF的相机坐标系位姿 第J帧看到的滑窗里的id
            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            // T_w_cj -> T_c0_cj 枢纽坐标系【不一定是第一帧】 c0[第一个观察到这个特征点的帧]
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix<double, 3, 4> P;
            // T_c0_cj -> T_cj_c0相当于把c0当作世界系
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            Eigen::Vector3d f = it_per_frame.point.normalized();
            // 构建超定方程的其中两个方程遍历观测到特征点的所有帧
            svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
            svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);
}

ROS_ASSERT(svd_idx == svd_A.rows());
Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();
// 求解齐次坐标下的深度
double svd_method = svd_V[2] / svd_V[3];
//it_per_id->estimated_depth = -b / A;
//it_per_id->estimated_depth = svd_V[2] / svd_V[3];
// 得到的深度值实际上就是第一个观察到这个特征点的相机坐标系下的深度值
it_per_id.estimated_depth = svd_method;
//it_per_id->estimated_depth = INIT_DEPTH;

if (it_per_id.estimated_depth < 0.1)
{
    it_per_id.estimated_depth = INIT_DEPTH; // 具体太近就设置成默认值
}

参考链接

https://blog.csdn.net/weixin_39568744/article/details/88582406

https://blog.csdn.net/wangshuailpp/article/details/78719593

https://zhuanlan.zhihu.com/p/391505370

https://scm_mos.gitlab.io/slam/VINS-FUSION/

https://blog.csdn.net/KYJL888/article/details/109851256

https://gutsgwh1997.github.io/2020/05/21/VINS%E4%B8%AD%E7%9A%84%E8%A7%86%E8%A7%89%E6%AE%8B%E5%B7%AE/