VIO学习笔记（七）—— VINS 初始化和 VIO 系统

最新推荐文章于 2024-04-29 00:27:10 发布

梁小憨憨

最新推荐文章于 2024-04-29 00:27:10 发布

阅读量1.1k

点赞数 2

分类专栏： VIO 文章标签： slam

本文链接：https://blog.csdn.net/qq_41990294/article/details/105392004

版权

VIO 专栏收录该内容

11 篇文章 18 订阅

订阅专栏

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。

VIO学习笔记（一）—— 概述
 VIO学习笔记（二）—— IMU 传感器
 VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合
 VIO学习笔记（四）—— 基于滑动窗口算法的 VIO 系统:可观性和一致性
 VIO学习笔记（五）—— 后端优化实践:逐行手写求解器
 VIO学习笔记（六）—— 前端 Frontend

VIO 相关知识回顾

IMU 预积分技术

有关预积分的内容请参考我的这篇博客的IMU 测量值积分部分。
IMU 传感器模型
$^b = ω ^b + b^ g + n^ g\\ ã ^b = q _{bw} (a^ w +g^ w ) + b ^a + n^ a$
在这里插入图片描述
将一段时间内的 IMU 数据直接积分起来就能得到两时刻 i, j 之间关于 IMU 的测量约束,即预积分量:
$_{b _i b _j} =\iint_{t∈[i,j]}(q _{b _i b _t} a ^{b t} )δt^ 2\\ β _{b _i b _j} = \int_{t∈[i,j]}(q _{b _i b _t} a ^{b t} )δt\\ q _{b _i b _j}=\int_{t∈[i,j]}q _{b _i b _t} ⊗\begin{bmatrix} 0\\ \frac12w^{b_t}\\ \end{bmatrix}$

视觉几何基础

相关内容可以参考高翔博士的《视觉SLAM十四讲》。

已知俩图像:特征点提取 (fast),匹配 (光流,特征描述子).
已知俩图像特征匹配点:利用对极几何约束 (E 矩阵,H 矩阵),计算两图像之间的 pose (update to scale).
已知相机 pose, 已知特征点二维坐标: 通过三角化得到三维坐标.
已知 3d 点,2d 特征点:通过 Perspective-n-Point(PnP) 求取新的相机 pose.

问题

IMU 怎么和世界坐标系对齐,计算初始时刻的 $q _{wb _0}$ ?
单目视觉姿态如何和 IMU 轨迹对齐,尺度如何获取?
VIO 系统的初始速度 v,传感器 bias 等如何估计?
IMU 和相机之间的外参数等…

VINS 鲁棒初始化

视觉和 IMU 之间的联系

在这里插入图片描述
考虑相机坐标系 $c _0$ 为世界坐标系,则利用外参数 $q _{bc}$ , $t _{bc}$ 构建等式
$q_{ c _0 b _k} = q _{c _0 c _k} ⊗ q _{bc}^{-1}\\ s\tilde{p}_{ c _0 b _k} = s\tilde{p}_{ c _0 c_ k} − R_{ c _0 b_ k} p_{ bc}$
其中,s 为尺度因子, $\tilde{p}$ 表示非米制单位的轨迹。上式等价于
$\tilde{p}_{ c _0 b _k} = \tilde{p} _{c _0 c _k}− \frac1sR_{ c _0 b_ k} p_{ bc}\\ \tilde{p} _{c _0 c _k} = \frac1sR_{ c _0 b_ k} p_{ bc}+ \tilde{p}_{ c _0 b _k}$

视觉 IMU 对齐流程

旋转外参数 q bc 未知, 则先估计旋转外参数.
利用旋转约束估计陀螺仪 bias.
$q_{ c _0 b _k} = q _{c _0 c _k} ⊗ q _{bc}^{-1}$
利用平移约束估计重力方向,速度,以及尺度初始值.
$s\tilde{p}_{ c _0 b _k} = s\tilde{p}_{ c _0 c_ k} − R_{ c _0 b_ k} p_{ bc}$
对重力向量 $g ^{c _0}$ 进行进一步优化.
求解世界坐标系 w 和初始相机坐标系 $c_ 0$ 之间的旋转矩阵 $q _{wc _0}$ ,并将轨迹对齐到世界坐标系。

利用旋转约束估计外参数旋转 $q _{bc}$

相邻两时刻 k, k + 1 之间有:IMU 旋转积分 $q _{b _k b _{k+1}}$ ,视觉测量 $q _{c _k c_{ k+1}}$ 。则有:
$q _{b _k b _{k+1}} ⊗ q _{bc} = q _{bc} ⊗ q _{c _k c_{ k+1}}$
图形解释为;
在这里插入图片描述
上式可写成:
$_{b _k b _{k+1}}]_L-[q _{c _k c_{ k+1}}])q _{bc}=Q^ k_{k+1} · q _{bc} = 0$
其中, $_L$ , $_ R$ 表示 left and right quaternion multiplication。
将多个时刻线性方程累计起来,并加上鲁棒核权重得到:
在这里插入图片描述
其中:

由旋转矩阵和轴角之间的关系 $t r (R) = 1 + 2 c o s θ$ ,能得到角度误差 $r$ 的计算为:

公式的求解同样采用 SVD 分解,即最小奇异值对应的奇异向量。
具体代码见:initial_ex_rotation.cpp 函数 CalibrationExRotation().

 bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));
    Rimu.push_back(delta_q_imu.toRotationMatrix());
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

    Eigen::MatrixXd A(frame_count * 4, 4);
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        //ROS_DEBUG("%d %f", i, angular_distance);

        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        Quaterniond R_ij(Rimu[i]);
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

基于旋转约束的 Gyroscope Bias

如果外参数 $q _{bc}$ 已标定好,利用旋转约束,可估计陀螺仪 bias:
在这里插入图片描述

其中, B 表示所有的图像关键帧集合,另有预积分的一阶泰勒近似:

公式为普通的最小二乘问题,求取雅克比矩阵,构建正定方程HX = b 即可以求解。
具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias().

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
    Matrix3d A;
    Vector3d b;
    Vector3d delta_bg;
    A.setZero();
    b.setZero();
    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = next(frame_i);
        MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        VectorXd tmp_b(3);
        tmp_b.setZero();
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    delta_bg = A.ldlt().solve(b);
    // ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());

    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;

    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]);
    }
}

初始化速度、重力和尺度因子

需要估计的变量
在这里插入图片描述
其中, $v _k ^{b _k}$ 表示 k 时刻 body 坐标系的速度在 body 坐标系下的表示。 $g ^{c _0}$ 为重力向量在第 0 帧相机坐标系下的表示。s 表示尺度因子,将视觉轨迹拉伸到米制单位。
imu预积分误差：
在这里插入图片描述
预积分量约束,世界坐标系 w 下有:

将世界坐标系 w 换成相机初始时刻坐标系 $c _0$ 有:

将公式进行简单整理有:

将待估计变量放到方程右边,有:

公式中:

对以上公式进行说明;

转化成线性最小二乘问题对状态量进行求解:
在这里插入图片描述
具体代码见:initial_aligment.cpp 函数 LinearAlignment().

bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 3 + 1;

    MatrixXd A{n_state, n_state};
    A.setZero();
    VectorXd b{n_state};
    b.setZero();

    map<double, ImageFrame>::iterator frame_i;
    map<double, ImageFrame>::iterator frame_j;
    int i = 0;
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end(); frame_i++, i++)
    {
        frame_j = next(frame_i);

        MatrixXd tmp_A(6, 10);
        tmp_A.setZero();
        VectorXd tmp_b(6);
        tmp_b.setZero();

        double dt = frame_j->second.pre_integration->sum_dt;

        tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
        tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;     
        tmp_b.block<3, 1>(0, 0) = frame_j->second.pre_integration->delta_p + frame_i->second.R.transpose() * frame_j->second.R * TIC[0] - TIC[0];
        //cout << "delta_p   " << frame_j->second.pre_integration->delta_p.transpose() << endl;
        tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
        tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
        tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
        tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v;
        //cout << "delta_v   " << frame_j->second.pre_integration->delta_v.transpose() << endl;

        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
        //cov.block<6, 6>(0, 0) = IMU_cov[i + 1];
        //MatrixXd cov_inv = cov.inverse();
        cov_inv.setIdentity();

        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
        b.segment<6>(i * 3) += r_b.head<6>();

        A.bottomRightCorner<4, 4>() += r_A.bottomRightCorner<4, 4>();
        b.tail<4>() += r_b.tail<4>();

        A.block<6, 4>(i * 3, n_state - 4) += r_A.topRightCorner<6, 4>();
        A.block<4, 6>(n_state - 4, i * 3) += r_A.bottomLeftCorner<4, 6>();
    }
    A = A * 1000.0;
    b = b * 1000.0;
    x = A.ldlt().solve(b);
    double s = x(n_state - 1) / 100.0;
    // ROS_DEBUG("estimated scale: %f", s);
    g = x.segment<3>(n_state - 4);
    // ROS_DEBUG_STREAM(" result g     " << g.norm() << " " << g.transpose());
    if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
    {
        return false;
    }

    RefineGravity(all_image_frame, g, x);
    s = (x.tail<1>())(0) / 100.0;
    (x.tail<1>())(0) = s;
    // ROS_DEBUG_STREAM(" refine     " << g.norm() << " " << g.transpose());
    if(s < 0.0 )
        return false;   
    else
        return true;
}

优化重力向量 $g ^{c _0}$

疑问:为什么需要优化重力向量

利用公式(16)求解重力向量 $g ^{c _0}$ 过程中,并没有加入模长限制 $∥ g c 0 ∥ = 9.81$ 。三维变量 $g ^{c _0}$ 实际只有两个自由度。

重力向量参数化

三维向量自由度为 2,可以采用球面坐标进行参数化:

在这里插入图片描述

其中, $w_1$ , $w_2$ 为待优化变量

待优化变量变为:

观测方程变为:

采用最小二乘对 $X_ I$ 进行重新优化。

将相机坐标系对齐世界坐标系

对齐流程:

找到 $c _0$ 到 w 系的旋转矩阵 $R _{wc _0} = exp([θu])$
把所有 $c _0$ 坐标系下的变量旋转到 w 下。
把相机平移和特征点尺度恢复到米制单位。
至此,完成了系统初始化过程。

在这里插入图片描述

VINS 系统

VINS 系统三大块

前端,数据处理:特征提取匹配,imu 积分
初始化:系统初始状态变量(重力方向,速度,尺度等等)
后端:滑动窗口优化

在这里插入图片描述

后端滑动窗口优化

VINS 系统优化的状态变量为:
在这里插入图片描述
通过最小化滑动窗口中的残差项来估计系统的状态变量:

注意:其中鲁棒核函数 $ρ (\cdot)$ 仅处理视觉 outlier.

参考资料：

Zhenfei Yang and Shaojie Shen. “Monocular visual–inertial state estimation with online initialization and camera–IMU extrinsic calibration”. In: IEEE Transactions on Automation Science and Engineering 14.1 (2016), pp. 39–51.
Tong Qin and Shaojie Shen. “Robust initialization of monocular visual-inertial . estimation on aerial robots”. In: . 2017 .IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE.2017,pp. 4225–4232
Janne Mustaniemi et al. “Inertial-based scale estimation for structure from motion on mobile devices”. In:2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2017, pp. 4394–4401.
Javier Domínguez-Conti et al. “Visual-Inertial SLAM Initialization: A General Linear Formulation and a Gravity-Observing Non-Linear Optimization”. In: 2018 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE. 2018, pp. 37–45.

梁小憨憨

关注

2
点赞
踩
20

收藏

觉得还不错? 一键收藏
5
评论
VIO学习笔记（七）—— VINS 初始化和 VIO 系统

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。VIO学习笔记（一）—— 概述VIO学习笔记（二）—— IMU 传感器VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合VIO学习笔记（四）—— 基于滑动窗口算法的 VIO 系统:可观性和一致性VIO学习笔记（五）...
复制链接

扫一扫