vins estimator initial_aligment

xiaoma_bk

已于 2022-05-17 16:19:51 修改

阅读量261

点赞数

分类专栏： # vins 文章标签：线性代数矩阵算法

于 2022-05-17 15:50:24 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/124752815

版权

vins 专栏收录该内容

15 篇文章 3 订阅

订阅专栏

该文详细介绍了如何通过最小二乘法来校准陀螺仪的偏置，以及如何利用视觉信息初始化速度、重力向量和尺度因子。首先，通过相邻帧间旋转矩阵与IMU预积分旋转的对比，构建方程求解陀螺仪偏置。然后，通过比较相邻帧间位置和速度，计算尺度、重力加速度和速度，最后对重力向量进行细化。整个过程涉及到预积分的重新计算和优化变量的提取，确保了IMU数据与视觉数据的一致性。

摘要由CSDN通过智能技术生成

function

主要功能是：视觉与IMU对齐，初始化用
- 计算陀螺仪偏置：
  - 帧间旋转与 IMU递推旋转之差
  - 构建方程后，多组求解
- 计算尺度，重力加速度和速度
  - 帧间位姿变化与速度变化与imu递推之差
  - 构建方程后，多组求解

计算陀螺仪偏置-solveGyroscopeBias

简介：

brief：陀螺仪偏置校准
optional：根据视觉SFM的结果来校正陀螺仪Bias
- 主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
- 注意得到了新的Bias后对应的预积分需要repropagate
Param1：all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
Param2： Bgs 陀螺仪偏置

原理：

相邻帧间的旋转应等于 IMU 预积分的旋转值：
- ${\underset{\delta b_w}{min} \sum_{k \in B} \left \| {q^{co}_{b_{k+1}}}^{-1} \otimes q^{co}_{b_{k }} \otimes \gamma^{b_k}_{b_{k+1}} \right \|}$
其中： ${\gamma^{b_k}_{b_{k+1}} \approx \hat{\gamma}^{b_k}_{b_{k+1}} \otimes \begin{bmatrix} 1\\ \frac 1 2 J^{\gamma}_{bw} \delta b_w \end{bmatrix}}$
目标函数的最小值为单位四元数,所以可以将目标函数进一步写为：
- ${{q^{co}_{b_{k+1}}}^{-1} \otimes q^{co}_{b_{k}} \otimes \gamma^{b_k}_{b_{k+1}} = \begin{bmatrix}1\\ 0\end{bmatrix} }$
- ${\hat{\gamma}^{b_k}_{b_{k+1}} \otimes \begin{bmatrix} 1\\ \frac 1 2 J^{\gamma}_{bw} \delta b_w \end{bmatrix} = {q^{co}_{b_k}}^{-1} \otimes {q^{co}_{b_{k+1}}} \otimes \begin{bmatrix}1\\ 0\end{bmatrix} }$
- ${\begin{bmatrix} 1\\ \frac 1 2 J^{\gamma}_{bw} \delta b_w \end{bmatrix} ={\hat{\gamma}^{b_k}_{b_{k+1}}}^{-1} \otimes {q^{co}_{b_k}}^{-1} \otimes {q^{co}_{b_{k+1}}} \otimes \begin{bmatrix}1\\ 0\end{bmatrix} }$
只考虑虚部，则有：
- ${J^{\gamma}_{bw} \delta b_w =2({\hat{\gamma}^{b_k}_{b_{k+1}}}^{-1} \otimes {q^{co}_{b_k}}^{-1} \otimes {q^{co}_{b_{k+1}}} )_{vec}}$
将左边转换为正定矩阵，这样就可以用Cholesky进行分解：
- ${(J^{\gamma}_{bw})^T J^{\gamma}_{bw} \delta b_w =2(J^{\gamma}_{bw})^T({\hat{\gamma}^{b_k}_{b_{k+1}}}^{-1} \otimes {q^{co}_{b_k}}^{-1} \otimes {q^{co}_{b_{k+1}}} )_{vec}}$
求解出陀螺仪的bias后，需对预积分重新计算
实现：
- 计算的变量为：角速度偏差 ${Bg}$ ，构建： ${Ax =b}$
- 雅克比为 dq_dbg
- 构建求解方程：
  - ${ J^TJx = J^Tb}$
  - $delta\_b = A^{-1}b$
- LDLT方法：delta_bg = A.ldlt().solve(b);

具体实现见下：

/**
 * @brief   陀螺仪偏置校正
 * @optional    根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
 *              主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
 *              注意得到了新的Bias后对应的预积分需要repropagate
 * @param[in]   all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
 * @param[out]  Bgs 陀螺仪偏置
 * @return      void
*/
static void solveGyroscopeBias(std::map<double, ImageFrame> &all_image_frame, Eigen::Vector3d* Bgs)
{
    Eigen::Matrix3d A;
    Eigen::Vector3d b;
    Eigen::Vector3d delta_bg;
    A.setZero();
    b.setZero();
    std::map<double, ImageFrame>::iterator frame_i;
    std::map<double, ImageFrame>::iterator frame_j;

    // 遍历所有图像帧
    for (frame_i = all_image_frame.begin(); std::next(frame_i) != all_image_frame.end(); frame_i++)
    {
        frame_j = std::next(frame_i);
        Eigen::MatrixXd tmp_A(3, 3);
        tmp_A.setZero();
        Eigen::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);

        // 角增量对角偏置的偏导 dq_dbg
        //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 += tmp_A.transpose() * tmp_A;
        b += tmp_A.transpose() * tmp_b;

    }
    //LDLT方法
    delta_bg = A.ldlt().solve(b);
    LOG(INFO)<<"gyroscope bias initial calibration: " << delta_bg.transpose();

    // 更新Bgs 若多帧，则每个都更新
//    for (int i = 0; i <= WINDOW_SIZE; i++)
//        Bgs[i] += delta_bg;
    Bgs[0] += delta_bg;

    // Bgs 改变后，需 重新进行递推
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = std::next(frame_i);
        frame_j->second.pre_integration->repropagate(Eigen::Vector3d::Zero(), Bgs[0]);
    }
}

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

优化变量：

${X_I^{3(n+1)+3+1} =[v_{b_0}^{b_0},v_{b_1}^{b_1},\cdots,v_{b_n}^{b_n},g^{c_0},s]}$
维度：3(n+1)+3+1
其中， $g^{c_0}$ 为在第0帧Camera相机坐标系下的重力向量.

原理：

相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘
残差可以定义为向量两帧之间IMU的预积分增量 ${\alpha_{b_{k+1}^{b_k}}}$ ， ${\beta_{b_{k+1}^{b_k}}}$ ，与预测值之间的误差，可以写成：
$\gamma(\hat{z}_{b_{k+1}^{b_k}},X_I)=\begin{bmatrix} \delta \alpha_{b_{k+1}^{b_k}} \\ \delta \beta_{b_{k+1}^{b_k}} \end{bmatrix} = \begin{bmatrix} \alpha_{b_{k+1}^{b_k}}-R_{c_0}^{b_k}(s(\bar{p}_{b_{k+1}^{c_0}}-\bar{p}_{b_{k}^{c_0}})-R_{b_k}^{c_0}v_{b_k}^{b_k}\Delta t_k + \frac{1}{2}g^{c_0}\Delta t_k^2) \\ \beta_{b_{k+1}^{b_k}}-R_{c_0}^{b_k}(R_{b_{k+1}}^{c_0}v_{b_{k+1}}^{b_{k+1}}-R_{b_k}^{c_0}v_{b_k}^{b_k}+g^{c_0}\Delta t_k) \end{bmatrix} = \begin{bmatrix} 0_{3 \times 1} \\ 0_{3 \times 1} \end{bmatrix}}$
将body到c_0坐标系 ${s\bar{p}_{b_k}^{c_0}=s\bar{p}_{c_k}^{c_0}-R_{b_k}^{c_0}p_c^b}$ 带入 ${\gamma(\hat{z}_{b_{k+1}^{b_k}},X_I)}$ ，
想办法将变量提取出来，变为 ${H^{6\times 10}x^{10 \times 1}=b^{6 \times 1}}$ 的形式
${\begin{bmatrix} -I\Delta t_k & 0 & \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2 & R_{c_0}^{b_k}(\bar{p}_{b_{k+1}^{c_0}}-\bar{p}_{b_{k}^{c_0}}) \\ -I & R_{c_0}^{b_k}R_{b_{k+1}} & R_{c_0}^{b_k}\Delta t_k & 0 \end{bmatrix} \begin{bmatrix} v_{b_k}^{b_k} \\ v_{b_{k+1}^{b_{k+1}}} \\ g^{c_0} \\ s \end{bmatrix} = \begin{bmatrix} \alpha_{b_{k+1}^{b_k}} -p_c^b + R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b \\ \beta _{b_{k+1}^{b_k}} \end{bmatrix}}$
使用Cholosky分解求解 ${x_I}$ ： ${H^THx_I^{10 \times1}=H^Tb}$

代码：

/**
 * @brief   计算尺度，重力加速度和速度
 * @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
*/
static bool LinearAlignment(std::map<double, ImageFrame> &all_image_frame, Eigen::Vector3d &g, Eigen::VectorXd &x)
{
    int all_frame_count = all_image_frame.size();
    //优化量x的总维度
    int n_state = all_frame_count * 3 + 3 + 1;

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

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

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

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

        // 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 * Eigen::Matrix3d::Identity();
        tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Eigen::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) = - Eigen::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 * Eigen::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;

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

        Eigen::MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
        Eigen::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;
    LOG(INFO)<<"estimated scale:"<<s;

    g = x.segment<3>(n_state - 4);
    LOG(INFO)<<" 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;
    LOG(INFO)<<" refine:   " << g.norm() << " " << g.transpose();

    if(s < 0.0 )
        return false;
    else
        return true;
}

修正重力矢量，对应代码在 RefineGravity()

原理：

因为重力矢量的模型固定，因此将为两个自由度，可写成：
${\hat{g}^{3 \times1}= \left\|g\right\| \cdot \bar{\hat g}^{3 \times 1} + w_1 \vec{b_1}^{3 \times 1}+w_2\vec{b_2}^{3 \times 1}=\left\|g\right\| \cdot \bar{\hat g}^{3 \times 1} + \vec b^{3 \times 2} w^{2 \times 1}}$
将上式带入初始化问题中，即得到新 ${H^{6\times 9}x^{9 \times 1}=b^{6 \times 1}}$
${\begin{bmatrix} -I\Delta t_k & 0 & \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2 \vec b & R_{c_0}^{b_k}(\bar{p}_{b_{k+1}^{c_0}}-\bar{p}_{b_{k}^{c_0}}) \\ -I & R_{c_0}^{b_k}R_{b_{k+1}} & R_{c_0}^{b_k}\Delta t_k \vec b & 0 \end{bmatrix} \begin{bmatrix} v_{b_k}^{b_k} \\ v_{b_{k+1}^{b_{k+1}}} \\ w \\ s \end{bmatrix} = \begin{bmatrix} \alpha_{b_{k+1}^{b_k}} -p_c^b + R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b - \frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2||g|| \cdot \bar{\hat g} \\ \beta{b_{k+1}^{b_k}} - R_{c_0}^{b_k}\Delta t_k||g|| \cdot \bar{\hat g} \end{bmatrix} }$
就可得到了 ${C_0}$ 系下的重力向量 ${g^{c_0}}$ 旋转至惯性坐标系中的z轴方向，可以计算相机系到惯性系的旋转矩阵 ${q_{c_0}^w}$ ，这样就可以将所有变量调整至惯性世界系中。
找相切的两个向量：
- 若 ${\hat g \neq [1,0,0]}$ ，则 ${b_1 = normalize(\hat g \times [1,0,0])}$
- 否则， ${b_1 = normalize(\hat g \times [0,0,1])}$
- ${b_2 = \hat g \times b_1}$
Eigen::Vector normal
- Eigen::vector::norm() 二范数
- Eigen::vector::normalize() 把自身的各元素除以它的范数。返回值为void
- Eigen::vector::normalized() 与上面相似，只不过normalize()是对自身上做修改，而normalized()返回的是一个新的Vector/Matrix`，并不改变原有的矩阵。

代码：

/**
 * @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
*/
static void RefineGravity(std::map<double, ImageFrame> &all_image_frame, Eigen::Vector3d &g, Eigen::VectorXd &x)
{
    //g0 = (g^-)*||g||
    Eigen::Vector3d g0 = g.normalized() * G.norm();
    Eigen::Vector3d lx, ly;
    //VectorXd x;
    int all_frame_count = all_image_frame.size();
    int n_state = all_frame_count * 3 + 2 + 1;

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

    std::map<double, ImageFrame>::iterator frame_i;
    std::map<double, ImageFrame>::iterator frame_j;

    for(int k = 0; k < 4; k++)//迭代4次
    {
        //lxly = b = [b1,b2]
        Eigen::MatrixXd lxly(3, 2);
        lxly = TangentBasis(g0);
        int i = 0;
        for (frame_i = all_image_frame.begin(); std::next(frame_i) != all_image_frame.end(); frame_i++, i++)
        {
            frame_j = std::next(frame_i);

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


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

            Eigen::MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
            Eigen::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
            Eigen::VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm();
            //double s = x(n_state - 1);
    }
    g = g0;
}