VINS中的速度-重力-尺度初始化(1)

VINS中速度-重力-尺度初始化

\(\quad\)在 VINS 中初始化中，需要对初始化帧的速度变量，重力，以及尺度因子进行初始化。则需要估计的变量为

\[\mathcal{X}_{I}^{3(n+1)+3+1}=\left[v_{b_{0}}^{b_{0}}, v_{b_{1}}^{b_{1}}, \ldots, v_{b_{n}}^{b_{n}}, g^{c_{0}}, s\right] \]

\(\quad\) \(3(n+1)\)是因为有 \(n+1\) 个 \(3\) 维的速度变量，\(3\)是重力方向，\(1\) 是尺度因子。其中, \(v_{k}^{b_{k}}\) 表示 \(\mathrm{k}\) 时刻body坐标系下的速度在body系下的表示 \(\boldsymbol{g}^{c_{0}}\) 为重力向量在第 \(0\) 帧相机坐标系下的表示。\(s\) 表示尺度。在预积分中我们有如下约束。该约束是在世界坐标系 \(w\) 下。意义为 \(i,j\) 帧位移和速度的增量，也是我们预积分的估计量。

\[\begin{array}{c} 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}} \end{array} \]

\(\quad\)在 vio 的初始化中，我们可以使用 \(c_{0}\) 代替 \(w\) , 并且将尺度 \(s\) 引入。得到下式：

\[\begin{array}{c} \alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(s p_{b_{k+1}}^{c_{0}}-s p_{b_{k}}^{c_{0}}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}\right) \\ \beta_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}+g^{c_{0}} \Delta t_{k}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}}\right) \end{array} \]

对于位置的预积分 \(\alpha\), 可以根据相机和IMU的测量得到下如下残差表示：

\[\begin{array}{c} \delta \alpha_{b_{k+1}}^{b_{k}}=\alpha_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}}\left(s p_{b_{k+1}}^{c_{0}}-s p_{b_{k}}^{c_{0}}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}-R_{b_{k}}^{c_{0}} b_{b_{k}}^{b_{k}} \Delta t_{k}\right) \\ =\alpha_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}}\left(s p_{c_{k+1}}^{c_{0}}-R_{b_{k+1}}^{c_{0}} p_{c}^{b}-\left(s p_{c_{k}}^{c_{0}}-R_{b_{k}}^{c_{0}} p_{c}^{b}\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) \\ =\alpha_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}} s\left(p_{c_{k+1}}^{c_{0}}-p_{c_{k}}^{c_{0}}\right)+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}+v_{b_{k}}^{b_{k}} \Delta t_{k}-\frac{1}{2} R_{c_{0}}^{b_{k}} g^{c_{0}} \Delta t_{k}^{2}=0_{3 \times 1} \end{array} \]

上式中 \(v_{b_{k}}^{b_{k}} , s , g^{c 0}\) 是待优化变量，将其转换成 \(H x=b\) 的形式（待优化变量都放到等式左边）:

\[R_{c_{0}}^{b_{k}}\left(p_{c_{k+1}}^{c_{0}}-p_{c_{k}}^{c_{0}}\right) s-v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2} g^{c_{0}}=\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b} \]

写成矩阵形式如下:

\[\left[\begin{array}{llll} -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) \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]=\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b} \]

同理，对于速度的预积分 \(\beta\) ，最后，可以得到下式:

\[\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]=\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} \\ \beta_{b_{k+1}}^{b_{k}} \end{array}\right] \]

其实也就是:

\[H \mathcal{X}_{I}=b \]

其中\(\beta\)与\(\alpha\)是预积分的测量值，这个公式我们将其等式两边同时乘以\(H^{T}\)，然后使用LDLT进行求解得到待优化的量。

下面来看下程序

• 首先 初始化一些变量

int all_frame_count = all_image_frame.size();
// 这里初始化变量的维度和前面我们解释的基本一致
int n_state = all_frame_count * 3 + 3 + 1;

// A = H^T * H 维度也就是 (n_state,n_state)
MatrixXd A{n_state, n_state};
A.setZero();
// b = H^T * b 维度也就是 (n_state, 1)，这个是最终的 b,注意和循环中的那个 b 区分
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_i 为 b_k, frmae_j 为 b_{k+1}
    frame_j = next(frame_i);
	
    // tmp_A=H 为 (6,10) 的维度
    MatrixXd tmp_A(6, 10);
    tmp_A.setZero();
    // tmp_b=b 为 (6,1) 的维度
    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();

    // 构造 H^T * H
    MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
    // 构造 H^T * b
    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()) > 0.5 || 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;
*/