VINS-Mono-紧耦合VIO中视觉残差的优化原理和代码解析

前言

承接上一篇博客，此篇分析残差:
$$\sum _{(l,j)\in C}\rho ({\Vert {\mathbf{r}}_C({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi })\Vert }_{{\mathbf{P}}_l^{c_j}}^2$$

原理

定义残差方程

视觉残差即重投影误差，但VINS-Mono中所使用的重投影误差模型与传统的针孔相机模型不同，其在一个单位球面上定义残差，因为这种残差定义方式可以满足几乎所有的相机模型(广角、鱼眼…)．我们考虑第$l$个特征第一次被观测到是在第$i$张图像中，则特征在第$j$张图像中被观察到时的残差定义如下:
$${\mathbf{r}}_C({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi })=[{\mathbf{b}}_1,{\mathbf{b}}_2{]}^T\cdot (\frac{{\mathbf{p}}_l^{c_j}}{\Vert {\mathbf{p}}_l^{c_j}\Vert }-{\widehat{\bar{\mathbf{p}}}}_l^{c_j})\text{\hspace{1em}(1)}$$
式中 ${\widehat{\bar{\mathbf{p}}}}_l^{c_j}={\pi }_c^{-1}(\left[\begin{array}{c}{\hat{u}}_l^{c_j}\\ {\hat{v}}_l^{c_j}\end{array}\right])$ 为第$l$个特征在第$j$张图像中的像素通过相机内参反向投影到单位球面上的三维坐标，式中 ${\mathbf{p}}_l^{c_j}={\mathbf{R}}_b^c({\mathbf{R}}_w^{b_j}({\mathbf{R}}_{b_i}^w({\mathbf{R}}_c^b\frac{1}{{\lambda }_l}{\bar{\mathbf{p}}}_l^{c_i}+{\mathbf{p}}_c^b)+{\mathbf{p}}_{b_i}^w-{\mathbf{p}}_{b_j}^w)-{\mathbf{p}}_c^b)$ 为通过第$l$个特征在第$i$张图像中的像素推算出的该特征在第j张图像中的估计值，$[{\mathbf{b}}_1,{\mathbf{b}}_2{]}^T$ 为 ${\widehat{\bar{\mathbf{p}}}}_l^{c_j}$ 向量的流平面上一对任意的互相垂直的基向量(这样做的原因是因为视觉残差的自由度是2，所以此处通过投影将三维残差限制为了2维)．

计算雅克比矩阵

从上述残差方程可以看出参与优化的状态变量有$({\mathbf{p}}_{b_i}^w,{\mathbf{q}}_{b_i}^w),({\mathbf{p}}_{b_j}^w,{\mathbf{q}}_{b_j}^w),({\mathbf{p}}_c^b,{\mathbf{q}}_c^b)$．

• 结果
  

• 推导
  
  

代码解析

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    TicToc tic_toc;
   // 第i帧位姿
    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
	// 第j帧位姿
    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
	// imu与相机之间的相对位姿关系
    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
	// 第i特征的逆深度
    double inv_dep_i = parameters[3][0];

    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;//特征点在第i帧相机坐标系下坐标
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;//特征点在第i帧imu坐标系下坐标
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;// 特征点在世界坐标系的坐标
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);//特征点在第j帧imu坐标系下坐标
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);//特征点在第j帧图像坐标系下坐标
    Eigen::Map<Eigen::Vector2d> residual(residuals);

// 计算残差值，公式(1)
#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

    residual = sqrt_info * residual;

    if (jacobians)
    {
        Eigen::Matrix3d Ri = Qi.toRotationMatrix();
        Eigen::Matrix3d Rj = Qj.toRotationMatrix();
        Eigen::Matrix3d ric = qic.toRotationMatrix();
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
        double norm = pts_camera_j.norm();
        Eigen::Matrix3d norm_jaco;
        double x1, x2, x3;
        x1 = pts_camera_j(0);
        x2 = pts_camera_j(1);
        x3 = pts_camera_j(2);
        norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3),            - x1 * x3 / pow(norm, 3),
                     - x1 * x2 / pow(norm, 3),            1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
                     - x1 * x3 / pow(norm, 3),            - x2 * x3 / pow(norm, 3),            1.0 / norm - x3 * x3 / pow(norm, 3);
        reduce = tangent_base * norm_jaco;
#else
        reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
            0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
        reduce = sqrt_info * reduce;
		// 计算残差方程相对于第i帧imu位姿的雅克比矩阵
        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix<double, 3, 6> jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
            jacobian_pose_i.rightCols<1>().setZero();
        }
		// 计算残差方程相对于第j帧imu位姿的雅克比矩阵
        if (jacobians[1])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);

            Eigen::Matrix<double, 3, 6> jaco_j;
            jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
            jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);

            jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
            jacobian_pose_j.rightCols<1>().setZero();
        }
        // 计算残差方程相对于imu与相机外参的雅克比矩阵
        if (jacobians[2])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
            Eigen::Matrix<double, 3, 6> jaco_ex;
            jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
            Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
            jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
                                     Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
            jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
            jacobian_ex_pose.rightCols<1>().setZero();
        }
        // 计算残差方程相对于特征点逆深度状态的雅克比矩阵
        if (jacobians[3])
        {
            Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
#if 1
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
            jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
        }
    }
    sum_t += tic_toc.toc();

    return true;
}

参考

https://github.com/StevenCui/VIO-Doc
https://github.com/HKUST-Aerial-Robotics/VINS-Mono

注释代码路径

https://github.com/chennuo0125-HIT/vins-note