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;
*/