VINS-MONO：VIO初始化部分原理推导+代码解析

每天都在努力学习SLAM的小黑

已于 2022-10-25 09:34:38 修改

阅读量2k

点赞数 5

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

于 2021-07-29 21:24:42 首次发布

本文链接：https://blog.csdn.net/weixin_45626706/article/details/118344720

版权

VINS 专栏收录该内容

6 篇文章 11 订阅

订阅专栏

在这里插入图片描述

1.为什么需要初始化

在这里插入图片描述

优化问题非常依赖初始值。
防止陷入局部最小值
与真实值越近迭代次数越少

1.1 需要优化哪些变量

在这里插入图片描述
优化变量: 地图点,p,v,q,零偏,外参

vins初始值不求解加速度计的零偏(存在重力影响,也不影响优化结果)和外参的平移(人工测量)

1.2 初始化不解决哪些变量

在这里插入图片描述

并不需要一个非常准确的外参,因为在非线性优化中会不断去优化

在这里插入图片描述

加速度计bias很难从重力估计中分离出来,忽略加速度计bias不会显著影响结果

1.3 初始化成功需要满足哪些条件

枢纽帧和最后一帧既有足够的共视也要有足够的视差
对关键帧纯视觉BA求解成功
对非关键帧位姿求解成功
成功求解各帧的速度,枢纽帧的重力方向,以及尺度

顺序依次往下

2. VINS对特征点的管理

在这里插入图片描述

根据id管理
起始帧id:被哪一帧最先观测到
求解状态:是否被成功三角化
一个id的特征点会被多帧所看到

class FeaturePerFrame
{
  public:
    //_point:[x,y,z,u,v,vx,vy]
    FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)
    {
        point.x() = _point(0);
        point.y() = _point(1);
        point.z() = _point(2);
        uv.x() = _point(3);
        uv.y() = _point(4);
        velocity.x() = _point(5); 
        velocity.y() = _point(6); 
        cur_td = td;
    }
    double cur_td;   // 最先被哪一帧看到
    Vector3d point;  // 去畸变后的归一化相机坐标
    Vector2d uv;     // 像素坐标
    Vector2d velocity; // 特征点速度
    double z;        // 特征点的深度
    bool is_used;    // 是否被使用了
    double parallax; // 视差
    MatrixXd A;      // 变换矩阵
    VectorXd b;
    double dep_gradient;
};

在这里插入图片描述

3.旋转外参初始化

不标定平移

由手眼标定公式表示相机和IMU集成的系统从到的运动，其中视觉可以通过特征匹配求得到时刻的旋转增量, 同时IMU也可以通过积分得到到时刻的旋转增量，设相机和IMU之间的旋转矩阵为，那么对任意时刻，满足：
$\mathbf{R}_{b_{k+1}}^{b_{k}} \cdot \mathbf{R}_{c}^{b}=\mathbf{R}_{c}^{b} \cdot \mathbf{R}_{c_{k+1}}^{c_{k}}$
将上式用四元数表示：
$\begin{aligned} \mathbf{q}_{b_{k+1}}^{b_{k}} \otimes \mathbf{q}_{c}^{b} &=\mathbf{q}_{c}^{b} \otimes \mathbf{q}_{c_{k+1}}^{c_{k}} \\ & \Rightarrow\left[\mathcal{Q}_{1}\left(\mathbf{q}_{b_{k+1}}^{b_{k}}\right)-\mathcal{Q}_{2}\left(\mathbf{q}_{c_{k+1}}^{c_{k}}\right)\right] \cdot \mathbf{q}_{c}^{b}=\mathbf{Q}_{k+1}^{k} \cdot \mathbf{q}_{c}^{b}=\mathbf{0} \end{aligned}$
其中：
$\begin{aligned} \mathcal{Q}_{1}(\mathbf{q}) &=\left[\begin{array}{cc} q_{w} \mathbf{I}_{3}+\left\lfloor\mathbf{q}_{x y z} \times\right\rfloor & \mathbf{q}_{x y z} \\ -\mathbf{q}_{x y z} & q_{w} \end{array}\right] \\ \mathcal{Q}_{2}(\mathbf{q}) &=\left[\begin{array}{cc} q_{w} \mathbf{I}_{3}-\left\lfloor\mathbf{q}_{x y z} \times\right\rfloor & \mathbf{q}_{x y z} \\ -\mathbf{q}_{x y z} & q_{w} \end{array}\right] \end{aligned}$
对于相对旋转的测量值，可得过约束的线性方程：
$\left[\begin{array}{c} w_{1}^{0} \cdot \mathbf{Q}_{1}^{0} \\ w_{2}^{1} \cdot \mathbf{Q}_{2}^{1} \\ \vdots \\ w_{N}^{N-1} \cdot \mathbf{Q}_{N}^{N-1} \end{array}\right] \cdot \mathbf{q}_{c}^{b}=\mathbf{Q}_{N} \cdot \mathbf{q}_{c}^{b}=\mathbf{0}$
在标定成功前，会一直增长, 为了更好的处理可能存在的异常值，每个都乘以了一个类似于huber核函数的权重。对进行Svd分解, 其中最小奇异值对应的右奇异向量便为结果。

详细证明:

U为正交矩阵，不会改变矩阵x的大小，所以可以约掉
x为单位四元数，因为只有单位四元数才能表示旋转

在这里插入图片描述

if(ESTIMATE_EXTRINSIC == 2) //如果没有外参则进行标定
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            // 得到两帧之间归一化特征点
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            // 标定从camera到IMU之间的旋转矩阵
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                ROS_WARN("initial extrinsic rotation calib success");
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

/**
 * 标定外参的旋转矩阵
 * @param corres            匹配的特征点
 * @param delta_q_imu       IMU预积分得的旋转矩阵Q
 * @param calib_ric_result  标定的外参数
 * @return
 */
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));               // 帧间cam的R，由对极几何得到
    Rimu.push_back(delta_q_imu.toRotationMatrix());     // 帧间IMU的R，由IMU预积分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);  // 每帧IMU相对于起始帧IMU的R,初始化为IMU预积分

    // SVD分解，Ax=0,对A填充。 多帧组成A，一对点组成的A是4×4的
    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);

        // huber核函数做加权
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        //R_bk+1^bk * R_c^b = R_c^b * R_ck+1^ck
        //[Q1(q_bk+1^bk) - Q2(q_ck+1^ck)] * q_c^b = 0
        //L R 分别为左乘和右乘矩阵，都是4×4矩阵
        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]);         // imu间旋转矩阵
        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);
    }

    // svd分解中最小奇异值对应的右奇异向量作为旋转四元数
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    // 这里的四元数存储的顺序是[x，y，z，w]‘
    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>();

    //至少迭代计算了WINDOW_SIZE次，且R的奇异值大于0.25才认为标定成功
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

4 单目视觉位姿估计

在这里插入图片描述

选取帧(枢纽帧)应与最后一帧尽可能远
但又希望两帧的共视点足够多
枢纽帧与最后一帧通过对极约束恢复位姿
第五帧及之后帧的位姿可以通过3D-2D的PNP求解出来,并优化出更多的点
同理对另一边也是

4.1 triangulatePoint原理

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
						Vector2d &point0, Vector2d &point1, Vector3d &point_3d)

作者自己实现了三角化的过程（Pc是归一化相机系下坐标系，已知帧的位姿和各自相机系下观测求三维点，为了等号意义成立，填上一个系数a）:
在这里插入图片描述

// 对特征点进行三角化求深度（SVD分解）
void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[])
{
    for (auto &it_per_id : feature)
    {
        it_per_id.used_num = it_per_id.feature_per_frame.size();
        // 如果该特征出现的次数<2  或 该特征被首次观测到时的对应帧在滑窗中是最后一帧了，就无法三角化了
        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
            continue;
        // 如果该特征点已经有深度了
        if (it_per_id.estimated_depth > 0)
            continue;
        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;

        ROS_ASSERT(NUM_OF_CAM == 1);
        // 两帧三角化时即可得到4个等式
        Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
        int svd_idx = 0;

        // R0 t0为第i帧相机坐标系到世界坐标系的变换矩阵
        // T_cw : [R|t]_3*4
        Eigen::Matrix<double, 3, 4> P0;
        Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
        Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
        P0.leftCols<3>() = Eigen::Matrix3d::Identity();
        P0.rightCols<1>() = Eigen::Vector3d::Zero();

        for (auto &it_per_frame : it_per_id.feature_per_frame)
        {
            imu_j++;
            //R t为第j帧相机坐标系到第i帧相机坐标系的变换矩阵，P为i到j的变换矩阵
            Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];
            Eigen::Vector3d t = R0.transpose() * (t1 - t0);
            Eigen::Matrix3d R = R0.transpose() * R1;
            Eigen::Matrix<double, 3, 4> P;
            P.leftCols<3>() = R.transpose();
            P.rightCols<1>() = -R.transpose() * t;
            Eigen::Vector3d f = it_per_frame.point.normalized();
            //P = [P1 P2 P3]^T 
            //AX=0      A = [A(2*i) A(2*i+1) A(2*i+2) A(2*i+3) ...]^T
            //A(2*i)   = x(i) * P3 - z(i) * P1
            //A(2*i+1) = y(i) * P3 - z(i) * P2
            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);

            if (imu_i == imu_j)
                continue;
        }
        // 对A的SVD分解得到其最小奇异值对应的单位奇异向量(x,y,z,w)，深度为z/w
        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;
        }

    }
}

4.2 ceres优化

优化变量 $x$ :

pose(滑窗中的11帧位姿)
points(滑窗pose之间生成的许许多多的地图点)

后续的代码都位于：/vins_estimator/src/initial/initial_aligment.cpp

//视觉和IMU对齐
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
    solveGyroscopeBias(all_image_frame, Bgs);//计算陀螺仪偏置

    if(LinearAlignment(all_image_frame, g, x))//计算尺度，重力加速度和速度
        return true;
    else 
        return false;
}

5. 陀螺仪零偏初始化

$b_k$ 到 $c_0$ 姿态的旋转: $\mathbf{q}_{b_{k}}^{c_{0}} =\mathbf{q}_{c_{k}}^{c_{0}} \otimes\left(\mathbf{q}_{c}^{b}\right)^{-1}$

$b_k$ 到 $c_0$ 姿态的平移: $\overline{\mathbf{p}}_{b_{k}}^{c_{b}}=s \overline{\mathbf{p}}_{c_{k}}^{c_{0} }-\mathbf{R}_{b_{k}}^{c_{0}} \mathbf{p}_{c}^{b}$

证明如下:
在这里插入图片描述
陀螺仪零偏的模型:

$\min _{\delta b_{w}} \sum_{k \in \mathcal{B}}\left\|\mathbf{q}_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes \mathbf{q}_{b_{k}}^{c_{0}} \otimes \boldsymbol{\gamma}_{b_{k+1}}^{b_{k}}\right\|^{2}$

其中对陀螺仪bias求IMU预积分项线性化，有：

$\boldsymbol{\gamma}_{b_{k+1}}^{b_{k}} \approx \hat{\boldsymbol{\gamma}}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}1 \\ \frac{1}{2} \mathbf{J}_{b_{w}}^{\gamma} \delta \mathbf{b}_{w}\end{array}\right]$ (近似估计出零偏变化后旋转预积分量的变化)

对陀螺仪零偏的补偿见预积分章节：由 $f (x + Δ x) = f (x) + J (x) Δ x =$ 原先预积分的结果 + 相对于零偏的一阶雅克比 * 补偿量
$\begin{aligned} \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}} & \approx \hat{\boldsymbol{\alpha}}_{b_{k+1}}^{b_{k}}+\mathbf{J}_{b_{a}}^{\alpha} \delta \mathbf{b}_{a_{k}}+\mathbf{J}_{b_{w}}^{\alpha} \delta \mathbf{b}_{w_{k}} \\ \boldsymbol{\beta}_{b_{k+1}}^{b_{k}} & \approx \hat{\boldsymbol{\beta}}_{b_{k+1}}^{b_{k}}+\mathbf{J}_{b_{a}}^{\beta} \delta \mathbf{b}_{a_{k}}+\mathbf{J}_{b_{w}}^{\beta} \delta \mathbf{b}_{w_{k}} \\ \boldsymbol{\gamma}_{b_{k+1}}^{b_{k}} & \approx \hat{\boldsymbol{\gamma}}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c} 1 \\ \frac{1}{2} \mathbf{J}_{b_{w}}^{\gamma} \delta \mathbf{b}_{w_{k}} \end{array}\right] \end{aligned}$

原则上 $\mathbf{q}_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes \mathbf{q}_{b_{k}}^{c_{0}}$ 应该等于 $\boldsymbol{\gamma}_{b_{k}}^{b_{k+1}}$ ,但因为存在噪声, $\mathbf{q}_{b_{k+1}}^{c_{0}}{ }^{-1} \otimes \mathbf{q}_{b_{k}}^{c_{0}} \otimes \boldsymbol{\gamma}_{b_{k+1}}^{b_{k}}$ 不等于单位四元数.

在这里插入图片描述
对应到代码：
$J_{b_{w}}^{\gamma} \delta b_{w}=2\left(\hat{\gamma}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{\text {vec }}$
两侧乘以 ${J_{b_{w}}^{\gamma}} ^T$ ，用LDLT分解求得 $\delta b_{w}$ 。

代码中的Quaterniond q_ij 就是 $q_{b_{k+1}}^{b_{k}}=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c 0}$
tmp_A就是 $J_{b_{w}}^{\gamma}$
tmp_B 就是 $2\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k+1}}^{b_{k}}\right)_{v e c}=2\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c}$
根据上面的代价函数构造 $A x = B$ 即
$J_{b_{w}}^{\gamma T} J_{b_{w}}^{\gamma} \delta b_{w}=2 J_{b_{w}}^{\gamma T}\left(\hat{r}_{b_{k+1}}^{b_{k}}{ }^{-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c}$
然后采用LDLT分解求得 $\delta b_{w}$ 。

/**
 * @brief   陀螺仪偏置校正  
 * @optional    根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
 *              主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
 *              注意得到了新的Bias后对应的预积分需要repropagate
 * @param[in]   all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
 * @param[out]  Bgs 陀螺仪偏置
 * @return      void
*/
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();

        //R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 转换为四元数 q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
        Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
        //tmp_A = J_j_bw
        tmp_A = frame_j->second.pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
        //tmp_b = 2 * (r^bk_bk+1)^-1 * (q^c0_bk)^-1 * (q^c0_bk+1)
        //      = 2 * (r^bk_bk+1)^-1 * q_ij
        tmp_b = 2 * (frame_j->second.pre_integration->delta_q.inverse() * q_ij).vec();
        //tmp_A * delta_bg = tmp_b
        // 其实就是A^T * A * x = A^T * b,在求解Ax=b的最小二乘解时，两边通乘A矩阵的转置得到的A^T * A一定是可逆的
        A += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    //LDLT方法
    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;
    // 得到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]);
    }
}

6. 视觉惯性对齐

在这里插入图片描述
$\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}$ 维度为6x1

$\mathbf{H}_{b_{k+1}}^{b_{k}}$ 维度为6x10

该部分需要优化的变量有速度（ $v^{b_k}_{b_k}$ 表示第k帧图像在其IMU坐标系下的速度），重力加速度（ $g^{c_0}$ 表示在第0帧相机坐标系下的重力向量）和尺度
$\mathcal{X}_{I}=\left[\mathbf{v}_{b_{0}}^{b_{0}}, \mathbf{v}_{b_{1}}^{b_{1}}, \ldots \mathbf{v}_{b_{n}}^{b_{n}}, \mathbf{g}^{c_{0}}, s\right]$ 在这一过程中需要求解每一帧的速度, $c_0$ 下的重力方向和尺度

对应到代码，主要就是对于AX=b的填充：
tmp_A： $\mathbf{H}_{b_{k+1}}^{b_{k}}$
tmp_b : $\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}$

/**
 * https://blog.csdn.net/try_again_later/article/details/104783107#t11
 * @brief   计算尺度，重力加速度和速度 （博客6.0，6.2，6.3）
 * @optional    速度、重力向量和尺度初始化Paper -> V-B-2
 *              相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘
 *              重力细化 -> Paper V-B-3    
 * @param[in]   all_image_frame 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息
 * @param[out]  g 重力加速度
 * @param[out]  x 待优化变量，窗口中每帧的速度V[0:n]、重力g、尺度s
 * @return      void
*/
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    //优化量x的总维度 3维速度×n + 3维重力 + 1维尺度
    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;

        // 论文公式(10)(11)
        // tmp_A(6,10) = H^bk_bk+1 = [-I*dt           0             (R^bk_c0)*dt*dt/2   (R^bk_c0)*((p^c0_ck+1)-(p^c0_ck))  ] 
        //                           [ -I    (R^bk_c0)*(R^c0_bk+1)      (R^bk_c0)*dt                  0                    ]
        // tmp_b(6,1 ) = z^bk_bk+1 = [ (a^bk_bk+1)+(R^bk_c0)*(R^c0_bk+1)*p^b_c-p^b_c , (b^bk_bk+1)]^T
        // tmp_A * x = tmp_b 求解最小二乘问题
        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();

        // Hx = b 使用LDLT分解求解：H^T * H * x = H^T * b
        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);

6.1 推导

由平移的预积分量:
在这里插入图片描述

从而得到由平移预积分量构成的线性方程(上半部分)

由速度的预积分量(也是将世界系转到 $c_0$ 系):

在这里插入图片描述

6.2 如何将每一帧的 $\mathcal{x}_{I}$ 集成到 $\mathcal{X}_{I}$ 中

在这里插入图片描述
6 : 3维的vk + 3维的vk+1 4：3维的重力向量+1维的尺度
对应代码

		// 形成方阵
        MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;

        // 6: 3维的v_k + 3维的v_k+1 
        A.block<6, 6>(i * 3, i * 3) += r_A.topLeftCorner<6, 6>();
        b.segment<6>(i * 3) += r_b.head<6>();
        // 4: 3维的重力向量 + 1维的尺度
        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>();

6.3 重力方向修正

在这里插入图片描述

已知重力的大小,对重力进行微调(因为误差分散在其他各个量之中,需要慢慢修正).
用已知的重力大小*重力方向的矢量 $\overrightarrow{\hat{g}}$ ,将其认为是当前估计出的重力大小和方向来调整重力的方向.
因为重力大小是已知的.调整重力的方向就好像是在球面上进行移动,球的半径=重力大小固定.
调整重力方向的办法: 将重力在球面上的运动方向在切平面分解为两个正交单位向量b1,b2乘以各自的权重.从而将重力的自由度从3个自由度转换为2个自由度

在这里插入图片描述

$\mathbf{g}^{c_{0}}=g \times \hat{\overline{\mathbf{g}}}^{c_{0}}+w_{1} \mathbf{b}_{1}+w_{2} \mathbf{b}_{2}$

其中 $\hat{\overline{\mathbf{g}}}$ 表示上一步估计出的重力方向的单位向量 $\mathbf{b}_{1}$ 和 $\mathbf{b}_{2}$ 是上一步估计出的重力向量 $\mathbf{g}$ 切平面上的一组单位正交基, $w_{1}$ 和 $w_{2}$ 是两个正交基上的扰动值。

$\left[\begin{array}{cccc} -\mathbf{I} \Delta t_{k} & 0 & \frac{1}{2} \mathbf{R}_{c_{0}}^{b_{k}} \Delta t_{k}^{2} \mathbf{b} & \mathbf{R}_{c_{0}}^{b_{k}}\left(\mathbf{p}_{c_{k+1}}^{c_{0}}-\mathbf{p}_{c_{k}}^{c_{0}}\right) \\ -\mathbf{I} & \mathbf{R}_{c_{0}}^{b_{k}} \mathbf{R}_{b_{k+1}}^{c_{0}} & \mathbf{R}_{c_{0}}^{b_{k}} \Delta t_{k} \mathbf{b} & 0 \end{array}\right]\left[\begin{array}{c} \mathbf{v}_{b_{k}}^{b_{k}} \\ \mathbf{v}_{b_{k+1}}^{b_{k+1}} \\ \mathbf{w} \\ s \end{array}\right]=\left[\begin{array}{c} \hat{\alpha}_{b_{k+1}}^{b_{k}}+\mathbf{R}_{c_{0}}^{b_{k}} \mathbf{R}_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}-\frac{1}{2} \mathbf{R}_{c_{0}}^{b_{k}} \Delta t_{k}^{2} \hat{\overline{\mathbf{g}}}^{c_{0}} \\ \beta_{b_{k+1}}^{b_{k}}-\mathbf{R}_{c_{0}}^{b_{k}} \Delta t_{k} \hat{\overline{\mathbf{g}}}^{c_{0}} \end{array}\right]$

对比

将这个重新参数化的重力代入到上一步求解的线性方程中, 可以得到对应的 $w_{1}$ 和 $w_{2}$ , 反复迭代几次, 最终会得到一个收敛的 $\hat{\mathbf{g}}^{c_{0}}$ 。
加入重力扰动后, 线性方程转变为

其中 $\mathbf{b}=\left[\mathbf{b}_{1}, \mathbf{b}_{2}\right], \mathbf{w}=\left[w_{1}, w_{2}\right]^{T}$

针对代码：b = (tmp - a * (a.transpose() * tmp)).normalized();

$P=\frac{a a^{T}}{a^{T} a}=\frac{a a^{T}}{\|a\|^{2}}$ 称为 $a$ 的投影矩阵。
对任意一个向量 $b$ , 它投影到 $a$ 上的向量一定是: $b$ 在 $a$ 上的投影 $=\pi_{a} b=P b$
所以上述代码相当于b向量 = tmp向量指向tmp在a向量上的投影向量的向量，这也保证了b向量一定垂直a向量

// 在半径为G的半球找到切面的一对正交基 -> Algorithm 1
MatrixXd TangentBasis(Vector3d &g0)
{
    Vector3d b, c;
    // 初始化估计出的重力方向的单位向量
    Vector3d a = g0.normalized();
    Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    // b向量 = tmp向量指向tmp在a向量上的投影向量的向量，保证了b向量一定垂直a向量
    b = (tmp - a * (a.transpose() * tmp)).normalized();
    // c向量与b向量和a向量都垂直
    c = a.cross(b);
    MatrixXd bc(3, 2);
    bc.block<3, 1>(0, 0) = b;
    bc.block<3, 1>(0, 1) = c;
    return bc;
}

/**
 * @brief   重力矢量细化
 * @optional    重力细化，在其切线空间上用两个变量重新参数化重力 -> Paper V-B-3 
                g^ = ||g|| * (g^-) + w1b1 + w2b2 
 * @param[in]   all_image_frame 所有图像帧构成的map,图像帧保存了位姿，预积分量和关于角点的信息
 * @param[out]  g 重力加速度
 * @param[out]  x 待优化变量，窗口中每帧的速度V[0:n]、二自由度重力参数w[w1,w2]^T、尺度s
 * @return      void
*/
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    //g0 = (g^-)*||g||
    Vector3d g0 = g.normalized() * G.norm();
    Vector3d lx, ly;
    //VectorXd x;
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 2 + 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;

    for(int k = 0; k < 4; k++)//迭代4次
    {
        //lxly = b = [b1,b2]
        MatrixXd lxly(3, 2);
        // 在半径为G的半球找到切面的一对正交基 -> Algorithm 1
        lxly = TangentBasis(g0);
        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, 9);
            tmp_A.setZero();
            VectorXd tmp_b(6);
            tmp_b.setZero();

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

            // tmp_A(6,9) = [-I*dt           0             (R^bk_c0)*dt*dt*b/2   (R^bk_c0)*((p^c0_ck+1)-(p^c0_ck))  ] 
            //              [ -I    (R^bk_c0)*(R^c0_bk+1)      (R^bk_c0)*dt*b                  0                    ]
            // tmp_b(6,1) = [ (a^bk_bk+1)+(R^bk_c0)*(R^c0_bk+1)*p^b_c-p^b_c - (R^bk_c0)*dt*dt*||g||*(g^-)/2 , (b^bk_bk+1)-(R^bk_c0)dt*||g||*(g^-)]^T
            // tmp_A * x = tmp_b 求解最小二乘问题
            tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
            tmp_A.block<3, 2>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity() * lxly;
            tmp_A.block<3, 1>(0, 8) = 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] - frame_i->second.R.transpose() * dt * dt / 2 * g0;

            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, 2>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity() * lxly;
            tmp_b.block<3, 1>(3, 0) = frame_j->second.pre_integration->delta_v - frame_i->second.R.transpose() * dt * Matrix3d::Identity() * g0;


            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<3, 3>() += r_A.bottomRightCorner<3, 3>();
            b.tail<3>() += r_b.tail<3>();

            A.block<6, 3>(i * 3, n_state - 3) += r_A.topRightCorner<6, 3>();
            A.block<3, 6>(n_state - 3, i * 3) += r_A.bottomLeftCorner<3, 6>();
        }
            A = A * 1000.0;
            b = b * 1000.0;
            x = A.ldlt().solve(b);
            //dg = [w1,w2]^T
            VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm();
            //double s = x(n_state - 1);
    }   
    g = g0;
}

6.4 其他

推导: 把平移对齐到滑窗第0帧

在这里插入图片描述
对应代码

// 所有的状态对齐到第0帧的IMU坐标系
    for (int i = frame_count; i >= 0; i--)
        //Ps转变为第i帧imu坐标系到第0帧imu坐标系的变换
        Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);

每天都在努力学习SLAM的小黑

关注

5
点赞
踩
18

收藏

觉得还不错? 一键收藏
5
评论
VINS-MONO：VIO初始化部分原理推导+代码解析

目录1.为什么需要初始化1.1 需要优化哪些变量1.2 初始化不解决哪些变量1.为什么需要初始化优化问题非常依赖初始值。防止陷入局部最小值与真实值越近迭代次数越少1.1 需要优化哪些变量优化变量: 地图点,p,v,q,零偏,外参vins初始值不求解加速度计的零偏(存在重力影响,也不影响优化结果)和外参的平移(人工测量)1.2 初始化不解决哪些变量并不需要一个非常准确的外参,因为在非线性优化中会不断去优化加速度计bias很难从重力估计中分离出来,忽略加速度计bias..
复制链接

扫一扫