视觉与IMU对齐

视觉与IMU对齐

修正陀螺仪的偏置

  从陀螺仪传感器获取到的角速度，初始化阶段，认为角速度存在一个偏置$b_{\omega }$。
  已知由图像计算的两帧陀螺仪的旋转四元数：$q_{b_{k+1}}^{c_0},q_{b_k}^{c_0}$ 和根据陀螺仪数据预积分得到的从第$k$帧到$k+1$帧的旋转四元数：${\hat{\gamma }}_{b_{k+1}}^{b_k}$。

保证根据视觉计算出的两帧之间的旋转变化与陀螺仪积分得到的旋转变化相等，得到如下条件约束：
$$\underset{\delta b_{\omega }}{\min }\sum \Vert {q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}{\Vert }^2$$
其中：
$${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]$$
以上公式的最小值为单位四元数，因此可将公式写为：
$${q_{b_{k+1}}^{c_0}}^{-1}\otimes q_{b_k}^{c_0}\otimes {\gamma }_{b_{k+1}}^{b_k}=\left[\begin{array}{c}1\\ 0\end{array}\right]$$
$${\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]={q_{b_k}^{c_o}}^{-1}\otimes q_{b_{k+1}}^{c_0}$$
$$\Rightarrow \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }\end{array}\right]={{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {q_{b_k}^{c_0}}^{-1}\otimes q_{b_{k+1}}^{c_0}$$
只取上式虚部再进行最小二乘求解：
$${J_{b_{\omega }}^{\gamma }}^T{J}_{b_{\omega }}^{\gamma }\delta b_{\omega }=2{J_{b_{\omega }}^{\gamma }}^T{\left({{\hat{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {q_{b_k}^{c_0}}^{-1}\otimes q_{b_{k+1}}^{c_0}\right)}_{.vec}$$

VINS中对应代码

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

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

已知：
$$p_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+\int {\int }_{t\in \left[t_k,t_{k+1}\right]}\left(R_t^w\left({\hat{a}}_t-b_{a_t}-n_a\right)-g^w\right){dt}^2$$
$$v_{b_{k+1}}^w=v_{b_k}^w+{\int }_{t\in \left[t_k,t_{k+1}\right]}\left(R_t^w\left({\hat{a}}_t-b_{a_t}-n_a\right)-g^w\right)dt$$
$$q_{b_{k+1}}^w=q_{b_k}^w\otimes {\int }_{t\in \left[t_k,t_{k+1}\right]}\frac{1}{2}\Omega \left({\hat{\omega }}_t-b_{{\omega }_t}-n_{\omega }\right)q_t^{b_k}dt$$
其中：
$$\Omega \left(\omega \right)=\left[\begin{array}{cc}{\left[\omega \right]}_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right],\left[\omega \right]=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
上面的三个式子写成两部分，第一部分为已知数据，第二部分为需要预积分得到部分：
$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}\left(p_{b_k}^w+v_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w{\Delta t_k}^2\right)+{\alpha }_{b_{k+1}}^{b_k}$$
$$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}\left(v_{b_k}^w+g^w{\Delta t}_k\right)+{\beta }_{b_{k+1}}^{b_k}$$
$$q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}$$
因为把相机的0帧的位姿当做世界坐标系的初始值，因此有
$$R_{b_K}^w=R_{b_k}^{c_0}\Rightarrow R_w^{b_k}={R_{b_k}^w}^{-1}={R_{b_k}^{c_0}}^{-1}=R_{c_0}^{b_k}$$

---

$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}=R_w^{b_k}\left(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Delta t_k+\frac{1}{2}{g}^w{\Delta t_k}^2\right) \\ =R_{c_0}^{b_k}\left(s\left(p_{b_{k+1}}^{c_0}-p_{b_k}^{c_0}\right)+\frac{1}{2}{g}^{c_0}{\Delta t_k}^2-v_{b_k}^{c_0}\Delta t_k\right) \\ =R_{c_0}^{b_k}\left(s\left(p_{b_{k+1}}^{c_0}-p_{b_k}^{c_0}\right)+\frac{1}{2}{g}^{c_0}{\Delta t_k}^2-R_{b_k}^{c_0}{v}_{b_k}^{b_k}\Delta t_k\right)\text{\hspace{1em}(1)} \end{gathered}$$

---

$${\beta }_{b_{k+1}}^{b_k}=R_w^{b_k}\left(v_{b_{k+1}}^w-v_{b_k}^w-g^w\Delta t_k\right)$$
$$=R_{c_0}^{b_k}\left(v_{b_{k+1}}^{c_0}-v_{b_k}^{c_0}+g^{c_0}\Delta t_k\right)$$
$$=R_{c_0}^{b_k}\left(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\right)\text{\hspace{1em}(2)}$$
  从$C_0$坐标系到 IMU坐标系和世界坐标系转换位置（P），速度（V）时需要引入尺度。设从IMU到相机的旋转为$q_c^b$，平移量为$p_c^b$，因此有以下等式：
$$q_{b_k}^{c_0}=q_{c_K}^{c_0}\otimes {\left(q_c^b\right)}^{-1}$$
$$s{\overline{p}}_{k_k}^{c_0}=s{\overline{p}}_{c_k}^{c_0}-R_{b_k}^{c_0}{p}_c^b\text{\hspace{1em}(3)}$$
进而有：
$$s{\overline{p}}_{b_{k+1}}^{c_0}=s{\overline{p}}_{c_{k+1}}^{c_0}-R_{b_{k+1}}^{c_0}{p}_c^b\text{\hspace{1em}(4)}$$
4式减3式得：
$$s\left({\overline{p}}_{b_{k+1}}^{c_0}-{\overline{p}}_{b_k}^{c_0}\right)=s\left({\overline{p}}_{c_{k+1}}^{c_0}-{\overline{p}}_{c_k}^{c_0}\right)+\left(R_{b_k}^{c_0}-R_{b_{k+1}}^{c_0}\right)p_c^b\text{\hspace{1em}(5)}$$
5式代入1式得到：
$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}= \\ {R}_{c_0}^{b_k}\left(s\left({\overline{p}}_{c_{k+1}}^{c_0}-{\overline{p}}_{c_k}^{c_0}\right)+\frac{1}{2}{g}^{c_0}{\Delta t_k}^2-R_{b_k}^{c_0}{v}_{b_k}^{b_k}\Delta t_k\right)+p_c^b-R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b\text{\hspace{1em}(6)} \end{gathered}$$
对6式整理得到：
$$\begin{gathered} {\alpha }_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b= \\ {R}_{c_0}^{b_k}\left(s\left({\overline{p}}_{c_{k+1}}^{c_0}-{\overline{p}}_{c_k}^{c_0}\right)+\frac{1}{2}{g}^{c_0}{\Delta t_k}^2-R_{b_k}^{c_0}{v}_{b_k}^{b_k}\Delta t_k\right)\text{\hspace{1em}(7)} \end{gathered}$$
用***2式、6式***整理得到：
$$\begin{gathered} \left[\begin{array}{c}{\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{array}\right]= \\ \left[\begin{array}{cccc}-I\Delta t_k& 0& \frac{1}{2}{R}_{c_0}^{b_k}{\Delta t_k}^2& R_{c_0}^{b_k}\left(p_{c_{k+1}}^{c_0}-p_{c_k}^{c_0)}\right)\\ -I& R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}& R_{c_0}^{b_k}\Delta t_k& 0\end{array}\right]\left[\begin{array}{c}{v}_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s\end{array}\right]=H_{b_{k+1}}^{b_k}{X}_I \end{gathered}$$
根据预积分可以得到${\alpha }_{b_{k+1}}^{b_k},{\beta }_{b_{k+1}}^{b_k}$
令$$z_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\alpha }_{b_{k+1}}^{b_k}\\ {\beta }_{b_{k+1}}^{b_k}\end{array}\right]$$
建立求解$X_I$的约束
$$\underset{X_I}{\min }\sum _{k\in B}\Vert z_{b_{k+1}}^{b_k}-H_{b_{k+1}}^{b_k}{X}_I\Vert$$
利用最小二乘法求解：
$${H_{b_{k+1}}^{b_k}}^T{H}_{b_{k+1}}^{b_k}{X}_I={H_{b_{k+1}}^{b_k}}^T{z}_{b_{k+1}}^{b_k}$$
即可得到$X_I$

改进重力向量g的值

通过该公式得到计算出的重力$\hat{g}$在切平面上的两个分量，其中$b$是与真实重力$g$和计算出的重力$\hat{g}$在同一个平面上，$a$是与$\hat{g}$和$b$平面垂直的向量。

MatrixXd TangentBasis(Vector3d &g0)
{
    Vector3d b, c;
    Vector3d a = g0.normalized();
    Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    // a和tmp可以看做半径为1的半球，球心在原点。
    // 因为a与tmp都是单位向量，并且tmp是(0, 0, 1)，所以a.transpose() * tmp
    // 计算的是a在tmp上投影的长度，a*(a.transpose() * tmp)是与a同向的向量，
    // 可以看做半径为(a.transpose() * tmp)的球，因此
    // (tmp - a * (a.transpose() * tmp)).normalized()是与a向量垂直，
    // 并且是半径为1的半球的切线，切点为a, a.cross(b) a叉乘b，垂直a,b平面的向量
    b = (tmp - a * (a.transpose() * tmp)).normalized();
    c = a.cross(b);
    MatrixXd bc(3, 2);
    bc.block<3, 1>(0, 0) = b;
    bc.block<3, 1>(0, 1) = c;
    return bc;
}

因为$\hat{g}$并不是竖直向下的，所以，认为$g^w=g+l_x+l_y$
代入公公式：
$$\begin{gathered} \left[\begin{array}{c}{\alpha }_{b_{k+1}}^{b_k}+R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}{p}_c^b-p_c^b-\frac{1}{2}{R}_{c_0}^{b_k}{\Delta t_k}^2\left({g^w}_{norm}\ast G\right)\\ {\beta }_{b_{k+1}}^{b_k}-R_{c_0}^{b_k}{\Delta t}_{k}I\left({g^w}_{norm}\ast G\right)\end{array}\right] \\ =\left[\begin{array}{cccc}-I\Delta t_k& 0& \frac{1}{2}{R}_{c_0}^{b_k}{\Delta t_k}^2{I}_{3\times 3}{lxy}_{3\times 2}& R_{c_0}^{b_k}\left(p_{c_{k+1}}^{c_0}-p_{c_k}^{c0)}\right)\\ -I& R_{c_0}^{b_k}{R}_{b_{k+1}}^{c_0}& R_{c_0}^{b_k}\Delta t_k{I}_{3\times 3}{lxy}_{3\times 2}& 0\end{array}\right]\left[\begin{array}{c}{v}_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ \delta g^{c_0}\\ s\end{array}\right] \\ =H_{b_{k+1}}^{b_k}{X}_I \end{gathered}$$
根据最小二乘即可得到$\delta g$
$$\hat{g}=(\hat{g}+lxly\ast \delta g).normalized()\ast G$$
通过对上式的迭代即可得到$g$

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);
        
            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.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() * 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();

            MatrixXd r_A = tmp_A.transpose() * tmp_A;
            VectorXd r_b = tmp_A.transpose() * 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;
        //double s = x(n_state - 1);
    }
    g = g0;
}