VINS-Mono代码总结-IMU预积分与残差计算
关于VINS-Mono系统的理论推导和代码讲解，网上有很多优秀的博客，如
VINS-Mono代码分析与总结(一) IMU预积分,VINS-Mono源码解析（三）后端: IMU预积分,VINS-Mono预积分公式推导,VINS-Mono 论文公式推导与代码解析,VINS-Mono代码分析总结VINS-Mono理论学习——IMU预积分 Pre-integration （Jacobian 协方差) VINS论文推导及代码解析 by 崔华坤。本文主要参考这些讲解，结合代码和理论推导，对IMU预积分部分进行梳理与总结，对看代码的过程中的一些疑惑进行分析讨论。
IMU模型
IMU测量值包括加速度计得到的线速度
a
^
b
\hat a_b
a^b和陀螺仪得到的角速度
w
^
b
\hat w_b
w^b，有
a
^
b
=
a
b
+
b
a
b
+
R
w
b
g
w
+
n
a
w
^
b
=
w
b
+
b
w
b
+
n
w
\hat a_b=a_b+b_{a_b}+R_w^bg^w+n_a \\ \hat w_b=w_b+b_{w_b}+n_w
a^b=ab+bab+Rwbgw+naw^b=wb+bwb+nw
其中，
b
b
b下标代表body坐标系，IMU测量值受到加速度偏置
b
a
b_a
ba，陀螺仪偏置
b
w
b_w
bw和附加噪声
n
a
,
n
w
n_a,n_w
na,nw的影响，线加速度是重力加速度和物体加速度的合矢量。
假设附加噪声为高斯噪声：
n
a
∽
N
(
0
,
σ
a
2
)
,
n
w
∽
N
(
0
,
σ
w
2
)
n_a \backsim N(0,\sigma_a^2),n_w \backsim N(0,\sigma_w^2)
na∽N(0,σa2),nw∽N(0,σw2)
加速度计偏置和陀螺仪偏置被建模为随机游走，其导数为高斯性的
b
˙
a
b
=
n
b
a
,
b
˙
w
b
=
n
b
w
n
b
a
∽
N
(
0
,
σ
b
a
2
)
,
n
b
w
∽
N
(
0
,
σ
b
w
2
)
\dot b_{a_b}=n_{b_a},\dot b_{w_b}=n_{b_w} \\ n_{b_a}\backsim N(0,\sigma_{b_a}^2),n_{b_w}\backsim N(0,\sigma_{b_w}^2)
b˙ab=nba,b˙wb=nbwnba∽N(0,σba2),nbw∽N(0,σbw2)
对于图像帧
k
k
k和
k
+
1
k+1
k+1，体坐标系对应为
b
k
,
b
k
+
1
b_k,b_{k+1}
bk,bk+1，位置/速度和方向状态值PVQ可以根据
[
t
k
,
t
k
+
1
]
[t_k,t_k+1]
[tk,tk+1]时间间隔内的IMU测量值，在世界坐标系下进行传递：
P
b
k
+
1
w
=
P
b
k
w
Δ
t
k
+
∬
t
∈
[
t
k
,
t
k
+
1
]
(
R
b
t
w
(
a
^
t
−
b
a
t
−
n
a
)
−
g
w
)
d
t
2
V
b
k
+
1
w
=
V
b
k
w
+
∫
t
∈
[
t
k
,
t
k
+
1
]
(
R
b
t
w
(
a
^
t
−
b
a
t
−
n
a
)
−
g
w
)
d
t
Q
b
k
+
1
w
=
Q
b
k
w
⊗
∫
t
∈
[
t
k
,
t
k
+
1
]
1
2
Q
b
t
b
k
⊗
[
(
w
^
t
−
b
w
t
−
n
w
)
0
]
=
Q
b
k
w
⊗
∫
t
∈
[
t
k
,
t
k
+
1
]
1
2
Ω
(
w
^
t
−
b
w
t
−
n
w
)
d
t
P_{b_{k+1}}^w=P_{b_k}^w\Delta t_k+\iint_{t\in [t_k,t_{k+1}]}(R_{b_t}^w(\hat a_t-b_{a_t}-n_a)-g^w)dt^2 \\ V_{b_{k+1}}^w=V_{b_k}^w+\int_{t\in [t_k,t_{k+1}]}(R_{b_t}^w(\hat a_t-b_{a_t}-n_a)-g^w)dt \\ Q_{b_{k+1}}^w=Q_{b_k}^w \otimes\int_{t\in [t_k,t_{k+1}]}\frac{1}{2}Q_{b_t}^{b_k}\otimes \begin{bmatrix} (\hat w_t-b_{w_t}-n_w) \\ 0 \end{bmatrix}=Q_{b_k}^w \otimes\int_{t\in [t_k,t_{k+1}]}\frac{1}{2} \Omega(\hat w_t-b_{w_t}-n_w)dt
Pbk+1w=PbkwΔtk+∬t∈[tk,tk+1](Rbtw(a^t−bat−na)−gw)dt2Vbk+1w=Vbkw+∫t∈[tk,tk+1](Rbtw(a^t−bat−na)−gw)dtQbk+1w=Qbkw⊗∫t∈[tk,tk+1]21Qbtbk⊗[(w^t−bwt−nw)0]=Qbkw⊗∫t∈[tk,tk+1]21Ω(w^t−bwt−nw)dt
Δ
t
k
=
t
k
+
1
−
t
k
\Delta t_k=t_{k+1}-t_k
Δtk=tk+1−tk，
R
b
t
w
R_{b_t}^w
Rbtw为
t
t
t时刻从本体坐标系到世界坐标系的旋转矩阵，可以看到坐标系
b
k
+
1
b_{k+1}
bk+1的状态PVQ依赖于
b
k
b_{k}
bk的状态。如果直接将体坐标系的状态作为变量放在非线性优化中，在进行后端优化时会迭代更新，将导致我们需要根据每次迭代后的状态量重新进行状态传递，即需要重新传递IMU状态值，将会产生很大的计算量。
##离散形式
上式对应的基于中值积分的离散形式，如下：
P
b
k
+
1
w
=
P
b
k
w
+
V
b
k
w
Δ
t
k
+
1
2
a
^
t
ˉ
δ
t
2
V
b
k
+
1
w
=
V
b
k
w
+
a
^
t
ˉ
δ
t
Q
b
k
+
1
w
=
Q
b
k
w
⊗
[
1
1
2
w
^
t
ˉ
δ
t
]
a
^
k
ˉ
=
1
2
[
Q
b
k
(
a
k
^
−
b
a
k
)
−
g
w
+
Q
b
k
+
1
(
a
k
^
−
b
a
k
)
−
g
w
]
w
^
t
ˉ
=
1
2
(
w
^
t
+
w
^
t
+
1
)
−
b
w
k
P_{b_{k+1}}^w=P_{b_k}^w+V_{b_k}^w\Delta t_k+\frac{1}{2}\bar{ \hat a_t }\delta t^2 \\ V_{b_{k+1}}^w = V_{b_k}^w+\bar{ \hat a_t }\delta t \\ Q_{b_{k+1}}^w = Q_{b_k}^w \otimes \begin{bmatrix} 1 \\ \frac{1}{2}\bar{\hat w_t}\delta t \end{bmatrix} \\ \bar{ \hat a_k }=\frac{1}{2}[Q_{b_k}(\hat {a_k}-b_{a_k})-g^w+Q_{b_{k+1}}(\hat {a_k}-b_{a_{k}})-g^w] \\ \bar{\hat w_t}=\frac{1}{2}(\hat w_t+\hat w_{t+1})-b_{w_k}
Pbk+1w=Pbkw+VbkwΔtk+21a^tˉδt2Vbk+1w=Vbkw+a^tˉδtQbk+1w=Qbkw⊗[121w^tˉδt]a^kˉ=21[Qbk(ak^−bak)−gw+Qbk+1(ak^−bak)−gw]w^tˉ=21(w^t+w^t+1)−bwk
即取
k
,
k
+
1
k,k+1
k,k+1时刻的角速度和线加速度的测量值的均值作为当前时间段内的角速度和加速度，然后进行积分计算求解位姿。
这部分实现是在函数
void Estimator::processIMU()
中的，我们具体来看其实现。
1.初始化
if (!first_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]};
}
2. 预积分
开始预积分
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);
linear_acceleration, angular_velocity分别是当前的测量值，上述两个函数执行同样的操作，计算预计分，因为
k
,
k
+
1
k,k+1
k,k+1时刻之间有很多IMU数据，每到来一个IMU数据，就会计算并累加一次预计分。
3. 存储测量值到buf中
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);
4. 基于中值积分对当前的位姿进行估计
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
其中，acc_0，gyr_0是上一时刻的测量值，代码分别与前述中值积分的公式相对应。这里所得到的位姿估计是用于后面作为非线性优化的初始值。
IMU预积分
这里对预积分的具体实现进行介绍。IMU预积分的思想就是将参考坐标系从世界坐标系调整到第
k
k
k帧的体坐标系
b
k
b_k
bk，即同时乘以
R
w
b
k
R_w^{b_k}
Rwbk
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
Q
w
b
k
⊗
Q
b
k
+
1
w
=
γ
b
k
+
1
b
k
α
b
k
+
1
b
k
=
∬
t
∈
[
t
k
,
t
k
+
1
]
R
t
b
k
(
a
^
t
−
b
a
t
−
n
a
)
d
t
2
β
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
R
t
b
k
(
a
^
t
−
b
a
t
−
n
a
)
d
t
γ
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
1
2
Ω
(
w
^
t
−
b
w
t
−
n
w
)
γ
t
b
k
d
t
R_w^{b_k}P_{b_{k+1}}^w=R_w^{b_k}(P_{b_k}^w+V_{b_k}^w\Delta t_k-\frac{1}{2}g^w\Delta t_k^2)+\alpha_{b_{k+1}}^{b_k} \\ R_w^{b_k}V_{b_{k+1}}^w=R_w^{b_k}(V_{b_k}^w-g^w\Delta t_k)+\beta_{b_{k+1}}^{b_k} \\ Q_w^{b_k}\otimes Q_{b_{k+1}}^w=\gamma_{b_{k+1}}^{b_k} \\ \alpha_{b_{k+1}}^{b_k}=\iint_{t\in [t_k,t_{k+1}]}R_t^{b_k}(\hat a_t-b_{a_t}-n_a)dt^2 \\ \beta_{b_{k+1}}^{b_k} =\int_{t\in [t_k,t_{k+1}]}R_t^{b_k}(\hat a_t-b_{a_t}-n_a)dt \\ \gamma_{b_{k+1}}^{b_k}=\int_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega(\hat w_t-b_{w_t}-n_w)\gamma_t^{b_k}dt
RwbkPbk+1w=Rwbk(Pbkw+VbkwΔtk−21gwΔtk2)+αbk+1bkRwbkVbk+1w=Rwbk(Vbkw−gwΔtk)+βbk+1bkQwbk⊗Qbk+1w=γbk+1bkαbk+1bk=∬t∈[tk,tk+1]Rtbk(a^t−bat−na)dt2βbk+1bk=∫t∈[tk,tk+1]Rtbk(a^t−bat−na)dtγbk+1bk=∫t∈[tk,tk+1]21Ω(w^t−bwt−nw)γtbkdt
此时的积分结果
α
b
k
+
1
b
k
,
β
b
k
+
1
b
k
,
γ
b
k
+
1
b
k
\alpha_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k},\gamma_{b_{k+1}}^{b_k}
αbk+1bk,βbk+1bk,γbk+1bk可以理解为
b
k
+
1
b_{k+1}
bk+1对
b
k
b_k
bk的相对运动量，
b
k
b_k
bk的状态的改变并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递。下面对其离散形式进行介绍，
代码中基于中值积分对
α
b
k
+
1
b
k
,
β
b
k
+
1
b
k
,
γ
b
k
+
1
b
k
\alpha_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k},\gamma_{b_{k+1}}^{b_k}
αbk+1bk,βbk+1bk,γbk+1bk进行计算，在
IntergrationBase::push_back()
中实现。对应的计算公式为
α
^
i
+
1
b
k
=
α
^
i
b
k
+
β
^
i
b
k
δ
t
+
1
2
a
^
i
ˉ
δ
t
2
β
^
i
+
1
b
k
=
β
^
i
b
k
+
a
^
i
ˉ
δ
t
γ
^
i
+
1
b
k
=
γ
^
i
b
k
⊗
[
1
1
2
w
^
i
ˉ
δ
t
]
a
^
i
ˉ
=
1
2
[
Q
i
(
a
^
i
−
b
a
i
)
+
Q
i
+
1
(
a
^
i
+
1
−
b
a
i
)
]
w
^
i
ˉ
=
1
2
(
w
^
i
+
w
^
i
+
1
)
−
b
w
i
\hat \alpha_{i+1}^{b_k}=\hat \alpha_{i}^{b_k}+\hat \beta_i^{b_k} \delta_t+\frac{1}{2} \bar{\hat a_i}\delta t^2 \\ \hat \beta_{i+1}^{b_k}=\hat \beta_{i}^{b_k}+ \bar{\hat a_i}\delta t \\ \hat \gamma_{i+1}^{b_k}=\hat \gamma_{i}^{b_k} \otimes \begin{bmatrix} 1 \\ \frac{1}{2} \bar{\hat w_i}\delta t \end{bmatrix} \\ \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})] \\ \bar{\hat w_i}=\frac{1}{2}(\hat w_i+\hat w_{i+1})-b_{w_i}
α^i+1bk=α^ibk+β^ibkδt+21a^iˉδt2β^i+1bk=β^ibk+a^iˉδtγ^i+1bk=γ^ibk⊗[121w^iˉδt]a^iˉ=21[Qi(a^i−bai)+Qi+1(a^i+1−bai)]w^iˉ=21(w^i+w^i+1)−bwi
初始状态
α
b
k
+
1
b
k
,
β
b
k
+
1
b
k
\alpha_{b_{k+1}}^{b_k},\beta_{b_{k+1}}^{b_k}
αbk+1bk,βbk+1bk为0，
γ
b
k
+
1
b
k
\gamma_{b_{k+1}}^{b_k}
γbk+1bk为单位四元数，
n
a
,
n
w
n_a,n_w
na,nw被视为0。
所对应的代码为
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);
}
中值积分包含在
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;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
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);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
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;
}
下面是中值积分的函数
首先，按照公式进行积分计算。
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");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
接下来就是更新误差传递的协方差矩阵
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
预积分残差计算
预积分所形成的约束边，对应的代码在void Estimator::problemSolve()中，为
// IMU
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[j]));
std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;
edge_vertex.push_back(vertexCams_vec[i]);
edge_vertex.push_back(vertexVB_vec[i]);
edge_vertex.push_back(vertexCams_vec[j]);
edge_vertex.push_back(vertexVB_vec[j]);
imuEdge->SetVertex(edge_vertex);
problem.AddEdge(imuEdge);
}
其中，vertexCams_vec[i]和vertexCams_vec[j]分别为两个时刻的body坐标系的全局位姿估计，对应的是para_Pose。vertexVB_vec[i]和vertexVB_vec[j]分别为两个时刻的速度，陀螺仪和加速度偏置，对应的是para_SpeedBias。在增加了约束边之后，在接下来的非线性优化中就要对该边的残差进行计算，对应的函数为
void EdgeImu::ComputeResidual() {
VecX param_0 = verticies_[0]->Parameters();
Qd Qi(param_0[6], param_0[3], param_0[4], param_0[5]);
Vec3 Pi = param_0.head<3>();
VecX param_1 = verticies_[1]->Parameters();
Vec3 Vi = param_1.head<3>();
Vec3 Bai = param_1.segment(3, 3);
Vec3 Bgi = param_1.tail<3>();
VecX param_2 = verticies_[2]->Parameters();
Qd Qj(param_2[6], param_2[3], param_2[4], param_2[5]);
Vec3 Pj = param_2.head<3>();
VecX param_3 = verticies_[3]->Parameters();
Vec3 Vj = param_3.head<3>();
Vec3 Baj = param_3.segment(3, 3);
Vec3 Bgj = param_3.tail<3>();
residual_ = pre_integration_->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
// Mat1515 sqrt_info = Eigen::LLT< Mat1515 >(pre_integration_->covariance.inverse()).matrixL().transpose();
SetInformation(pre_integration_->covariance.inverse());
}
首先是取出节点的值，然后进入到pre_integration_->evaluate()函数中，进行计算残差。其代码为
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}
下面对代码进行说明。首先获取预积分的PQV向量对陀螺仪和加速度偏置的雅克比矩阵，然后计算偏置的变化量，然后根据偏置的更新对预积分的值进行矫正，矫正后的预积分为corrected_delta_q，corrected_delta_v，corrected_delta_p。
下面对上述对应的理论公式进行说明。
在求解预积分的时候，是假设IMU的偏置
b
a
,
b
w
b_a,b_w
ba,bw已经确定，实际上在后续的优化中对偏置也进行了优化。那么每次迭代时，
b
a
,
b
w
b_a,b_w
ba,bw发生改变，需要重新根据公式求得所有帧之间的IMU的预积分。当偏置变化很小时，可以将预积分值按其对偏置的一阶近似来调整，否则就进行重新传递reprogagte。
α
b
k
+
1
b
k
≈
α
^
b
k
+
1
b
k
+
J
b
a
α
δ
b
a
+
J
b
w
α
δ
b
w
β
b
k
+
1
b
k
≈
β
^
b
k
+
1
b
k
+
J
b
a
β
δ
b
a
+
J
b
w
β
δ
b
w
γ
b
k
+
1
b
k
≈
γ
b
k
+
1
b
k
^
⊗
[
1
1
2
J
b
w
γ
δ
b
w
]
\alpha_{b_{k+1}}^{b_k}\approx \hat \alpha_{b_{k+1}}^{b_k}+J_{b_a}^{\alpha}\delta b_a+J_{b_w}^{\alpha}\delta b_w \\ \beta_{b_{k+1}}^{b_k}\approx \hat \beta_{b_{k+1}}^{b_k}+J_{b_a}^{\beta}\delta b_a+J_{b_w}^{\beta}\delta b_w \\ \gamma_{b_{k+1}}^{b_k}\approx \hat{\gamma_{b_{k+1}}^{b_k}} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}J_{b_w}^{\gamma}\delta b_w\end{bmatrix}
αbk+1bk≈α^bk+1bk+Jbaαδba+Jbwαδbwβbk+1bk≈β^bk+1bk+Jbaβδba+Jbwβδbwγbk+1bk≈γbk+1bk^⊗[121Jbwγδbw]
然后残差的计算是按照当前估计量对应的预积分值减去根据测量值得到的预积分值的偏差得到的。
附协方差以及雅克比矩阵的推导.
410
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
