Vins-Mono 论文 && Coding 一 5(2). 初始化: visual-inertial 对齐

最新推荐文章于 2022-10-29 10:40:15 发布

scofield_ly

最新推荐文章于 2022-10-29 10:40:15 发布

阅读量252

点赞数 1

分类专栏： # vio 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40072161/article/details/110694169

版权

vio 专栏收录该内容

13 篇文章 1 订阅

订阅专栏

一、alignment 图解

待优化变量：

cam-imu 外参： $q^b_c$ , $p^b_c$

尺度： $s$

二、外参转换

由 sfm 求得的 camera 的 pose 变换得到 imu 的 pose

$q^{c_0}_{b_{k}} =q^{c_0}_{c_k}\otimes(q^b_c)^{-1}$ (1)

$s\bar{p}^{c_0}_{b_{k}} =s\bar{p}^{c_0}_{c_{k}} -R^{c_0}_{b_k}p^b_c$ (2)

三、Gyroscope Bias Calibration

1. 原理

修正 gyroscope bias, 使得预积分求得的旋转分量和 sfm 求出的旋转分量误差最小

这儿仅仅一次迭代

(1). 目标函数

$\min_{\delta b_\omega}||q^{c_0-1}_{b_{k+1}}\otimes q^{c_0}_{b_{k}} \otimes \gamma^{b_k}_{b_{k+1}} ||^2$ (3)

其中 $q^{c_0-1}_{b_{k+1}} \otimes q^{c_0}_{b_{k}}=q^{b_k+1}_{b_{k}}$ ，是 sfm 求出的 delta translation

$\gamma^{b_k}_{b_{k+1}}$ 是预积分求出的旋转分量

(2). 线性化

$\gamma^{b_k}_{b_{k+1}}\approx \hat{\gamma}^{b_k}_{b_{k+1}}\otimes \bigl(\begin{matrix} 1 \\ \frac{1}{2}J^\gamma_{b_{\omega}} \delta b_\omega\end{matrix}\bigr)$ (4)

(3). 线性方程

$\hat{\gamma}^{b_k}_{b_{k+1}}\otimes \bigl(\begin{matrix} 1 \\ \frac{1}{2}J^\gamma_{b_{\omega}} \delta b_\omega\end{matrix}\bigr)=q^{c_0-1}_{b_{k}} \otimes q^{c_0}_{b_{k+1}}$ (5)

$\bigl(\begin{matrix} 1 \\ \frac{1}{2}J^\gamma_{b_{\omega}} \delta b_\omega\end{matrix}\bigr)=\hat{\gamma}^{b_k-1}_{b_{k+1}} \otimes q^{c_0-1}_{b_{k}} \otimes q^{c_0}_{b_{k+1}}$ (6)

只考虑虚部，有：

$J^\gamma_{b_{\omega}} \delta b_\omega=2(\hat{\gamma}^{b_k-1}_{b_{k+1}} \otimes q^{c_0-1}_{b_{k}} \otimes q^{c_0}_{b_{k+1}}).vec$ (7)

上式就得到了常见的 $Ax=b$ ，通过转换为： $A^TAx=A^Tb$ ，可以求解得到 bias 修正量

(4). 重新预积分

加上 bias 增量，重新计算预积分

2. 代码实现

void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)
{
   ......
   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;  // A^TA
        b += tmp_A.transpose() * tmp_b;  // A^Tb

    }
    delta_bg = A.ldlt().solve(b);  // 求解 delta bias

    // bias 修正, 以及重新计算预积分
    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]);
    }
}

四、Velocity, Gravity Vector, and Metric Scale Initialization

1. 原理

(1). 状态变量

$x_I=[v^{b_0}_{b_0}, v^{b_1}_{b_1}, ..., v^{b_n}_{b_n}, g^{c_0}, s]$ (8)

其中 $v^{b_k}_{b_k}$ 是第 $k$ 帧的速度， $g^{c_0}$ 是加速度分量在 sfm 世界坐标系下的分量，s 是 sfm 计算的 translation 相比于 imu 预积分结果的尺度

(2). sfm结果，位置、速度预积分量的相对关系

旋转预积分量已经在三、中使用了

$\alpha^{b_k}_{k_{k+1}}=R^{b_k}_{c_0}(s(\bar{p}^{c_0}_{b_{k+1}}- \bar{p}^{c_0}_{b_{k}})+\frac{1}{2}g^{c_0}\Delta t^2_k-R^{c_0}_{b_k}v_{b_k}^{b_k}\Delta t_k)$ (9)

$\beta^{b_k}_{k_{k+1}}=R^{b_k}_{c_0}(R^{c_0}_{b_{k+1}}v^{b_{k+1}}_{{b_{k+1}}}+g^{c_0}\Delta t_k-R^{c_0}_{b_k}v_{b_k}^{b_k})$ (10)

写成 $z=Hx$ 的形式

$\hat{z}^{b_k}_{b_{k+1}}=\bigl(\begin{smallmatrix} \hat{\alpha}^{b_k}_{b_{k+1}}-p^b_c+R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}p^b_c\\ \hat{\beta}^{b_k}_{b_{k+1}} \end{smallmatrix}\bigr)=H^{b_k}_{b_{k+1}}x_I+b^{b_k}_{b_{k+1}}$ (11)

雅可比矩阵为

$H^{b_k}_{b_{k+1}}=\bigl(\begin{smallmatrix} -I\Delta t_k & 0 & \frac{1}{2}R^{b_k}_{c_0}\Delta t^2_k & R^{b_k}_{c_0}(\bar{p}^{c_0}_{c_{k+1}}-\bar{p}^{c_0}_{c_{k}}) \\ -I & R^{b_k}_{c_0}R^{c_0}_{b_{k+1}} & R^{b_k}_{c_0}\Delta t_k & 0 \end{smallmatrix}\bigr)$ (12)

(3). 求解

同 Gyroscope Bias Calibration

2. 代码实现

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;

        // H,z
        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];
        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;
        

        Matrix<double, 6, 6> cov_inv = Matrix<double, 6, 6>::Zero();
        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;
    g = x.segment<3>(n_state - 4);
    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;
    if(s < 0.0 )
        return false;   
    else
        return true;
}

五、Gravity Refinement

1. 原理

模型还是和 3 一样，不过加入重力加速度模长固定的约束，重力加速度被建模为2 个自由度

重力加速的扰动 $\delta g$ 作用于当前重力加速度

$\hat{g}^{3\times 1}= ||g||\cdot \bar{\hat{g}}^{3\times 1} + \omega_1b_1^{3\times 1} + \omega_2 b_2^{3\times 1}=||g||\cdot \bar{\hat{g}}^{3\times 1}+\delta g$ (13)

其中， $\bar{\hat{g}}^{3\times 1}$ 是当前重力加速度的单位方向向量， $b_1^{3\times 1}, b_2^{3\times 1}$ 是 $\bar{\hat{g}}^{3\times 1}$ 的 tangent space 的正交向量基， $\omega_1, \omega_2$ 是重力加速的扰动的二维坐标

2. 代码实现

循环迭代

// G: 已知重力加速度(0, 0, 9.8)
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
{
    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++)
    {
        MatrixXd lxly(3, 2);
        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);

            // velocity, delta_g, s
            MatrixXd tmp_A(6, 9);
            tmp_A.setZero();
            VectorXd tmp_b(6);
            tmp_b.setZero();

            double dt = frame_j->second.pre_integration->sum_dt;
 
            // H, z
            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_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);
            VectorXd dg = x.segment<2>(n_state - 3);
            g0 = (g0 + lxly * dg).normalized() * G.norm(); // 作用扰动，保证模长等于 G 的模长
            //double s = x(n_state - 1);
    }   
    g = g0;
}

六、Completing Initialization

we can get the rotation $q^w_{c_0}$ between the world frame and the camera frame $c_0$ by rotating the gravity to the z-axis

Matrix3d R0 = Utility::g2R(g);
double yaw = Utility::R2ypr(R0 * Rs[0]).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
g = R0 * g;
//Matrix3d rot_diff = R0 * Rs[0].transpose();
Matrix3d rot_diff = R0;
for (int i = 0; i <= frame_count; i++) {
    Ps[i] = rot_diff * Ps[i];
    Rs[i] = rot_diff * Rs[i];
    Vs[i] = rot_diff * Vs[i];
}