非常全局的一个关于VINS-Mono的总结
IMU预积分部分
Estimator
是一个类,processIMU
是它的一个成员函数;
该函数的作用:使用中值法 求解 当前时刻的PVQ
传入参数:IMU传感器的输出——加速度、角速度;
传出参数:
同时,其中用到的一些数据成员(按使用顺序)列举如下:
bool first_imu;
起一个标志作用,初始值为false
;
IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];
是一个数组,里面存放着(WINDOW_SIZE + 1)
个指针,指针指向的类型是IntegrationBase
;详见PS.1
int frame_count;
个人理解为滑窗中图片的帧数;在Estimator::clearState()
这个成员函数中被赋初值为0。感觉上限为WINDOW_SIZE
。
IntegrationBase *tmp_pre_integration;
和pre_integrations
中的一个元素类似;
vector<double> dt_buf[(WINDOW_SIZE + 1)];
记录IMU数据之间的间隔时间?是Vector
?什么含义?
linear_acceleration_buf
和angular_velocity_buf
类似;
Matrix3d Rs[(WINDOW_SIZE + 1)];
一个数组
Bas
,Bgs
,
PS.
IntegrationBase
是一个类型,定义在factor/integration_bash.h
中,有待下一步分析;
pre_integrations[frame_count]->push_back
是该类型中的一个成员函数,有待分析;
WINDOW_SIZE
是在estimator/parameters.h
中定义的一个参数,(个人暂时理解为滑窗的大小const int WINDOW_SIZE = 10;
)
与使用中值法求解当前时刻PVQ的公式与代码的对应:
ω
^
ˉ
i
=
1
2
(
ω
^
i
+
ω
^
i
+
1
)
−
b
ω
i
\bar{\hat{\omega}}_i=\frac{1}{2}({\hat{\omega}}_i+{\hat{\omega}}_{i+1})-b_{\omega_i}
ω^ˉi=21(ω^i+ω^i+1)−bωi
(2) Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
在已知上一时刻姿态矩阵的情况下,使用陀螺仪的角增量(近似旋转矢量),可以求出当前时刻的姿态矩阵:
q
b
i
+
1
w
=
q
b
i
+
1
w
⊗
[
1
1
2
ω
^
ˉ
i
δ
t
]
q^{w}_{b_{i+1}}=q^{w}_{b_{i+1}}\otimes \begin{bmatrix}1\\ \frac{1}{2}\bar{\hat{\omega}}_i \delta t \end{bmatrix}
qbi+1w=qbi+1w⊗[121ω^ˉiδt]
(3) Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
(感觉j=frame_count
不变,即在新的图像未到来之前,Rs[j]
是在不断根据IMU的输出而刷新的)。
(1)(4)(5)
对应的是:
a
^
ˉ
i
=
1
2
[
q
i
(
a
^
i
−
b
a
i
)
−
g
w
+
q
i
+
1
(
a
^
i
+
1
−
b
a
i
)
−
g
w
]
\bar{\hat{a}}_i=\frac{1}{2}[q_i(\hat{a}_i-b_{a_i})-g^w+q_{i+1}(\hat{a}_{i+1}-b_{a_i})-g^w]
a^ˉi=21[qi(a^i−bai)−gw+qi+1(a^i+1−bai)−gw]
(1) Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
(4) Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
(5) Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
最后,解析上公式(2)的前两行
p
b
i
+
1
w
=
p
b
i
w
+
v
b
i
w
δ
t
+
1
2
a
^
ˉ
i
δ
t
2
p^w_{b_{i+1}}=p^w_{b_{i}}+v^w_{b_i}\delta t+\frac{1}{2}\bar{\hat{a}}_i \delta t^2
pbi+1w=pbiw+vbiwδt+21a^ˉiδt2
(6) Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
v
b
i
+
1
w
=
v
b
i
w
+
a
^
ˉ
i
δ
t
v^w_{b_{i+1}}=v^w_{b_i}+\bar{\hat{a}}_i\delta t
vbi+1w=vbiw+a^ˉiδt
(7) Vs[j] += dt * un_acc;
(变量上 一巴 代表的是平均,程序里加了前缀un_
;变量上 一尖 代表的是测量值;)
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (!first_imu) // 第一组IMU数据的处理
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
if (!pre_integrations[frame_count]) // 如果为空
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]}; // 构造函数需要四个参数
}
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
int j = frame_count;
(1) Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
(2) Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
(3) Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
(4) Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
(5) Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
(6) Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
(7) Vs[j] += dt * un_acc;
}
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
=============== 分割线 ===================
前面并不知道在pre_integrations[frame_count]->push_back
中发生了什么,下面来分析:
class IntegrationBase{}
是一个类型,push_back()
是它的一个成员函数;
该类型的数据成员依照函数中出现的先后顺序,依次介绍:
std::vector<double> dt_buf;
类似地,gyr_buf
和acc_buf
;
push_back()
这个成员函数最后调用了另一个成员函数propagate()
,且原封不动地把自己的三个输入参数都传递了过去。
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr); // 传播?
}
进入propagate()
成员函数,其中数据成员有:
double dt;
Eigen::Vector3d acc_0, gyr_0;
保存上一时刻的值?
Eigen::Vector3d acc_1, gyr_1;
使用当前时刻传入的值赋值
const Eigen::Vector3d linearized_acc, linearized_gyr;
const常量
剩余的数据成员,用来保存结果
double sum_dt;
Eigen::Vector3d delta_p;
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_v;
Eigen::Vector3d linearized_ba, linearized_bg;
本函数的核心在于midPointIntegration()
,可以和解析里(8)
式对应了?
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p; // 几个中间变量,用来保存midPointIntegration的结果
Quaterniond result_delta_q; // 四元数!
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
// 函数实参中,前面不带result_的几个为输入,后面几个带result_的为输出
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
midPointIntegration()
函数
传入参数:
输出参数:
其中,(2)
对应公式
ω
^
ˉ
i
=
1
2
(
ω
^
i
+
ω
^
i
+
1
)
−
b
ω
i
\bar{\hat{\omega}}_i=\frac{1}{2}(\hat{\omega}_i+\hat{\omega}_{i+1})-b_{\omega_i}
ω^ˉi=21(ω^i+ω^i+1)−bωi
公式符号与变量符号对应:
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
同时,这里使用Eigen::Quaterniond
类型的delta_q
(上一时刻的姿态四元数?),以及dt
时间内陀螺的角增量输出(旋转矢量?)计算了result_delta_q
(当前时刻的姿态四元数)。解析上公式(8)的第三个式子:
γ
^
i
+
1
b
k
=
γ
^
i
b
k
⊗
γ
^
i
i
+
1
=
γ
^
i
b
k
⊗
[
1
1
2
ω
^
ˉ
i
δ
t
]
\hat{\gamma}^{b_k}_{i+1}=\hat{\gamma}^{b_k}_{i}\otimes \hat{\gamma}^{i+1}_{i}=\hat{\gamma}^{b_k}_{i}\otimes \begin{bmatrix}1 \\ \frac{1}{2}\bar{\hat{\omega}}_i \delta t \end{bmatrix}
γ^i+1bk=γ^ibk⊗γ^ii+1=γ^ibk⊗[121ω^ˉiδt]
公式和代码的对应:
(3) result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
【注意】:这里有一个奇怪的对应,代码中的同一个量,对应了公式中的两个不同符号?暂时还未搞明白。
(1)(4)(5)
构成了公式
a
^
ˉ
i
=
1
2
[
q
i
(
a
^
i
−
b
a
i
)
+
q
i
+
1
(
a
^
i
+
1
−
b
a
i
)
]
\bar{\hat{a}}_i=\frac{1}{2}[q_i(\hat{a}_i-b_{a_i})+q_{i+1}(\hat{a}_{i+1}-b_{a_i})]
a^ˉi=21[qi(a^i−bai)+qi+1(a^i+1−bai)]
公式符号与变量符号对应:
(5) Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
(1) Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
(4) Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
【注意】:result_delta_q
是Eigen::Quaterniond
类型的四元数,
(
4
×
1
)
(4\times 1)
(4×1),与一个
(
3
×
1
)
(3\times 1)
(3×1)的向量相乘时,使用的是重载的乘法(十四讲)!即代码上的q*v
表示数学上的
q
v
q
−
1
qvq^{-1}
qvq−1,其中为四元数乘,
v
v
v需要先改写成乘第一个元素为0的四元数然后再参与运算。
最后,根据解析上(8)中的第二式和第一式:
β
^
i
+
1
b
k
=
β
^
i
b
k
+
a
^
ˉ
i
δ
t
\hat{\beta}^{b_k}_{i+1}=\hat{\beta}^{b_k}_{i}+\bar{\hat{a}}_i\delta t
β^i+1bk=β^ibk+a^ˉiδt
与之对应的代码:
(7)result_delta_v = delta_v + un_acc * _dt;
α
^
i
+
1
b
k
=
α
^
i
b
k
+
β
^
i
b
k
δ
t
+
1
2
a
^
ˉ
i
δ
t
2
\hat{\alpha}^{b_k}_{i+1}=\hat{\alpha}^{b_k}_{i}+\hat{\beta}^{b_k}_{i}\delta t+\frac{1}{2}\bar{\hat{a}}_i\delta t^2
α^i+1bk=α^ibk+β^ibkδt+21a^ˉiδt2
与之对应的代码:
(6)result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
这样就使用中值积分完成了两帧之间PVQ增量的计算。 下一步,还要具体看一下调用他们的函数。
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
(1)Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
(2)Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
(3)result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
(4)Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
(5)Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
(6)result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
(7)result_delta_v = delta_v + un_acc * _dt;
(8)result_linearized_ba = linearized_ba;
(9)result_linearized_bg = linearized_bg;
if(update_jacobian)
{ // bala 见最后,矩阵的块状赋值,很长但比较简单
}
}
bool update_jacobian
是一个布尔型的变量,且一个传入的值。根据实参我们知道,给它的赋值为1,。
解析上(15)式,分别给出了F
和V
的结构,分别就其中一个小块解读代码:
f
03
=
−
1
4
(
R
k
+
R
k
+
1
)
δ
t
2
f_{03}=-\frac{1}{4}(R_{k}+R_{k+1})\delta t^2
f03=−41(Rk+Rk+1)δt2
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
v
00
=
−
1
4
R
k
δ
t
2
v_{00}=-\frac{1}{4}R_k\delta t^2
v00=−41Rkδt2
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
解析上(16)式
J
k
+
1
15
×
15
=
F
J
k
{J_{k+1}}^{15\times 15}=FJ_{k}
Jk+115×15=FJk
jacobian = F * jacobian;
解析上(17)式
P
k
+
1
15
×
15
=
F
P
k
F
T
+
V
Q
V
T
{P_{k+1}}^{15\times 15}=FP_kF^T+VQV^T
Pk+115×15=FPkFT+VQVT
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
其中,jacobian
和covariance
是IntegrationBase
类型下的成员函数,在执行构造函数的时候(即实例化一个对象时),二者的初值分别为:
J
b
k
=
I
J_{b_k}=I
Jbk=I和
P
b
k
b
k
=
0
P^{b_k}_{b_k}=0
Pbkbk=0
对应构造函数中的代码:
jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, covariance{Eigen::Matrix<double, 15, 15>::Zero()},