视觉惯性联合初始化
其对应的函数为
visualInitialAlign()
是在初始窗口中的图像帧完成SFM三维重建之后,即各图像帧在参考坐标系下的初始位姿都已经计算完成之后,执行的。该函数主要实现了陀螺仪的偏置校准(加速度偏置没有处理),计算速度V[0:n]、重力g、尺度s。
同时更新了Bgs后,IMU测量量需要repropagate;得到尺度s和重力g的方向后,需更新所有图像帧在世界座标系下的Ps、Rs、Vs。
1、 计算陀螺仪偏置,尺度,重力加速度和速度
bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);
if(!result)
{
ROS_DEBUG("solve g failed!");
return false;
}
2、 获取所有图像帧的位姿Ps、Rs,并将其置为关键帧
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、 重新计算所有特征点的深度
//将所有特征点的深度置为-1
VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)
dep[i] = -1;
f_manager.clearDepth(dep);
//重新计算特征点的深度
Vector3d TIC_TMP[NUM_OF_CAM];
for(int i = 0; i < NUM_OF_CAM; i++)
TIC_TMP[i].setZero();
ric[0] = RIC[0];
f_manager.setRic(ric);
f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));
4、 陀螺仪的偏置bgs改变,重新计算预积分
for (int i = 0; i <= WINDOW_SIZE; i++)
{
pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}
5、 将Ps、Vs、depth尺度s缩放后转变为相对于第0帧图像座标系下
论文提到的以第一帧c0为基准座标系,通过相机座标系ck位姿得到IMU座标系bk位姿的公式为:
q
b
k
c
0
=
q
c
k
c
0
⊗
(
q
c
b
)
−
1
s
p
ˉ
b
k
c
0
=
s
p
ˉ
c
k
c
0
−
R
b
k
c
0
p
c
b
\mathbf{q}_{b_k}^{c_0}=\mathbf{q}_{c_k}^{c_0} \otimes (\mathbf{q}_c^b)^{-1} \\ s\bar{\mathbf{p}}_{b_k}^{c_0}=s\bar{\mathbf{p}}_{c_k}^{c_0}-\mathbf{R}_{b_k}^{c_0}\mathbf{p}_c^b
qbkc0=qckc0⊗(qcb)−1spˉbkc0=spˉckc0−Rbkc0pcb
之前都是以第l帧为基准坐标系,转换到以第一帧b0为基准坐标系的话应该是:
s
p
b
k
b
0
=
s
p
b
k
c
l
−
s
p
b
0
c
l
=
(
s
p
c
k
c
l
−
R
b
k
c
l
p
c
b
)
−
(
s
p
c
0
c
l
−
R
b
0
c
l
p
c
b
)
s\mathbf{p}_{b_k}^{b_0}=s\mathbf{p}_{b_k}^{c_l}-s\mathbf{p}_{b_0}^{c_l}=(s{\mathbf{p}}_{c_k}^{c_l}-\mathbf{R}_{b_k}^{c_l}\mathbf{p}_c^b)-(s{\mathbf{p}}_{c_0}^{c_l}-\mathbf{R}_{b_0}^{c_l}\mathbf{p}_c^b)
spbkb0=spbkcl−spb0cl=(spckcl−Rbkclpcb)−(spc0cl−Rb0clpcb)
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);
}
}
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;
}
6、 通过将重力旋转到z轴上,得到世界座标系与摄像机座标系c0之间的旋转矩阵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;
7、 所有变量从参考座标系c0旋转到世界座标系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];
}
陀螺仪偏置,尺度,重力加速度和速度初始化
所有帧的位姿
(
R
c
k
c
0
,
q
c
k
c
0
)
(\mathbf{R}_{c_k}^{c_0},\mathbf{q}_{c_k}^{c_0})
(Rckc0,qckc0)表示相对于第一帧相机位姿态的坐标系。相机到IMU的外参为
(
R
c
b
,
q
c
b
)
(\mathbf{R}_c^b,\mathbf{q}_c^b)
(Rcb,qcb),则有
T
c
k
c
0
=
T
b
k
c
0
T
c
b
R
c
k
c
0
=
R
b
k
c
0
R
c
b
=
R
b
k
c
0
(
R
c
b
)
−
1
s
p
c
k
c
0
=
R
b
k
c
0
p
c
b
+
s
p
b
k
c
0
  
⟹
  
s
p
b
k
c
0
=
s
p
c
k
c
0
−
R
b
k
c
0
p
c
b
\mathbf{T}_{c_k}^{c_0} = \mathbf{T}_{b_k}^{c_0}\mathbf{T}_c^b \\ \mathbf{R}_{c_k}^{c_0}=\mathbf{R}_{b_k}^{c_0}\mathbf{R}_{c}^{b} = \mathbf{R}_{b_k}^{c_0}(\mathbf{R}_{c}^{b})^{-1} \\ s\mathbf{p}_{c_k}^{c_0}=\mathbf{R}_{b_k}^{c_0}\mathbf{p}_c^b+s\mathbf{p}_{b_k}^{c_0} \implies s\mathbf{p}_{b_k}^{c_0}=s\mathbf{p}_{c_k}^{c_0}-\mathbf{R}_{b_k}^{c_0}\mathbf{p}_c^b
Tckc0=Tbkc0TcbRckc0=Rbkc0Rcb=Rbkc0(Rcb)−1spckc0=Rbkc0pcb+spbkc0⟹spbkc0=spckc0−Rbkc0pcb
VisualIMUAlignment()中调用了两个函数,分别用于对陀螺仪的偏置进行标定,以及估计尺度/重力和速度。
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{
solveGyroscopeBias(all_image_frame, Bgs);
if(LinearAlignment(all_image_frame, g, x))
return true;
else
return false;
}
陀螺仪偏置标定 solveGyroscopeBias()
对于窗口中的连续两帧
b
k
b_k
bk和
b
k
+
1
b_{k+1}
bk+1,已经从视觉SFM中得到了旋转
q
b
k
c
0
\mathbf{q}_{b_k}^{c_0}
qbkc0和
q
b
k
+
1
c
0
\mathbf{q}_{b_{k+1}}^{c_0}
qbk+1c0,从预积分中得到了相邻两帧旋转
γ
^
b
k
+
1
b
k
\hat \mathbf{\gamma}_{b_{k+1}}^{b_k}
γ^bk+1bk,根据约束方程,建立所有相邻帧最小代价函数
min
∑
k
∈
B
∥
q
b
k
+
1
c
0
−
1
⊗
q
b
k
c
0
⊗
γ
b
k
+
1
b
k
∥
\min \sum_{k \in \mathcal{B}} \lVert { \mathbf{q}_{b_{k+1}}^{c_0}}^{-1} \otimes \mathbf{q}_{b_k}^{c_0} \otimes \mathbf{\gamma}_{b_{k+1}}^{b_k} \rVert
mink∈B∑∥qbk+1c0−1⊗qbkc0⊗γbk+1bk∥
其中,对陀螺仪偏置求IMU预积分项线性化,有
γ
b
k
+
1
b
k
≈
γ
^
b
k
+
1
b
k
⊗
[
1
1
2
J
b
w
γ
δ
b
w
]
\mathbf{\gamma}_{b_{k+1}}^{b_k}\approx \hat \mathbf{\gamma}_{b_{k+1}}^{b_k} \otimes \begin {bmatrix} 1 \\ \frac{1}{2} \mathbf{J}_{b_w}^{\gamma}\delta{b_w} \end{bmatrix}
γbk+1bk≈γ^bk+1bk⊗[121Jbwγδbw]
在具体实现的时候,上述约束方程为
q
b
k
+
1
c
0
−
1
⊗
q
b
k
c
0
⊗
γ
b
k
+
1
b
k
=
[
1
0
]
{ \mathbf{q}_{b_{k+1}}^{c_0}}^{-1} \otimes \mathbf{q}_{b_k}^{c_0} \otimes \mathbf{\gamma}_{b_{k+1}}^{b_k} =\begin{bmatrix}1 \\ 0 \end{bmatrix}
qbk+1c0−1⊗qbkc0⊗γbk+1bk=[10]
有:
γ
b
k
+
1
b
k
=
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
⊗
[
1
0
]
\mathbf{\gamma}_{b_{k+1}}^{b_k}= {\mathbf{q}_{b_k}^{c_0}}^{-1} \otimes \mathbf{q}_{b_{k+1}}^{c_0} \otimes \begin{bmatrix}1 \\ 0 \end{bmatrix}
γbk+1bk=qbkc0−1⊗qbk+1c0⊗[10]
代入上一阶展开式,有
γ
^
b
k
+
1
b
k
⊗
[
1
1
2
J
b
w
γ
δ
b
w
]
=
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
⊗
[
1
0
]
\hat \mathbf{\gamma}_{b_{k+1}}^{b_k} \otimes \begin{bmatrix} 1\\ \frac{1}{2} \mathbf{J}_{b_w}^{\gamma}\delta{b_w} \end{bmatrix}={\mathbf{q}_{b_k}^{c_0}}^{-1} \otimes \mathbf{q}_{b_{k+1}}^{c_0} \otimes \begin{bmatrix}1 \\ 0 \end{bmatrix}
γ^bk+1bk⊗[121Jbwγδbw]=qbkc0−1⊗qbk+1c0⊗[10]
只考虑虚部,有:
J
b
w
γ
δ
b
w
=
2
(
γ
^
b
k
+
1
b
k
−
1
⊗
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
)
\mathbf{J}_{b_w}^{\gamma}\delta{b_w}=2({\hat \mathbf{\gamma}_{b_{k+1}}^{b_k}}^{-1} \otimes {\mathbf{q}_{b_k}^{c_0}}^{-1} \otimes \mathbf{q}_{b_{k+1}}^{c_0} )
Jbwγδbw=2(γ^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)
两侧乘以
J
b
w
γ
T
{\mathbf{J}_{b_w}^{\gamma}}^{T}
JbwγT,用LDLT分解求得
δ
b
w
\delta{b_w}
δbw.
在代码中,Quaterniond q_ij即
q
b
k
+
1
b
k
=
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
\mathbf{q}_{b_{k+1}}^{b_k}={\mathbf{q}_{b_k}^{c_0}}^{-1} \otimes\mathbf{q}_{b_{k+1}}^{c_0}
qbk+1bk=qbkc0−1⊗qbk+1c0,tmp_A即
J
b
w
γ
\mathbf{J}_{b_w}^{\gamma}
Jbwγ,tmp_B即
2
(
γ
^
b
k
+
1
b
k
−
1
⊗
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
)
2({\hat \mathbf{\gamma}_{b_{k+1}}^{b_k}}^{-1} \otimes {\mathbf{q}_{b_k}^{c_0}}^{-1} \otimes \mathbf{q}_{b_{k+1}}^{c_0} )
2(γ^bk+1bk−1⊗qbkc0−1⊗qbk+1c0),根据上面的代价函数构造
A
x
=
B
\mathbf{Ax=B}
Ax=B,即
J
b
w
γ
T
J
b
w
γ
δ
b
w
=
2
J
b
w
γ
T
(
γ
^
b
k
+
1
b
k
−
1
⊗
q
b
k
c
0
−
1
⊗
q
b
k
+
1
c
0
)
{\mathbf{J}_{b_w}^{\gamma}}^{T}\mathbf{J}_{b_w}^{\gamma}\delta{b_w}=2{\mathbf{J}_{b_w}^{\gamma}}^{T}({\hat \mathbf{\gamma}_{b_{k+1}}^{b_k}}^{-1} \otimes {\mathbf{q}_{b_k}^{c_0}}^{-1} \otimes \mathbf{q}_{b_{k+1}}^{c_0} )
JbwγTJbwγδbw=2JbwγT(γ^bk+1bk−1⊗qbkc0−1⊗qbk+1c0)
然后用LDLT分解求得偏置
δ
b
w
\delta{b_w}
δbw.代码如下
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))_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]);
}
}
速度/重力和尺度初始化 LinearAligment()
该函数需要优化的变量有速度/重力加速度和尺度:
X
I
3
(
n
+
1
)
+
3
+
1
=
[
v
b
0
b
0
,
v
b
1
b
1
,
…
,
v
b
n
b
n
,
g
c
0
,
s
]
\mathbf{X}_I^{3(n+1)+3+1}=[\mathbf{v}_{b_0}^{b_0},\mathbf{v}_{b_1}^{b_1},\dots,\mathbf{v}_{b_n}^{b_n},\mathbf{g}^{c_0},s]
XI3(n+1)+3+1=[vb0b0,vb1b1,…,vbnbn,gc0,s]
其中,
v
b
k
b
k
\mathbf{v}_{b_k}^{b_k}
vbkbk是第
k
k
k帧图像在其IMU坐标系下的速度,
g
c
0
\mathbf{g}^{c_0}
gc0是在第0帧相机坐标系下的重力向量。
在IMU预积分中,有
R
w
b
k
p
b
k
+
1
w
=
R
w
b
k
(
p
b
k
w
+
v
b
k
w
Δ
t
k
−
1
2
g
w
Δ
t
k
2
)
+
α
b
k
+
1
b
k
R
w
b
k
v
b
k
+
1
w
=
R
w
b
k
(
v
b
k
w
−
g
w
Δ
t
k
+
β
b
k
+
1
b
k
\mathbf{R}_w^{b_k}\mathbf{p}_{b_{k+1}}^{w}=\mathbf{R}_w^{b_k}(\mathbf{p}_{b_k}^w+\mathbf{v}_{b_k}^w \Delta t_k-\frac{1}{2}\mathbf{g}^w\Delta t_k^2)+\alpha_{b_{k+1}}^{b_k} \\ \mathbf{R}_w^{b_k}\mathbf{v}_{b_{k+1}}^w=\mathbf{R}_w^{b_k}(\mathbf{v}_{b_k}^w-\mathbf{g}^w \Delta t_k+\beta_{b_{k+1}}^{b_k}
Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk−21gwΔtk2)+αbk+1bkRwbkvbk+1w=Rwbk(vbkw−gwΔtk+βbk+1bk
将
c
0
c_0
c0替代
w
w
w,有
α
b
k
+
1
b
k
=
R
c
0
b
k
(
s
p
b
k
+
1
c
0
−
s
p
b
k
c
0
+
1
2
g
c
0
Δ
t
k
2
−
R
b
k
c
0
v
b
k
b
k
Δ
t
k
)
β
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
Δ
t
k
−
R
b
k
c
0
v
b
k
b
k
)
\alpha_{b_{k+1}}^{b_k}=\mathbf{R}_{c_0}^{b_k}(s\mathbf{p}_{b_{k+1}}^{c_0}-s\mathbf{p}_{b_k}^{c_0}+\frac{1}{2}\mathbf{g}^{c_0}\Delta t_k^2-\mathbf{R}_{b_k}^{c_0}\mathbf{v}_{b_k}^{b_k}\Delta t_k) \\ \beta_{b_{k+1}}^{b_k}=\mathbf{R}_{c_0}^{b_k}(\mathbf{R}_{b_{k+1}}^{c_0}\mathbf{v}_{b_{k+1}}^{b_{k+1}}+\mathbf{g}^{c_0}\Delta t_k-\mathbf{R}_{b_k}^{c_0}\mathbf{v}_{b_k}^{b_k})
αbk+1bk=Rc0bk(spbk+1c0−spbkc0+21gc0Δtk2−Rbkc0vbkbkΔtk)βbk+1bk=Rc0bk(Rbk+1c0vbk+1bk+1+gc0Δtk−Rbkc0vbkbk)
进一步,有
α
b
k
+
1
b
k
=
R
c
0
b
k
(
s
p
c
k
+
1
c
0
−
R
b
k
+
1
c
0
p
c
b
−
(
s
p
c
k
c
0
−
R
b
k
c
0
p
c
b
)
+
1
2
g
c
0
Δ
t
k
2
−
R
b
k
c
0
v
b
k
b
k
Δ
t
k
)
\alpha_{b_{k+1}}^{b_k}=\mathbf{R}_{c_0}^{b_k}(s\mathbf{p}_{c_{k+1}}^{c_0}-\mathbf{R}_{b_{k+1}}^{c_0}\mathbf{p}_c^b-(s\mathbf{p}_{c_{k}}^{c_0}-\mathbf{R}_{b_{k}}^{c_0}\mathbf{p}_c^b)+\frac{1}{2}\mathbf{g}^{c_0}\Delta t_k^2-\mathbf{R}_{b_k}^{c_0}\mathbf{v}_{b_k}^{b_k}\Delta t_k)
αbk+1bk=Rc0bk(spck+1c0−Rbk+1c0pcb−(spckc0−Rbkc0pcb)+21gc0Δtk2−Rbkc0vbkbkΔtk)
根据待优化变量,整理上述方程,转换成
H
x
=
b
Hx=b
Hx=b的形式,有
R
c
0
b
k
(
p
c
k
+
1
c
0
−
p
c
k
c
0
)
s
−
v
b
k
b
k
Δ
t
k
+
1
2
R
c
0
b
k
Δ
t
k
2
g
c
0
=
δ
α
b
k
+
1
b
k
+
R
c
0
b
k
R
b
k
+
1
c
0
p
)
c
b
−
p
c
b
\mathbf{R}_{c_0}^{b_k}(\mathbf{p}_{c_{k+1}}^{c_0}-\mathbf{p}_{c_k}^{c_0})s-\mathbf{v}_{b_k}^{b_k}\Delta t_k+\frac{1}{2}\mathbf{R}_{c_0}^{b_k}\Delta t_k^2\mathbf{g}^{c_0}=\delta \alpha_{b_{k+1}}^{b_k}+\mathbf{R}_{c_0}^{b_k}\mathbf{R}_{b_{k+1}}^{c_0}\mathbf{p})_c^b-\mathbf{p}_c^b
Rc0bk(pck+1c0−pckc0)s−vbkbkΔtk+21Rc0bkΔtk2gc0=δαbk+1bk+Rc0bkRbk+1c0p)cb−pcb
转换成矩阵形式:
[
−
I
Δ
t
k
0
1
2
R
c
0
b
k
Δ
t
k
2
R
c
0
b
k
(
p
c
k
+
1
c
0
−
p
c
k
c
0
)
]
[
v
b
k
b
k
v
b
k
+
1
b
k
+
1
g
c
0
s
]
=
α
^
b
k
+
1
b
k
−
p
c
b
+
R
c
0
b
k
R
b
k
+
1
c
0
p
c
b
\begin{bmatrix} -\mathbf{I} \Delta t_k &0 &\frac{1}{2}\mathbf{R}_{c_0}^{b_k}\Delta t_k^2 & \mathbf{R}_{c_0}^{b_k}(\mathbf{p}_{c_{k+1}}^{c_0}-\mathbf{p}_{c_k}^{c_0})\end{bmatrix} \begin{bmatrix} \mathbf{v}_{b_k}^{b_k} \\ \mathbf{v}_{b_{k+1}}^{b_{k+1}} \\ \mathbf{g}^{c_0} \\ s\end{bmatrix}=\hat \mathbf{\alpha}_{b_{k+1}}^{b_k}-\mathbf{p}_c^b+\mathbf{R}_{c_0}^{b_k}\mathbf{R}_{b_{k+1}}^{c_0}\mathbf{p}_c^b
[−IΔtk021Rc0bkΔtk2Rc0bk(pck+1c0−pckc0)]⎣⎢⎢⎡vbkbkvbk+1bk+1gc0s⎦⎥⎥⎤=α^bk+1bk−pcb+Rc0bkRbk+1c0pcb
同理,有
[
−
I
R
c
0
b
k
R
b
k
+
1
c
0
R
c
0
b
k
Δ
t
k
0
]
[
v
b
k
b
k
v
b
k
+
1
b
k
+
1
g
c
0
s
]
=
β
^
b
k
+
1
b
k
\begin{bmatrix} -\mathbf{I} &\mathbf{R}_{c_0}^{b_k}\mathbf{R}_{b_{k+1}}^{c_0} &\mathbf{R}_{c_0}^{b_k}\Delta t_k&0\end{bmatrix} \begin{bmatrix} \mathbf{v}_{b_k}^{b_k} \\ \mathbf{v}_{b_{k+1}}^{b_{k+1}} \\ \mathbf{g}^{c_0} \\ s\end{bmatrix}=\hat \mathbf{\beta}_{b_{k+1}}^{b_k}
[−IRc0bkRbk+1c0Rc0bkΔtk0]⎣⎢⎢⎡vbkbkvbk+1bk+1gc0s⎦⎥⎥⎤=β^bk+1bk
最后得到
H
6
×
10
X
I
10
×
1
=
b
6
×
1
H
T
H
X
I
10
×
1
=
H
T
b
6
×
1
\mathbf{H}^{6 \times 10}\mathbf{X}_I^{10 \times 1}=\mathbf{b}^{6 \times 1} \\ \mathbf{H}^T\mathbf{H}\mathbf{X}_I^{10 \times 1}=\mathbf{H}^T\mathbf{b}^{6 \times 1}
H6×10XI10×1=b6×1HTHXI10×1=HTb6×1
使用LDLT分解求解上式。代码中,MatrixXd tmp_A(6, 10)就是
H
\mathbf{H}
H,VectorXd tmp_b(6,1)就是
b
\mathbf{b}
b。
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
最后求得尺度 s s s和重力加速度 g \mathbf{g} g
double s = x(n_state - 1) / 100.0;
g = x.segment<3>(n_state - 4);
//如果重力加速度與參考值差太大或者尺度爲負則說明計算錯誤
if(fabs(g.norm() - G.norm()) > 1.0 || s < 0)
{
return false;
}
重力细化RefineGravity()
考虑到上一步求得的g存在误差,一般认为重力矢量的模长是已知的,因此重力只剩下两个自由度,在切线空间上用两个变量重新参数化重力,将其表示为
g
^
=
∥
g
∥
g
^
ˉ
c
0
+
w
1
b
1
+
w
2
b
2
=
∥
g
∥
g
^
ˉ
c
0
+
b
T
w
\hat \mathbf{g}=\lVert \mathbf{g} \rVert \bar{\hat \mathbf{g}}^{c_0} +w_1b_1+w_2b_2= \lVert \mathbf{g} \rVert \bar{\hat \mathbf{g}}^{c_0} +\mathbf{b^Tw}
g^=∥g∥g^ˉc0+w1b1+w2b2=∥g∥g^ˉc0+bTw
将其带入优化求解公式,待优化变量和观测方程变为
[
v
b
k
b
k
v
b
k
+
1
b
k
+
1
g
c
0
s
]
→
[
v
b
k
b
k
v
b
k
+
1
b
k
+
1
w
c
0
s
]
z
^
b
k
+
1
b
k
=
[
α
^
b
k
+
1
b
k
−
p
c
b
+
R
c
0
b
k
R
b
k
+
1
c
0
p
c
b
−
1
2
R
c
0
b
k
Δ
t
k
2
∥
g
∥
⋅
g
^
ˉ
c
0
β
b
k
+
1
b
k
−
R
c
0
b
k
Δ
t
k
∥
g
∥
⋅
g
^
ˉ
c
0
]
\begin {bmatrix} \mathbf{v}_{b_k} ^{b_k} \\ \mathbf{v}_{b_{k+1}}^{b_{k+1}} \\ \mathbf{g}^{c_0} \\ s \end {bmatrix} \to \begin {bmatrix} \mathbf{v}_{b_k} ^{b_k} \\ \mathbf{v}_{b_{k+1}}^{b_{k+1}} \\ \mathbf{w}^{c_0} \\ s \end {bmatrix} \\ \hat \mathbf{z}_{b_{k+1}}^{b_k}=\begin{bmatrix} \hat \mathbf{\alpha}_{b_{k+1}}^{b_k}-\mathbf{p}_c^b+\mathbf{R}_{c_0}^{b_k}\mathbf{R}_{b_{k+1}}^{c_0}\mathbf{p}_c^b-\frac{1}{2}\mathbf{R}_{c_0}^{b_k}\Delta t_k^2 \lVert \mathbf{g} \rVert \cdot \bar{\hat \mathbf{g}}^{c_0} \\ \mathbf{\beta}_{b_{k+1}}^{b_{k}}-\mathbf{R}_{c_0}^{b_k}\Delta t_k \lVert \mathbf{g} \rVert \cdot \bar{\hat \mathbf{g}}^{c_0} \end{bmatrix}
⎣⎢⎢⎡vbkbkvbk+1bk+1gc0s⎦⎥⎥⎤→⎣⎢⎢⎡vbkbkvbk+1bk+1wc0s⎦⎥⎥⎤z^bk+1bk=[α^bk+1bk−pcb+Rc0bkRbk+1c0pcb−21Rc0bkΔtk2∥g∥⋅g^ˉc0βbk+1bk−Rc0bkΔtk∥g∥⋅g^ˉc0]
对应的函数为
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)
函数中的MatrixXd lxly(3, 2)= TangentBasis(g0); 即对应 b \mathbf{b} b,VectorXd dg = x.segment<2>(n_state - 3); 即对应 w \mathbf{w} w,用于寻找参数 b \mathbf{b} b的Algorithm 1对应的代码如下
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;
}
外参标定
VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化。
在参数配置文件yaml中,参数estimate_extrinsic反映了外参的情況:
1、等于0表示这外参不需要再优化;
2、等于1表示外参只是一個估计值,后续还需要将其作为初始值放入非线性优化中;
3、等于2表示不知道外参,需要进行标定,从代码estimator.cpp中的processImage()中的以下代码進入,主要是标定外参的旋转矩阵。
if(ESTIMATE_EXTRINSIC == 2)//如果沒有外參則進行標定
{
ROS_INFO("calibrating extrinsic param, rotation movement is needed");
if (frame_count != 0)
{
//得到當前幀與前一幀之間歸一化特徵點
vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
Matrix3d calib_ric;
//標定從camera到IMU之間的旋轉矩陣
if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
{
ROS_WARN("initial extrinsic rotation calib success");
ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
ric[0] = calib_ric;
RIC[0] = calib_ric;
ESTIMATE_EXTRINSIC = 1;
}
}
}
CalibrationExRotation()标定外参旋转矩阵
该函数的目的是标定外惨的旋转矩阵。下面介绍函数的内部操作。
1. solveRelativeR(corres)根据对极几何计算相邻帧见相机坐标系的旋转矩阵,这里的corres传入的是当前帧和其之前一帧的对应特征点对的归一化坐标。
1.1将corres中对应点的二维坐标分别存入ll和rr中
vector<cv::Point2f> ll, rr;
for (int i = 0; i < int(corres.size()); i++)
{
ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
}
1.2 根据对极几何求解这两帧的本质矩阵
cv::Mat E = cv::findFundamentalMat(ll, rr);
1.3 对本质矩阵进行svd分解得到4组Rt解
cv::Mat_<double> R1, R2, t1, t2;
decomposeE(E, R1, R2, t1, t2);
1.4 采用三角化对每组Rt进行验证,选择是正深度的Rt
double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;
1.5 当前的R是上一帧到当前帧的变换矩阵,对其求转置变为当前帧想对于上一帧的姿态。
Matrix3d ans_R_eigen;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
ans_R_eigen(j, i) = ans_R_cv(i, j);
return ans_R_eigen;
2. 获取相邻帧之间相机坐标系和IMU坐标系的旋转矩阵,存入vector中
frame_count++;
Rc.push_back(solveRelativeR(corres));//幀間cam的R,由對極幾何得到
Rimu.push_back(delta_q_imu.toRotationMatrix());//幀間IMU的R,由IMU預積分得到
Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每幀IMU相對於起始幀IMU的R
3. 求解相机到IMU的外参旋转矩阵,根据等式
R
c
k
+
1
b
k
=
R
b
k
+
1
b
k
⋅
R
c
b
=
R
c
b
⋅
R
b
c
+
1
c
k
q
b
k
+
1
b
k
⋅
q
c
b
=
q
c
b
⋅
q
b
c
+
1
c
k
\mathbf{R}_{c_{k+1}}^{b_k}=\mathbf{R}_{b_{k+1}}^{b_k}\cdot \mathbf{R}_c^b= \mathbf{R}_c^b \cdot \mathbf{R}_{b_{c+1}}^{c_k} \\ \mathbf{q}_{b_{k+1}}^{b_k}\cdot \mathbf{q}_c^b= \mathbf{q}_c^b \cdot \mathbf{q}_{b_{c+1}}^{c_k}
Rck+1bk=Rbk+1bk⋅Rcb=Rcb⋅Rbc+1ckqbk+1bk⋅qcb=qcb⋅qbc+1ck
则可以得到
(
[
q
b
k
+
1
b
k
]
L
−
[
q
c
k
+
1
c
k
]
R
)
q
c
b
=
Q
k
+
1
k
⋅
q
c
b
=
0
([\mathbf{q}_{b_{k+1}}^{b_k}]_L-[\mathbf{q}_{c_{k+1}}^{c_k}]_R)\mathbf{q}_c^b=\mathbf{Q}_{k+1}^k\cdot \mathbf{q}_c^b=\mathbf{0}
([qbk+1bk]L−[qck+1ck]R)qcb=Qk+1k⋅qcb=0
将多帧之间的等式关联在一起构建超定方程
A
x
=
0
Ax=0
Ax=0,对
A
A
A进行svd分解,其中最小的奇异值对应的右奇异向量便为
x
x
x的估计。对应的代码为
Eigen::MatrixXd A(frame_count * 4, 4);
A.setZero();
int sum_ok = 0;
for (int i = 1; i <= frame_count; i++)
{
Quaterniond r1(Rc[i]);
Quaterniond r2(Rc_g[i]);
double angular_distance = 180 / M_PI * r1.angularDistance(r2);
ROS_DEBUG(
"%d %f", i, angular_distance);
//huber核函數做加權
double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
++sum_ok;
Matrix4d L, R;
//L R 分別爲左乘和右乘矩陣
double w = Quaterniond(Rc[i]).w();
Vector3d q = Quaterniond(Rc[i]).vec();
L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
L.block<3, 1>(0, 3) = q;
L.block<1, 3>(3, 0) = -q.transpose();
L(3, 3) = w;
Quaterniond R_ij(Rimu[i]);
w = R_ij.w();
q = R_ij.vec();
R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
R.block<3, 1>(0, 3) = q;
R.block<1, 3>(3, 0) = -q.transpose();
R(3, 3) = w;
A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
}
//svd分解中最小奇異值對應的右奇異向量作爲旋轉四元數
JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
Matrix<double, 4, 1> x = svd.matrixV().col(3);
Quaterniond estimated_R(x);
ric = estimated_R.toRotationMatrix().inverse();
4. 至少迭代计算了WINDOW_SIZE次,其R的奇异值大于0。25才认为标定成功。
Vector3d ric_cov;
ric_cov = svd.singularValues().tail<3>();
if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
{
calib_ric_result = ric;
return true;
}
else
return false;
在初步标定完外参的旋转矩阵后,后续会对旋转矩阵与平移向量进行继续优化。