VINS初始化转载

最新推荐文章于 2024-09-28 20:06:35 发布

Jack Ju

最新推荐文章于 2024-09-28 20:06:35 发布

阅读量508

点赞数

分类专栏： VIO

本文链接：https://blog.csdn.net/qq_40464599/article/details/112556490

版权

VIO 专栏收录该内容

9 篇文章 1 订阅

订阅专栏

vins_estimator中的相机-IMU对齐

https://blog.csdn.net/moyu123456789/article/details/103453897

本篇笔记紧接上一篇VINS-Mono代码阅读笔记（八）：vins_estimator中的相机-IMU初始化。阅读分析视觉惯导对齐部分的代码。相机-IMU对齐指的是将视觉SFM结构和IMU的预积分结果进行对齐，主要分为1）陀螺仪偏差的标定；2）速度、重力向量和尺度初始化；3）对重力进行修正三部分。效果如论文中提供的下图所示。

1.陀螺仪偏差的标定

1）论文中理论描述推导

从滑动窗口中根据视觉SFM可以获得两个连续的帧 $b_k,b_{k+1}$ 相对于相机第一帧图像之间的旋转 $q_{b_k}^{c_0}$ 和 $q_{b_{k+1}}^{c_0}$ ，从IMU预积分中可以获得相邻帧间的旋转 $\widehat{\gamma }_{b_{k+1}}^{b_k}$ 。理论上来讲，视觉SFM获得的相邻帧间旋转应该和IMU预积分计算出的相邻帧间旋转相等。因此，线性化IMU预积分项和陀螺仪偏差，可以得到最小化的代价函数如下：

$\large \underset{\delta b_w}{min}\underset{k\in \ss }{\sum }\left \|{q_{b_{k+1}}^{c_0}} ^{-1} \bigotimes q_{b_k}^{c_0}\bigotimes \gamma _{b_{k+1}}^{b_k}\right \|^2$

$\large \gamma _{b_{k+1}}^{b_k}\approx \widehat{\gamma }_{b_{k+1}}^{b_k}\bigotimes \begin{bmatrix} 1\\ \frac{1}{2}J_{b_w}^\gamma \delta b_w \end{bmatrix}$

其中， $\large \ss$ 表示了滑动窗口中所有帧的index。这样我们就获得了陀螺仪的偏差 $\large b_w$ 的初始化标定，然后使用新获得的陀螺仪偏差对IMU的预积分项进行重新积分。

由于损失函数的最小值为单位四元数，所以代价函数可以写为：

$\large q_{b_{k+1}}^{c_0}} ^{-1} \bigotimes q_{b_k}^{c_0}\bigotimes \gamma _{b_{k+1}}^{b_k}=\begin{bmatrix} 1\\ 0 \end{bmatrix}$

$\large \Rightarrow \gamma _{b_{k+1}}^{b_k}=q_{b_k}^{c_0}^{-1}\bigotimes q_{b_{k+1}}^{c_0}\bigotimes \begin{bmatrix} 1\\ 0 \end{bmatrix}$

$\large \Rightarrow \widehat{\gamma }_{b_{k+1}}^{b_k}\bigotimes \begin{bmatrix} 1\\ \frac{1}{2}J_{b_w}^\gamma \delta b_w \end{bmatrix}={q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0}\bigotimes \begin{bmatrix} 1\\ 0 \end{bmatrix}$

$\large \Rightarrow$ $\large \begin{bmatrix} 1\\ \frac{1}{2}J_{b_w}^\gamma \delta b_w \end{bmatrix}=(\hat{\gamma }_{b_{k+1}}^{b_k})^{-1}\bigotimes {q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0}\bigotimes \begin{bmatrix} 1\\ 0 \end{bmatrix}$

此时，如果只考虑虚部则有下面式子：

$\large \frac{1}{2}J_{b_w}^\gamma \delta b_w=((\hat{\gamma }_{b_{k+1}}^{b_k})^{-1}\bigotimes {q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0})_{vec}$

将左侧转为正定阵，这样就可以进行分解了。如下式：

$\large {J_{b_w}^\gamma} ^{T}J_{b_w}^\gamma \delta b_w=2{J_{b_w}^\gamma} ^{T}{({\hat{\gamma}_{b_{k+1}}^{b_k}}^{-1}\bigotimes {q_{b_k}^{c_0}}^{-1}\bigotimes q_{b_{k+1}}^{c_0})}_{vec}$

上式就是我们最终要求解的方程，其中 $\large \delta b_w$ 为要求解的未知数（陀螺仪偏差）。该式形式可理解为为： $\large Ax=b$

下面代码中就按照这个方式来进行求解。

2）代码分析

solveGyroscopeBias函数是进行陀螺仪偏差计算的函数，代码如下。

这里我有点不明白的是，论文公式中推出来的最终公式里是 $\large {\color{Red} \mathbf{}{q_{b_k}^{c_0}}^{-1}}$ 应该是逆，代码中是frame_i->second.R.transpose()，意思是转置，这里不应该是逆吗？？

2）代码分析

solveGyroscopeBias函数是进行陀螺仪偏差计算的函数，代码如下。

----------后来我明白了，在此补充说明一下，由于旋转矩阵为正交矩阵，所以它的逆和转置是相等的，都描述了一个相反的旋转。

/**
* 陀螺仪偏差标定
*/
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();
//对应公式中的四元数乘积运算：q_ij = (q^c0_bk)^-1 * (q^c0_bk+1)
Eigen::Quaterniond q_ij(frame_i->second.R.transpose() * frame_j->second.R);
//tmp_A = J_j_bw
//O_R的值为3 O_BG的值为12 3 12
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))_vec
// = 2 * ((r^bk_bk+1)^-1 * q_ij)_vec
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);
ROS_WARN_STREAM("gyroscope bias initial calibration " << delta_bg.transpose());
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]);
}
}

2.速度、重力向量和尺度的初始化

1）论文中理论描述推导

本部分需要优化求解速度、重力向量和尺度，如下所示：

$\large \chi _I=\begin{bmatrix} v_{b_0}^{b_0},v_{b_1}^{b_1},\cdots v_{b_n}^{b_n},g^{c_0},s \end{bmatrix}$

其中 $\large v_{b_k}^{b_k}$ 是相机拍摄到第k帧图像对应的IMU体坐标中对应的速度， $\large g^{c_0}$ 是相机第 $\large c_0$ 帧的重力向量， $\large s$ 是SFM的尺度。

这部分的损失函数构造是通过求解IMU预积分获得的值和视觉SFM运动估计的值之间的残差来进行求解。

通过IMU预积分获取连续的两帧 $\large b_k$ 和 $\large b_{k+1}$ 两帧之间的IMU测量值如下：

$\large \alpha _{b_{k+1}}^{b_k}=\iint_{t\in [t_k,t_{k+1}]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt^2$ 这个也可以说是IMU预积分出来的位移的增量

$\large \beta _{b_{k+1}}^{b_k}=\int _{t\in [t_k,t_{k+1}]}R_t^{b_k}(\hat{a}_t-b_{a_t})dt$ 这个理解为是IMU预积分出来的速度的增量

在滑动窗口中，将相机第一帧 $\large \left ( . \right )^{c_0}$ 作为SFM的参考帧，所有帧的位姿为 $\large \left ( \bar{p}_{c_k}^{c_0},q_{c_k}^{c_0} \right )$ ，有因为通过相机和IMU之间外参标定计算出来的外参为 $\large \left ( p_c^b,q_c^b \right )$ ，因此将位姿从相机坐标下转到IMU坐标后如下：

$\large q_{b_k}^{c_0}=q_{c_k}^{c_0}\bigotimes \left ( q_c^b \right )^{-1}$

$\large s\bar{p}_{b_k}^{c_0}=s\bar{p}_{c_k}^{c_0}-R_{b_k}^{c_0}p_c^b$

滑动窗口中连续的两帧 $\large b_k$ 和 $\large b_{k+1}$ 之间位移和速度的估计增量计算如下：

$\large \alpha _{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(s\bar{p}_{b_{k+1}}^{c_0}+\frac{1}{2}g^{c_0}\Delta t_k^2-s\bar{p}_{b_k}^{c_0}-R_{b_k}^{c_0}v_{b_k}^{b_k}\Delta t_k)$

$\large =R_{c_0}^{b_k}(s\bar{p}_{c_{k+1}}^{c_0}-R_{b_{k+1}}^{c_0}p_c^b+\frac{1}{2}g^{c_0}\Delta t_k^2-s\bar{p}_{c_k}^{c_0}+R_{b_k}^{c_0}p_c^b-R_{b_k}^{c_0}v_{b_k}^{b_k}\Delta t_k)$

$\large =R_{c_0}^{b_k}s\bar{p}_{c_{k+1}}^{c_0}-R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b+\frac{1}{2}R_{c_0}^{b_k}g^{c_0}\Delta t_k^2-sR_{c_0}^{b_k}\bar{p}_{c_k}^{c_0}+p_c^b-v_{b_k}^{b_k}\Delta t_k$

$\large =R_{c_0}^{b_k}s(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})-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$

$\large \beta _{b_{k+1}}^{b_k}=R_{c_0}^{b_k}(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}).$

此时，IMU预积分的增量和估计出的增量之间的误差可写成如下：

$\large \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}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})+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\\ \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}=$ $\large \begin{bmatrix} 0_{3\times 1}\\ 0_{3\times 1} \end{bmatrix}$

此时，将上式调整为 $\large Hx=b$ 的形式，这样便于直接利用Cholesky进行求解：

$\large \delta \alpha_{b_{k+1}}^{b_k}=$ $\large \alpha _{b_{k+1}}^{b_k}-R_{c_0}^{b_k}s(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})+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$ $\large =0_{3\times 1}$

$\large =R_{c_0}^{b_k}s(\bar{p}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})-\Delta t_kv_{b_k}^{b_k}+\frac{1}{2}R_{c_0}^{b_k}\Delta t_k^2g^{c_0}$ $\large =\alpha _{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b$

转成矩阵形式可以写成如下：

$\large \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}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0}) \end{bmatrix}\begin{bmatrix} v_{b_k}^{b_k}\\ v_{b_{k+1}}^{b_{k+1}}\\ g^{c_0}\\ s \end{bmatrix}=\alpha_{b_{k+1}}^{b_k}-p_c^b+R_{c_0}^{b_k}R_{b_{k+1}}^{c_0}p_c^b$

同样，也将 $\large \delta \beta _{b_{k+1}}^{b_k}$ 转为矩阵形式，综合写成下式：

$\large \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}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})\\ -I& R_{c_0}^{b_k}R_{b_{k+1}}^{c_0} & 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}$

这个矩阵乘积形式为 $\large H^{6\times 10}X_I^{10\times 1}=b^{6\times 1}$ ，这样，两侧同时左乘 $\large H^T$ 就可以利用Cholesky分解方程求解 $\large X$ :

$\large H^{T}HX_I^{10\times 1}=H^{T}b$

此处最开始有不清楚的地方，借鉴了崔华坤发表在泡泡机器人上边的文章。下面对照着代码分析具体的求解。

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);
//tmp_A为6*10的矩阵，就是H
MatrixXd tmp_A(6, 10);
tmp_A.setZero();
//tmp_b对应b
VectorXd tmp_b(6);
tmp_b.setZero();
double dt = frame_j->second.pre_integration->sum_dt;
//-I*delta_tk
tmp_A.block<3, 3>(0, 0) = -dt * Matrix3d::Identity();
//1/2*R_c0^bk*deltat_k^2*
tmp_A.block<3, 3>(0, 6) = frame_i->second.R.transpose() * dt * dt / 2 * Matrix3d::Identity();
//R_c0^bk*(bar{p}_{c_{k+1}}^{c_0}-bar{p}_{c_{k}}^{c_0})
tmp_A.block<3, 1>(0, 9) = frame_i->second.R.transpose() * (frame_j->second.T - frame_i->second.T) / 100.0;
//alpha_{b_{k+1}}^{b_k}+R_{c_0}^{b_k}*R_{b_{k+1}}^c_0*p_c^b-p_c^b
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;
//-I
tmp_A.block<3, 3>(3, 0) = -Matrix3d::Identity();
//R_c0^{b_k}*R_{b_{k+1}}^c0
tmp_A.block<3, 3>(3, 3) = frame_i->second.R.transpose() * frame_j->second.R;
//R_{c_0}^{b_k}*delta t
tmp_A.block<3, 3>(3, 6) = frame_i->second.R.transpose() * dt * Matrix3d::Identity();
//beta_{b_{k+1}}^{b_k}
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();
//10*6 6*6 6*10 = 10*10
MatrixXd r_A = tmp_A.transpose() * cov_inv * tmp_A;
//10*6 6*6 6*1 = 10*1
VectorXd r_b = tmp_A.transpose() * cov_inv * tmp_b;
//构造A和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和b都同时乘个1000.0呢？？？
A = A * 1000.0;
b = b * 1000.0;
//对Ax=b进行分解求出x
x = A.ldlt().solve(b);
//从求解出的x向量里边取出最后边的尺度s
double s = x(n_state - 1) / 100.0;
ROS_DEBUG("estimated scale: %f", s);
//取出对重力向量g的计算值
g = x.segment<3>(n_state - 4);
ROS_DEBUG_STREAM(" 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;
ROS_DEBUG_STREAM(" refine " << g.norm() << " " << g.transpose());
if(s < 0.0 )
return false;
else
return true;
}

我没有理解这块为什么要给A和b都乘以1000.0？？？

3.对重力进行修正

1）论文中相关描述

由于重力矢量的模大小是已知的，因此剩下两个自由度。在半径为 $\large g=9.81$ 的半球面的切面空间上用两个正交的向量对重力进行参数化描述，如论文中提供的下图所示：

此时重力向量表示为：

$\large \hat{g}=\left \| g \right \|\cdot \hat{\bar{g}}^{3\times 1}+w_1\vec{b_1}^{3\times 1}+w_2\vec{b_2}^{3\times 1}=\left \| g \right \|\cdot \hat{\bar{g}}^{3\times 1}+\vec{b}^{3\times 2}w^{2\times 1}$

其中 $\large \left \| g \right \|$ 为重力向量的模， $\large \hat{\bar{g}}$ 为表示重力方向的单位向量， $\large w_1,w_2$ 为在两个正交向量 $\large b_1,b_2$ 方向上的大小。

将新写的重力向量带入上边2中所推得的公式中，得如下结果：

$\large \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}_{c_{k+1}}^{c_0}-\bar{p}_{c_k}^{c_0})\\ -I& R_{c_0}^{b_k}R_{b_{k+1}}^{c_0} & 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^2_{k}\left \| g \right \|\hat{\bar{g}}\\ \beta _{b_{k+1}}^{b_k}-R_{c_0}^{b_k}\Delta t_k\left \| g \right \| \hat{\bar{g}}\end{bmatrix}$

此时，该矩阵乘法表现形式为 $\large H^{6\times 9}X_I^{9\times 1}=b^{6\times 1}, w^{2\times 1}=[w_1,w_2]^T$ ,然后用LDLT分解下面方程求解 $\large X_I$ ：

$\large H^THX_I=H^{T}b$

2）代码实现

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;
//迭代求解4次
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() * 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.block<6, 6>(0, 0) = IMU_cov[i + 1];
//MatrixXd cov_inv = cov.inverse();
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();
//double s = x(n_state - 1);
}
g = g0;
}
/**
* 在半径为g=9.81的半球上找到切面的一对正交基，存放在bc当中
*/
MatrixXd TangentBasis(Vector3d &g0)
{
Vector3d b, c;
Vector3d a = g0.normalized();
Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
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;
}

在将重力向量进行修正以后，我们就可以通过将重力旋转到z轴的方向上，从而获得相机第0帧到世界坐标系之间的旋转 $\large q_{c_0}^w$ 。这时候就可以把所有变量从以相机第0帧为参考帧的坐标系中旋转到世界坐标系下。IMU帧中的速度也可以旋转到世界坐标系中。这时候从视觉SFM中获得的平移量将达到米级单位的刻度。

4.视觉SFM运动和IMU预积分结果对齐后位姿的计算

在将从视觉SFM中估计出来的位姿信息和IMU预积分的结果对齐之后，也就意味着本篇笔记开头的那张图中展示的视觉结构和IMU预积分结果匹配完成了。此时，我们需要获得世界坐标系中的位姿，也就是计算出PVQ，这样就完成了位姿的初始化估计，后边将用于进行单目紧耦合的VIO操作。

visualInitialAlign函数代码如下：

/**
* 视觉惯导对齐
*/
bool Estimator::visualInitialAlign()
{
TicToc t_g;
VectorXd x;
//solve scale 1.视觉惯性联合初始化，计算陀螺仪的偏置，尺度，重力加速度和速度
bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
if(!result)
{
ROS_DEBUG("solve g failed!");
return false;
}
// change state 2.获取滑动窗口内所有图像帧相对于第l帧的位姿信息，并设置为关键帧
for (int i = 0; i <= frame_count; i++)
{
Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
Ps[i] = Pi;
Rs[i] = Ri;
all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
}
//3.获取特征点深度
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)
dep[i] = -1;
f_manager.clearDepth(dep);
//triangulat on cam pose , no tic
Vector3d TIC_TMP[NUM_OF_CAM];
for(int i = 0; i < NUM_OF_CAM; i++)
TIC_TMP[i].setZero();
//RIC中存放的是相机到IMU的旋转，在相机-IMU外参标定部分求得
ric[0] = RIC[0];
f_manager.setRic(ric);
//三角化计算地图点的深度，Ps中存放的是各个帧相对于参考帧之间的平移，RIC[0]为相机-IMU之间的旋转
f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
double s = (x.tail<1>())(0);
//4.这里陀螺仪的偏差Bgs改变了，需遍历滑动窗口中的帧，重新预积分
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
//5.计算各帧相对于b0的位姿信息，前边计算的都是相对于第l帧的位姿
/**
* 前面初始化中，计算出来的是相对滑动窗口中第l帧的位姿，在这里转换到第b0帧坐标系下
* s*p_bk^b0=s*p_bk^cl−s*p_b0^cl=(s*p_ck^cl−R_bk^cl*p_c^b)−(s*p_c0^cl−R_b0^cl*p_c^b)
* TIC[0]是相机到IMU的平移量
* Rs是IMU第k帧到滑动窗口中图像第l帧的旋转
* Ps是滑动窗口中第k帧到第l帧的平移量
* 注意：如果运行的脚本是配置文件中无外参的脚本，那么这里的TIC都是0
*/
for (int i = frame_count; i >= 0; i--)
Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
int kv = -1;
map<double, ImageFrame>::iterator frame_i;
for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
{
if(frame_i->second.is_key_frame)
{
kv++;
//存储速度
Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
}
}
//更新每个地图点被观测到的帧数(used_num)和预测的深度(estimated_depth)
for (auto &it_per_id : f_manager.feature)
{
it_per_id.used_num = it_per_id.feature_per_frame.size();
if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
it_per_id.estimated_depth *= s;
}
/**
* refine之后就获得了C_0坐标系下的重力g^{c_0}，此时通过将g^{c_0}旋转至z轴方向，
* 这样就可以计算相机系到世界坐标系的旋转矩阵q_{c_0}^w，这里求得的是rot_diff,这样就可以将所有变量调整至世界系中。
*/
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;
//所有变量从参考坐标系c_0旋转到世界坐标系w
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];
}
ROS_DEBUG_STREAM("g0 " << g.transpose());
ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose());
return true;
}