自学内容网 自学内容网
自学内容网 > 正文

VINS-FUSION 优化-视觉误差因子

🕗 发布于 2024-07-22 23:39 vins-fusion  重投影误差  视觉误差因子  外参校准  雅克比  

一、视觉误差因子

VINS-FUSION 优化问题可以统一用损失函数表示如下：

式中，包括先验误差因子（滑窗边缘化产生），IMU预积分误差因子，视觉误差因子。本文将结合论文和对应源码详细叙述。

在经典的视觉slam算法中，视觉误差通常用重投影误差表示。
重投影误差：指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值。
在计算重投影误差时，有两种特征参数化方式：特征点3D位置和特征点深度/逆深度。

1.特征点3D位置


其中：3D位置P_{l} = [x_{l},y_{l},z_{l}]^{T}；

z_{l}^{k}表示特征点l在第k帧中的观测；

((R_{c_{k}}^{w})^{T}, p_{c_{k}}^{w})相机在global位姿；

\pi (\cdot )表示相机重投影模型，将3D特征点去畸变。

2.深度/逆深度

从第i帧到j帧重投影误差可以表示如下：

其中：\lambda _{i}表示第i帧深度；

实质上两者是一致的，可以相互转化。

可以把P_{l}看成是i帧特征在global位姿，计算如下：

P_{l} =(R_{c_{i}}^{w})^{T}\lambda_{i} \begin{pmatrix} z_{l}^{i}\\ 1 \end{pmatrix} + p_{c_{i}}^{w}

二、vins-fusion源码中视觉误差因子

1.求视觉残差（重投影误差）
bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
    TicToc tic_toc;
    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]);

    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]);

    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]);

    double inv_dep_i = parameters[3][0];

    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
    Eigen::Map<Eigen::Vector2d> residual(residuals);

#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

...

}

其中：Pi,Qi表示i帧位姿，对应公式为(p_{b_{i}}^{w}, q_{b_{i}}^{w})；

Pj,Qj表示i帧位姿，对应公式为(p_{b_{j}}^{w}, q_{b_{j}}^{w})；

tic,qic表示外参，对应公式为(p_{c}^{b},q_{c}^{b})；

inv_dep_i 表示特征点l的逆深度值;

pts_i表示为该特征点在第i帧被观测到；

pts_j表示为该特征点在第j帧被观测到；

pts_i，pts_j表示为global的特征点被i, j帧同时观测到，是同一个global的特征点。

将上述代码转译成公式如下：

对比如下公式：

其实就是上述深度/逆深度参数，从第i帧到j帧重投影误差公式的展开形式。

2.雅可比矩阵

残差对优化变量的雅可比矩阵。

残差：

z_j = pts_camera_j.z();
residual = (pts_camera_j / z_j ).head<2>() - pts_j_td.head<2>();

优化变量\chi：(p_{b_{i}}^{w}, q_{b_{i}}^{w})，(p_{b_{j}}^{w}, q_{b_{j}}^{w})，(p_{c}^{b},q_{c}^{b}), \lambda _{l}

根据链式求导法则：

jacobians = \frac{\delta (e)}{\delta (p)} \frac{\delta (p)}{\delta (\chi)}

其中：p=[u_{i},v_{i},z_{j}]

（1）残差对p的雅可比矩阵

\frac{\delta (e)}{\delta (p)}=\begin{vmatrix} \frac{1}{z_{j}} & 0 & - \frac{p_{l}^{c_{j}}(0)}{z_{j}^2}\\ 0&\frac{1}{z_{j}} & - \frac{p_{l}^{c_{j}}(1)}{z_{j}^2}\end{vmatrix}

(2)残差对(p_{b_{i}}^{w}, q_{b_{i}}^{w})的雅可比矩阵

(3)残差对(p_{b_{j}}^{w}, q_{b_{j}}^{w})的雅可比矩阵

(3)残差对(p_{c}^{b},q_{c}^{b})的雅可比矩阵

\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{c}^{b}}& \frac{\partial r_{c}}{\partial q_{c}^{b}} \end{pmatrix}

(4)残差对\lambda _{l}的雅可比矩阵

雅克比矩阵对应源码：

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {

...

#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;

        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();
        }

        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();
        }

        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

...

}

其中：

reduce与\frac{\delta (e)}{\delta (p)}相对应；

jacobians[0]与\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{b_{i}}^{w}}& \frac{\partial r_{c}}{\partial q_{b_{i}}^{w}} \end{pmatrix}相对应；

jacobians[1]与\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{b_{j}}^{w}}& \frac{\partial r_{c}}{\partial q_{b_{j}}^{w}} \end{pmatrix}相对应；

jacobians[2]与\begin{pmatrix} \frac{\partial r_{c}}{\partial p_{c}^{b}}& \frac{\partial r_{c}}{\partial q_{c}^{b}} \end{pmatrix}相对应；

jacobians[3]与\frac{\delta (r_{c})}{\delta (\lambda_{l})}相对应。

3.协方差

视觉约束的噪声协方差与标定相机内参的重投影误差相关。即视觉约束的噪声协方差可以根据标定内参时重投影误差来取定。代码中选取的是偏移1.5个像素，对应到归一化相机平面的协方差矩阵需要除以焦距f。而信息矩阵sqrt_info等于协方差矩阵的逆，如下：

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

至此，论文公式和源码就一一对应起来了。

三、参考

1.VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator

2.Online Temporal Calibration for Monocular Visual-Inertial Systems

3.https://github.com/HKUST-Aerial-Robotics/VINS-Fusion


原文地址：https://blog.csdn.net/u010196944/article/details/135627444

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！

相关文章

自学内容网
Copyright © 2015-2024