VINS
中关于陀螺仪零偏的初始化估计
对于窗口中得连续两帧 \(b_{k}\) 和 \(b_{k+1}\) ,已经从视觉SFM
中得到了旋转 \(q_{b_{k}}^{c_{0}}\) 和 \(q_{b_{k+1}}^{c_{0}}\) ,从IMU
预积分中得到了相邻帧旋转 \(\hat{\gamma}^{b_{k}}_{b_{k+1}}\) 。 根据约束方程,联立所有相邻帧,最小化代价函数(论文式(7)):
\]
其中对陀螺仪偏置求IMU
预积分项线性化,有:
1 \\
\frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w}
\end{array}\right]
\]
在具体实现的时候,因为上述约束方程为:
1 \\
0
\end{array}\right]
\]
有:
1 \\
0
\end{array}\right]
\]
代入 \(\delta b_{w}\) 得 :
1 \\
\frac{1}{2} J_{b_{w}}^{\gamma} \delta b_{w}
\end{array}\right]=q_{b_{k}}^{c 0-1} \otimes q_{b_{k+1}}^{c 0} \otimes\left[\begin{array}{l}
1 \\
0
\end{array}\right]
\]
只考虑虚部,有 :
\]
两侧乘以 \(\mathbf{J_{b_{w}}^{\gamma}}^{T}\) ,用LDLT
分解求得 \(\delta b_{w}\) 。
在代码中, \(\mathrm{q}_\mathrm{i,j}\) 即 \(q_{b_{k+1}}^{b_{k}}=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\)
\(\mathrm{tmp}_{-} \mathrm{A}\) 即 \(J_{b_{w}}^{\gamma}\)
\(\mathrm{tmp}_{-} \mathrm{B}\) 即
\]
根据上面的代价函数构造 \(\mathrm{Ax}=\mathrm{B}\) 即
\]
然后采用LDLT
分解求得 \(\delta b_{w}\) 。
VINS
中的代码
/**
* @brief 陀螺仪偏置校正
* @optional 根据视觉SFM的结果来校正陀螺仪Bias -> Paper V-B-1
* 主要是将相邻帧之间SFM求解出来的旋转矩阵与IMU预积分的旋转量对齐
* 注意得到了新的Bias后对应的预积分需要repropagate
* @param[in] all_image_frame所有图像帧构成的map,图像帧保存了位姿、预积分量和关于角点的信息
* @param[out] Bgs 陀螺仪偏置
* @return void
*/
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();
//R_ij = (R^c0_bk)^-1 * (R^c0_bk+1) 转换为四元数 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
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)
// = 2 * (r^bk_bk+1)^-1 * q_ij
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;
// 得到了新的Bias后对应的预积分需要repropagate
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]);
}
}
之所以 A +=tmp_A.transpose() * tmp_A
,其实就是 \(A^T Ax=A^Tb\)。在求解 \(Ax=b\) 的最小二乘解时,两边同乘以A矩阵的转置得到的 \(A^TA\) 一定是可逆的。
-
TIPS
这里为什么是可以连加的呢,直接构造该正定方程呢,简单来说就是VINS中认为滑窗中陀螺仪的Bags都是一样的。所以把所有的方程写在一起就构成了同一个变量的连加形式。