1. 图优化
（1）使用方法
损失函数
F
(
x
)
F(x)
F(x)由残差函数
f
(
x
)
f(x)
f(x)组成：
min
x
F
(
x
)
=
1
2
∣
∣
f
(
x
)
∣
∣
2
2
\min_x F(x) = \frac{1}{2}||f(x)||_2^2
xminF(x)=21∣∣f(x)∣∣22
当考虑方差时，可以写为：
min
x
F
(
x
)
=
f
(
x
)
T
Ω
f
(
x
)
\min_x F(x) = f(x)^T\Omega f(x)
xminF(x)=f(x)TΩf(x)
利用高斯牛顿方法进行优化求解，可得到如下增量方程：
J
T
Ω
J
⏟
Δ
x
=
−
J
T
Ω
f
(
x
)
⏟
H
g
\underbrace{J^T\Omega J}\Delta x = \underbrace{-J^T\Omega f(x)} \\ H\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ g \ \ \ \ \
JTΩJΔx=
−JTΩf(x)H g
（2）优化中使用预积分的步骤
通过上述增量方程可知，需要获取的变量如下：
- 待优化变量的残差 Δ x \Delta x Δx
- 残差关于待优化变量的雅可比 J J J
- 残差的方差 Ω \Omega Ω
- 在预积分优化中，bias也作为待优化变量参与优化，为避免因bias变化而重新进行预积分，需要在bias处通过泰勒展开进行更新
2. IMU预积分约束
2.1 IMU预积分残差
（1）残差定义
已知姿态更新方法如下：
[
p
w
b
j
q
w
b
j
v
j
w
b
j
a
b
j
g
]
15
×
1
=
[
p
w
b
i
+
v
i
w
Δ
t
−
1
2
g
w
Δ
t
2
+
q
w
b
i
α
b
i
b
j
q
w
b
i
q
b
i
b
j
v
i
w
−
g
w
Δ
t
+
q
w
b
i
β
b
i
b
j
b
i
a
b
i
g
]
\left[\begin{array}{cc} p_{wb_j} \\ q_{wb_j} \\ v_j^w \\ b_j^a \\ b_j^g \end{array} \right]_{15\times1} = \left[\begin{array}{cc} p_{wb_i}+v_i^w\Delta{t}-\frac{1}{2}g^w\Delta{t^2}+q_{wb_i}\alpha_{b_ib_j} \\ q_{wb_i}q_{b_ib_j} \\ v_i^w-g^w\Delta{t}+q_{wb_i}\beta_{b_ib_j} \\ b_i^a \\ b_i^g \end{array} \right]
pwbjqwbjvjwbjabjg
15×1=
pwbi+viwΔt−21gwΔt2+qwbiαbibjqwbiqbibjviw−gwΔt+qwbiβbibjbiabig
于是，残差可以写为如下形式：
e
B
(
x
i
,
x
j
)
=
[
r
p
r
q
r
v
r
b
a
r
b
g
]
15
×
1
=
[
q
w
b
i
−
1
(
p
w
b
j
−
p
w
b
i
−
v
i
w
Δ
t
+
1
2
g
w
Δ
t
2
)
−
α
b
i
b
j
2
[
q
b
i
b
j
−
1
⊗
(
q
w
b
i
−
1
⊗
q
w
b
j
)
]
x
y
z
q
w
b
i
−
1
(
v
j
w
−
v
i
w
+
g
w
Δ
t
)
−
β
b
i
b
j
b
j
a
−
b
i
a
b
j
g
−
b
i
g
]
e_B(x_i,x_j) = \left[\begin{array}{cc} r_p \\ r_q \\ r_v \\ r_{ba} \\ r_{bg} \end{array} \right]_{15\times1} = \left[\begin{array}{cc} q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j} \\ 2[q_{b_ib_j}^{-1}\otimes{(q_{wb_i}^{-1}\otimes{q_{wb_j}})}]_{xyz} \\ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j} \\ b_j^a-b_i^a \\ b_j^g-b_i^g \end{array} \right]
eB(xi,xj)=
rprqrvrbarbg
15×1=
qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj2[qbibj−1⊗(qwbi−1⊗qwbj)]xyzqwbi−1(vjw−viw+gwΔt)−βbibjbja−biabjg−big
待优化变量为：
[
p
w
b
i
q
w
b
i
v
i
w
b
i
a
b
i
g
]
\left[\begin{array}{cc} p_{wb_i} & q_{wb_i} & v_i^w & b_i^a & b_i^g \end{array} \right]
[pwbiqwbiviwbiabig]
[
p
w
b
j
q
w
b
j
v
j
w
b
j
a
b
j
g
]
\left[\begin{array}{cc} p_{wb_j} & q_{wb_j} & v_j^w & b_j^a & b_j^g \end{array} \right]
[pwbjqwbjvjwbjabjg]
在求雅可比时，是利用残差对待优化变量的扰动来求导得到的：
[
δ
p
w
b
i
δ
q
w
b
i
δ
v
i
w
δ
b
i
a
δ
b
i
g
]
[
δ
p
w
b
j
δ
q
w
b
j
δ
v
j
w
δ
b
j
a
δ
b
j
g
]
\left[\begin{array}{cc} \delta p_{wb_i} & \delta q_{wb_i} & \delta v_i^w & \delta b_i^a & \delta b_i^g \end{array} \right] \\ \left[\begin{array}{cc} \delta p_{wb_j} & \delta q_{wb_j} & \delta v_j^w & \delta b_j^a & \delta b_j^g \end{array} \right]
[δpwbiδqwbiδviwδbiaδbig][δpwbjδqwbjδvjwδbjaδbjg]
（2）代码实现
主要是计算IMU中位姿，速度以及零偏的相邻时刻的残差，在计算时需要考虑零偏变化对IMU预积分的影响，因此需要先更新IMU预积分（在零偏处进行一阶泰勒展开）
// 计算和给定相邻帧状态量的残差
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; // 定义15维的残差项
// 下面的雅可比在预积分过程中已求解出
// 位移预积分在加速度零偏ba处的导数(一阶雅可比)
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
// 位移预积分在陀螺仪零偏bg处的导数(一阶雅可比)
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
// 姿态预积分在加速度零偏ba处的导数(一阶雅可比)
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
// 速度预积分在加速度零偏ba处的导数(一阶雅可比)
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
// 速度预积分在陀螺仪零偏bg处的导数(一阶雅可比)
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba; // 加速度零偏ba变化量
Eigen::Vector3d dbg = Bgi - linearized_bg; // 角速度零偏bg变化量
/*
delta_q, delta_v, delta_p为原先的预积分量
corrected_delta_q, corrected_delta_v, corrected_delta_p为考虑相应零偏变化的预积分量
因为零偏也是待优化变量，为了避免因零偏变化导致重新进行预积分，对原先预积分量在相应零偏处进行一阶泰勒展开
对姿态预积分量在零偏处(dbg)进行一阶泰勒展开(对应的雅可比为dp_dbg), 得到考虑零偏变化的新的姿态预积分量
*/
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
// 对速度预积分量在零偏处(dba和dbg)进行一阶泰勒展开(对应的雅可比分别为dp_dba和dp_dbg), 得到考虑零偏变化的新的速度预积分量
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
// 对平移预积分量在零偏处(dba和dbg)进行一阶泰勒展开(对应的雅可比分别为dp_dba和dp_dbg), 得到考虑零偏变化的新的平移预积分量
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
/*
Qi:q_{b_i}^w Qi:q_{b_j}^w G: g^w sum_dt:delta_t
Pj:p_{b_j}^w Pi:p_{b_i}^w Vi:v_{b_i}^w Vj:v_{b_j}^w
corrected_delta_p:alpha_{b_j}^{b_i} corrected_delta_q: q_{b_j}^{b_i}
corrected_delta_v: beta_{b_j}^{b_i}
*/
// 位置残差项
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(); // 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;
}
2.2 IMU残差对待优化变量的雅可比
2.2.1 残差对i时刻各状态量的雅可比
（1）位移残差在 i i i时刻对各状态量的雅可比
- i i i时刻位移残差 r p r_p rp对位移的雅可比
∂ r p ∂ δ p i p i , = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ p i p i , = ∂ q w b i − 1 ( − p w b i ) ∂ δ p i p i , = − q w b i − 1 \frac{\partial r_p}{\partial \delta p_ip_i^,} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta p_ip_i^,} = \frac{\partial q_{wb_i}^{-1}(-p_{wb_i})}{\partial \delta p_ip_i^,} = -q_{wb_i}^{-1} ∂δpipi,∂rp=∂δpipi,∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=∂δpipi,∂qwbi−1(−pwbi)=−qwbi−1
-
i
i
i时刻位移
r
p
r_p
rp对姿态的雅可比
- i i i时刻位移 r p r_p rp对速度的雅可比
∂ r p ∂ δ v i w = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ v i w = ∂ q w b i − 1 ( − v i w Δ t ) ∂ δ v i w = − q w b i − 1 Δ t \frac{\partial r_p}{\partial \delta v_i^w} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta v_i^w} = \frac{\partial q_{wb_i}^{-1}(-v_i^w \Delta t)}{\partial \delta v_i^w} = -q_{wb_i}^{-1}\Delta t ∂δviw∂rp=∂δviw∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=∂δviw∂qwbi−1(−viwΔt)=−qwbi−1Δt
- i i i时刻位移 r p r_p rp对加速度计偏置的雅可比
∂ r p ∂ δ b i a = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ b i a = ∂ ( − α b i b j ) ∂ δ b i a = ∂ ( − α b i b j − J b a α δ b a − J b g α δ b g ) ∂ δ b i a = − J b a α \frac{\partial r_p}{\partial \delta b_i^a} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta b_i^a} \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \frac{\partial (-\alpha_{b_ib_j})}{\partial \delta b_i^a} = \frac{\partial (-\alpha_{b_ib_j} - J_{b_a}^{\alpha}\delta b_a - J_{b_g}^{\alpha}\delta b_g)}{\partial \delta b_i^a} = - J_{b_a}^{\alpha} ∂δbia∂rp=∂δbia∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj] =∂δbia∂(−αbibj)=∂δbia∂(−αbibj−Jbaαδba−Jbgαδbg)=−Jbaα
- i i i时刻位移$ r p r_p rp对陀螺仪偏置的雅可比
∂ r p ∂ δ b i g = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ b i g = ∂ ( − α b i b j ) ∂ δ b i g = ∂ ( − α b i b j − J b a α δ b a − J b g α δ b g ) ∂ δ b i g = − J b g α \frac{\partial r_p}{\partial \delta b_i^g} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta b_i^g} \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \frac{\partial (-\alpha_{b_ib_j})}{\partial \delta b_i^g} = \frac{\partial (-\alpha_{b_ib_j} - J_{b_a}^{\alpha}\delta b_a - J_{b_g}^{\alpha}\delta b_g)}{\partial \delta b_i^g} = - J_{b_g}^{\alpha} ∂δbig∂rp=∂δbig∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj] =∂δbig∂(−αbibj)=∂δbig∂(−αbibj−Jbaαδba−Jbgαδbg)=−Jbgα
（2）姿态残差在 i i i时刻对各状态量的雅可比
- i i i时刻姿态残差 r q r_q rq对位移的雅可比
∂ r q ∂ δ p i p i , = ∂ 2 [ q b i b j − 1 ⊗ ( q w b i − 1 ⊗ q w b j ) ] x y z ∂ δ p i p i , = 0 \frac{\partial r_q}{\partial \delta p_ip_i^,} = \frac{\partial 2[q_{b_ib_j}^{-1}\otimes{(q_{wb_i}^{-1}\otimes{q_{wb_j}})}]_{xyz}}{\partial \delta p_ip_i^,} = 0 ∂δpipi,∂rq=∂δpipi,∂2[qbibj−1⊗(qwbi−1⊗qwbj)]xyz=0
-
i
i
i时刻姿态残差
r
q
r_q
rq对姿态的雅可比
- i i i时刻姿态残差 r q r_q rq对速度的雅可比
∂ r q ∂ δ v i w = ∂ 2 [ q b i b j − 1 ⊗ ( q w b i − 1 ⊗ q w b j ) ] x y z ∂ δ v i w = 0 \frac{\partial r_q}{\partial \delta v_i^w } = \frac{\partial 2[q_{b_ib_j}^{-1} \otimes (q_w{b_i}^{-1} \otimes q_{wb_j})]_{xyz}}{\partial \delta v_i^w } = 0 ∂δviw∂rq=∂δviw∂2[qbibj−1⊗(qwbi−1⊗qwbj)]xyz=0
- i i i时刻姿态残差 r q r_q rq对加速度计偏置的雅可比
∂ r q ∂ δ b i a = ∂ 2 [ q b i b j − 1 ⊗ ( q w b i − 1 ⊗ q w b j ) ] x y z ∂ δ b i a = 0 \frac{\partial r_q}{\partial \delta b_i^a } = \frac{\partial 2[q_{b_ib_j}^{-1} \otimes (q_{wb_i}^{-1} \otimes q_{wb_j})]_{xyz}}{\partial \delta b_i^a } = 0 ∂δbia∂rq=∂δbia∂2[qbibj−1⊗(qwbi−1⊗qwbj)]xyz=0
-
i i i时刻姿态残差 r q r_q rq对陀螺仪偏置的雅可比
（3）速度残差在
i
i
i时刻对各状态量的雅可比 -
i i i时刻速度残差 r v r_v rv对位移的雅可比
∂ r v ∂ δ p b i b i , = ∂ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ∂ δ p b i b i , = 0 \frac{\partial r_v}{\partial \delta p_{b_ib_i^,}} = \frac{\partial \ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}}{\partial \delta p_{b_ib_i^,}} = 0 ∂δpbibi,∂rv=∂δpbibi,∂ qwbi−1(vjw−viw+gwΔt)−βbibj=0
-
i
i
i时刻速度残差
r
v
r_v
rv对姿态的雅可比
- i i i时刻速度残差 r v r_v rv对速度的雅可比
∂ r v ∂ δ v i w = ∂ [ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ] ∂ δ v i w = − q w b i − 1 \frac{\partial r_v}{\partial \delta v_i^w} = \frac{\partial [q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}]}{\partial \delta v_i^w} = -q_{wb_i}^{-1} ∂δviw∂rv=∂δviw∂[qwbi−1(vjw−viw+gwΔt)−βbibj]=−qwbi−1
- i i i时刻速度残差 r v r_v rv对加速度计偏置的雅可比
∂ r v ∂ δ b i a = ∂ [ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ] ∂ δ b i a = ∂ ( − β b i b j ) ∂ δ b i a = ∂ ( − β b i b j − J b a β δ b a − J b g β δ b g ) ∂ δ b i a = − J b a β \frac{\partial r_v}{\partial \delta b_i^a} = \frac{\partial [q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}]}{\partial \delta b_i^a} \\ = \frac{\partial (-\beta_{b_ib_j})}{\partial \delta b_i^a} = \frac{\partial (-\beta_{b_ib_j} - J_{b_a}^{\beta}\delta b_a - J_{b_g}^{\beta}\delta b_g)}{\partial \delta b_i^a} = - J_{b_a}^{\beta} ∂δbia∂rv=∂δbia∂[qwbi−1(vjw−viw+gwΔt)−βbibj]=∂δbia∂(−βbibj)=∂δbia∂(−βbibj−Jbaβδba−Jbgβδbg)=−Jbaβ
-
i i i时刻速度残差 r v r_v rv对陀螺仪偏置的雅可比
（4）加速度计偏置残差在 i i i时刻对各状态量的雅可比 -
i i i时刻加速度计偏置残差 r b a r_{b_a} rba对位移的雅可比
∂ r b a ∂ δ p i p i , = ∂ ( b j a − b i a ) ∂ ∂ δ p i p i , = 0 \frac{\partial \ r_{b_a}}{\partial \delta p_ip_i^,} = \frac{\partial (b_j^a - b_i^a)}{\partial \partial \delta p_ip_i^,} = 0 ∂δpipi,∂ rba=∂∂δpipi,∂(bja−bia)=0
- i i i时刻加速度计偏置残差 r b a r_{b_a} rba对姿态的雅可比
∂ r b a ∂ δ θ b i b i , = ∂ ( b j a − b i a ) ∂ δ θ b i b i , = 0 \frac{\partial \ r_{b_a}}{\partial \delta \theta_{b_ib_i^,}} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta \theta_{b_ib_i^,}} = 0 ∂δθbibi,∂ rba=∂δθbibi,∂(bja−bia)=0
- i i i时刻加速度计偏置残差 r b a r_{b_a} rba对速度的雅可比
∂ r b a ∂ δ v i w = ∂ ( b j a − b i a ) ∂ δ v i w = 0 \frac{\partial \ r_{b_a}}{\partial \delta v_i^w} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta v_i^w} = 0 ∂δviw∂ rba=∂δviw∂(bja−bia)=0
- i i i时刻加速度计偏置残差 r b a r_{b_a} rba对加速度计偏置的雅可比
∂ r b a ∂ δ b i a = ∂ ( b j a − b i a ) ∂ δ b i a = − I \frac{\partial \ r_{b_a}}{\partial \delta b_i^a} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta b_i^a} = -I ∂δbia∂ rba=∂δbia∂(bja−bia)=−I
- i i i时刻加速度计偏置残差 r b a r_{b_a} rba对陀螺仪偏置的雅可比
∂ r b a ∂ δ b i g = ∂ ( b j a − b i a ) ∂ δ b i g = 0 \frac{\partial \ r_{b_a}}{\partial \delta b_i^g} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta b_i^g} = 0 ∂δbig∂ rba=∂δbig∂(bja−bia)=0
（5）陀螺仪偏置残差在 i i i时刻对各状态量的雅可比
- i i i时刻陀螺仪偏置残差 r b g r_{b_g} rbg对位移的雅可比
∂ r b g ∂ δ p i p i , = ∂ ( b j g − b i g ) ∂ δ p i p i , = 0 \frac{\partial \ r_{b_g}}{\partial \delta p_ip_i^,} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta p_ip_i^,} = 0 ∂δpipi,∂ rbg=∂δpipi,∂(bjg−big)=0
- i i i时刻陀螺仪偏置残差 r b g r_{b_g} rbg对姿态的雅可比
∂ r b g ∂ δ θ b i b i , = ∂ ( b j g − b i g ) ∂ δ θ b i b i , = 0 \frac{\partial \ r_{b_g}}{\partial \delta \theta_{b_ib_i^,}} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta \theta_{b_ib_i^,}} = 0 ∂δθbibi,∂ rbg=∂δθbibi,∂(bjg−big)=0
- i i i时刻陀螺仪偏置残差 r b g r_{b_g} rbg对速度的雅可比
∂ r b g ∂ δ v i w = ∂ ( b j g − b i g ) ∂ δ v i w = 0 \frac{\partial \ r_{b_g}}{\partial \delta v_i^w} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta v_i^w} = 0 ∂δviw∂ rbg=∂δviw∂(bjg−big)=0
- i i i时刻陀螺仪偏置残差 r b g r_{b_g} rbg对加速度计偏置的雅可比
∂ r b g ∂ δ b i a = ∂ ( b j g − b i g ) ∂ δ b i a = 0 \frac{\partial \ r_{b_g}}{\partial \delta b_i^a} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta b_i^a} = 0 ∂δbia∂ rbg=∂δbia∂(bjg−big)=0
- i i i时刻陀螺仪偏置残差 r b g r_{b_g} rbg对陀螺仪偏置的雅可比
∂ r b g ∂ δ b i g = ∂ ( b j g − b i g ) ∂ δ b i g = − I \frac{\partial \ r_{b_g}}{\partial \delta b_i^g} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta b_i^g} = -I ∂δbig∂ rbg=∂δbig∂(bjg−big)=−I
2.2.2 残差对j时刻各状态量的雅可比
（1）位移残差在 j j j时刻对各状态量的雅可比
- j j j时刻位移残差 r p r_p rp对位移的雅可比
∂ r p ∂ δ p j p j , = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ p j p j , = ∂ q w b i − 1 ( p w b j ) ∂ δ p j p j , = q w b i − 1 \frac{\partial r_p}{\partial \delta p_jp_j^,} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta p_jp_j^,} = \frac{\partial q_{wb_i}^{-1}(p_{wb_j})}{\partial \delta p_jp_j^,} = q_{wb_i}^{-1} ∂δpjpj,∂rp=∂δpjpj,∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=∂δpjpj,∂qwbi−1(pwbj)=qwbi−1
- j j j时刻位移残差 r p r_p rp对姿态的雅可比
∂ r p ∂ δ θ b j b j , = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ θ b j b j , = 0 \frac{\partial r_p}{\partial \delta \theta_{b_jb_j^,}} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta \theta_{b_jb_j^,}} = 0 ∂δθbjbj,∂rp=∂δθbjbj,∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=0
- j j j时刻位移残差 r p r_p rp对速度的雅可比
∂ r p ∂ δ v j w = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ v j w = 0 \frac{\partial r_p}{\partial \delta v_j^w} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta v_j^w} = 0 ∂δvjw∂rp=∂δvjw∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=0
- j j j时刻位移残差 r p r_p rp对加速度计偏置的雅可比
∂ r p ∂ δ b i a = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ b i a = 0 \frac{\partial r_p}{\partial \delta b_i^a} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta b_i^a} = 0 ∂δbia∂rp=∂δbia∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=0
- j j j时刻位移残差 r p r_p rp对陀螺仪偏置的雅可比
∂ r p ∂ δ b i g = ∂ [ q w b i − 1 ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j ] ∂ δ b i g = 0 \frac{\partial r_p}{\partial \delta b_i^g} = \frac{\partial [q_{wb_i}^{-1}(p_{wb_j} - p_{wb_i}-v_i^w\Delta{t}+\frac{1}{2}g^w\Delta{t^2})-\alpha_{b_ib_j}]}{\partial \delta b_i^g} = 0 ∂δbig∂rp=∂δbig∂[qwbi−1(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj]=0
（2）姿态残差在 j j j时刻对各状态量的雅可比
- j j j时刻姿态残差 r q r_q rq对位移的雅可比
∂ r q ∂ δ p j p j , = ∂ 2 [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j , ] ] x y z ∂ δ p j p j , = 0 \frac{\partial r_q}{\partial \delta p_jp_j^,} = \frac{\partial \ 2[q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}\delta \theta_{b_jb_j^,} \end{array} \right]]_{xyz}}{\partial \delta p_jp_j^,} = 0 ∂δpjpj,∂rq=∂δpjpj,∂ 2[qbibj−1⊗qwbi−1⊗qwbj⊗[121δθbjbj,]]xyz=0
- j j j时刻姿态残差 r q r_q rq对姿态的雅可比
∂ r q ∂ δ θ b j b j , = ∂ 2 [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j , ] ] x y z ∂ δ θ b i b i , = ∂ 2 [ [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ] L [ 1 1 2 δ θ b j b j , ] ] x y z ∂ δ θ b i b i , = 2 [ 0 I ] [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ] L [ 0 1 2 I ] \frac{\partial r_q}{\partial \delta \theta_{b_jb_j^,}} = \frac{\partial 2[q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}\delta \theta_{b_jb_j^,} \end{array} \right]]_{xyz}}{\partial \delta \theta_{b_ib_i^,}} = \frac{\partial 2[[q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j}]_L \left[\begin{array}{cc} 1 \\ \frac{1}{2}\delta \theta_{b_jb_j^,} \end{array} \right]]_{xyz}}{\partial \delta \theta_{b_ib_i^,}} \\ = 2 \left[\begin{array}{cc} 0 & I \end{array} \right] [q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j}]_L \left[\begin{array}{cc} 0 \\ \frac{1}{2}I \end{array} \right] ∂δθbjbj,∂rq=∂δθbibi,∂2[qbibj−1⊗qwbi−1⊗qwbj⊗[121δθbjbj,]]xyz=∂δθbibi,∂2[[qbibj−1⊗qwbi−1⊗qwbj]L[121δθbjbj,]]xyz=2[0I][qbibj−1⊗qwbi−1⊗qwbj]L[021I]
- j j j时刻姿态残差 r q r_q rq对速度的雅可比
∂ r q ∂ δ v j w = ∂ 2 [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j , ] ] x y z ∂ δ v j w = 0 \frac{\partial r_q}{\partial \delta v_j^w} = \frac{\partial \ 2[q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}\delta \theta_{b_jb_j^,} \end{array} \right]]_{xyz}}{\partial \delta v_j^w} = 0 ∂δvjw∂rq=∂δvjw∂ 2[qbibj−1⊗qwbi−1⊗qwbj⊗[121δθbjbj,]]xyz=0
- j j j时刻姿态残差 r q r_q rq对加速度计偏置的雅可比
∂ r q ∂ δ b j a = ∂ 2 [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j , ] ] x y z ∂ δ b j a = 0 \frac{\partial r_q}{\partial \delta b_j^a} = \frac{\partial \ 2[q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}\delta \theta_{b_jb_j^,} \end{array} \right]]_{xyz}}{\partial \delta b_j^a} = 0 ∂δbja∂rq=∂δbja∂ 2[qbibj−1⊗qwbi−1⊗qwbj⊗[121δθbjbj,]]xyz=0
- j j j时刻姿态残差 r q r_q rq对陀螺仪偏置的雅可比
∂ r q ∂ δ b j g = ∂ 2 [ q b i b j − 1 ⊗ q w b i − 1 ⊗ q w b j ⊗ [ 1 1 2 δ θ b j b j , ] ] x y z ∂ δ b j g = 0 \frac{\partial r_q}{\partial \delta b_j^g} = \frac{\partial \ 2[q_{b_ib_j}^{-1} \otimes q_{wb_i}^{-1} \otimes q_{wb_j} \otimes \left[\begin{array}{cc} 1 \\ \frac{1}{2}\delta \theta_{b_jb_j^,} \end{array} \right]]_{xyz}}{\partial \delta b_j^g} = 0 ∂δbjg∂rq=∂δbjg∂ 2[qbibj−1⊗qwbi−1⊗qwbj⊗[121δθbjbj,]]xyz=0
（3）速度残差在 j j j时刻对各状态量的雅可比
- j j j时刻速度残差 r v r_v rv$对位移的雅可比
∂ r v ∂ δ p j p j , = ∂ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ∂ δ p j p j , = 0 \frac{\partial r_v}{\partial \delta p_jp_j^,} = \frac{\partial \ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}}{\partial \delta p_jp_j^,} = 0 ∂δpjpj,∂rv=∂δpjpj,∂ qwbi−1(vjw−viw+gwΔt)−βbibj=0
- j j j时刻速度残差 r v r_v rv对姿态的雅可比
∂ r v ∂ δ θ b j b j , = ∂ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ∂ δ θ b j b j , = 0 \frac{\partial r_v}{\partial \delta \theta_{b_jb_j^,}} = \frac{\partial \ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}}{\partial \delta \theta_{b_jb_j^,}} = 0 ∂δθbjbj,∂rv=∂δθbjbj,∂ qwbi−1(vjw−viw+gwΔt)−βbibj=0
- j j j时刻速度残差 r v r_v rv对速度的雅可比
∂ r v ∂ δ v j w = ∂ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ∂ δ v j w = q w b i − 1 \frac{\partial r_v}{\partial \delta v_j^w} = \frac{\partial \ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}}{\partial \delta v_j^w} = q_{wb_i}^{-1} ∂δvjw∂rv=∂δvjw∂ qwbi−1(vjw−viw+gwΔt)−βbibj=qwbi−1
- j j j时刻速度残差 r v r_v rv对加速度计偏置的雅可比
∂ r v ∂ δ b j a = ∂ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ∂ δ b j a = 0 \frac{\partial r_v}{\partial \delta b_j^a} = \frac{\partial \ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}}{\partial \delta b_j^a} = 0 ∂δbja∂rv=∂δbja∂ qwbi−1(vjw−viw+gwΔt)−βbibj=0
- j j j时刻速度残差 r v r_v rv对陀螺仪偏置的雅可比
∂ r v ∂ δ b j g = ∂ q w b i − 1 ( v j w − v i w + g w Δ t ) − β b i b j ∂ δ b j g = 0 \frac{\partial r_v}{\partial \delta b_j^g} = \frac{\partial \ q_{wb_i}^{-1}(v_j^w - v_i^w+g^w\Delta{t})-\beta_{b_ib_j}}{\partial \delta b_j^g} = 0 ∂δbjg∂rv=∂δbjg∂ qwbi−1(vjw−viw+gwΔt)−βbibj=0
（4）加速度计偏置残差在 j j j时刻对各状态量的雅可比
- j j j时刻加速度计偏置残差 r b a r_{b_a} rba对位移的雅可比
∂ r b a ∂ δ p j p j , = ∂ ( b j a − b i a ) ∂ δ p j p j , = 0 \frac{\partial \ r_{b_a}}{\partial \delta p_jp_j^,} = \frac{\partial (b_j^a - b_i^a)}{\partial\delta p_jp_j^,} = 0 ∂δpjpj,∂ rba=∂δpjpj,∂(bja−bia)=0
- j j j时刻加速度计偏置残差 r b a r_{b_a} rba对姿态的雅可比
∂ r b a ∂ δ θ b j b j , = ∂ ( b j a − b i a ) ∂ δ θ b j b j , = 0 \frac{\partial \ r_{b_a}}{\partial \delta \theta_{b_jb_j^,}} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta \theta_{b_jb_j^,}} = 0 ∂δθbjbj,∂ rba=∂δθbjbj,∂(bja−bia)=0
- j j j时刻加速度计偏置残差 r b a r_{b_a} rba对速度的雅可比
∂ r b a ∂ δ v j w = ∂ ( b j a − b i a ) ∂ δ v j w = 0 \frac{\partial \ r_{b_a}}{\partial \delta v_j^w} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta v_j^w} = 0 ∂δvjw∂ rba=∂δvjw∂(bja−bia)=0
- j j j时刻加速度计偏置残差 r b a r_{b_a} rba对加速度计偏置的雅可比
∂ r b a ∂ δ b j a = ∂ ( b j a − b i a ) ∂ δ b j a = I \frac{\partial \ r_{b_a}}{\partial \delta b_j^a} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta b_j^a} = I ∂δbja∂ rba=∂δbja∂(bja−bia)=I
- j j j时刻加速度计偏置残差 r b a r_{b_a} rba对陀螺仪偏置的雅可比
∂ r b a ∂ δ b j g = ∂ ( b j a − b i a ) ∂ δ b j g = 0 \frac{\partial \ r_{b_a}}{\partial \delta b_j^g} = \frac{\partial (b_j^a - b_i^a)}{\partial \delta b_j^g} = 0 ∂δbjg∂ rba=∂δbjg∂(bja−bia)=0
（5）陀螺仪偏置残差在 j j j时刻对各状态量的雅可比
- j j j时刻陀螺仪偏置残差 r b g r_{b_g} rbg对位移的雅可比
∂ r b g ∂ δ p j p j , = ∂ ( b j g − b i g ) ∂ δ p j p j , = 0 \frac{\partial \ r_{b_g}}{\partial \delta p_jp_j^,} = \frac{\partial (b_j^g - b_i^g)}{\partial\delta p_jp_j^,} = 0 ∂δpjpj,∂ rbg=∂δpjpj,∂(bjg−big)=0
- j j j时刻陀螺仪偏置残差 r b g r_{b_g} rbg对姿态的雅可比
∂ r b g ∂ δ θ b j b j , = ∂ ( b j g − b i g ) ∂ δ θ b j b j , = 0 \frac{\partial \ r_{b_g}}{\partial \delta \theta_{b_jb_j^,}} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta \theta_{b_jb_j^,}} = 0 ∂δθbjbj,∂ rbg=∂δθbjbj,∂(bjg−big)=0
- j j j时刻陀螺仪偏置残差 r b g r_{b_g} rbg对速度的雅可比
∂ r b g ∂ δ v j w = ∂ ( b j g − b i g ) ∂ δ v j w = 0 \frac{\partial \ r_{b_g}}{\partial \delta v_j^w} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta v_j^w} = 0 ∂δvjw∂ rbg=∂δvjw∂(bjg−big)=0
- j j j时刻陀螺仪偏置残差 r b g r_{b_g} rbg对加速度计偏置的雅可比
∂ r b g ∂ δ b j a = ∂ ( b j g − b i g ) ∂ δ b j a = 0 \frac{\partial \ r_{b_g}}{\partial \delta b_j^a} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta b_j^a} = 0 ∂δbja∂ rbg=∂δbja∂(bjg−big)=0
- j j j时刻陀螺仪偏置残差 r b g r_{b_g} rbg对陀螺仪偏置的雅可比
∂ r b g ∂ δ b j g = ∂ ( b j g − b i g ) ∂ δ b j g = I \frac{\partial \ r_{b_g}}{\partial \delta b_j^g} = \frac{\partial (b_j^g - b_i^g)}{\partial \delta b_j^g} = I ∂δbjg∂ rbg=∂δbjg∂(bjg−big)=I
2.2.3 残差对各状态量的雅可比计算
（1）代码分析
前面已推导出残差对
i
i
i时刻和
j
j
j时刻各状态量的雅可比形式，将总雅可比矩阵进行分块，记残差对位姿的雅可比为
∂
e
∂
P
\frac{\partial e}{\partial P}
∂P∂e，残差对速度、加速度计零偏以及陀螺仪零偏的雅可比为
∂
e
∂
V
\frac{\partial e}{\partial V}
∂V∂e。由于残差维度为15，位置和姿态的自由度分别为3和4，故
∂
e
∂
P
\frac{\partial e}{\partial P}
∂P∂e的维度是
15
×
7
15 \times 7
15×7。同理，由于速度、加速度计零偏以及陀螺仪零偏均为3，故
∂
e
∂
V
\frac{\partial e}{\partial V}
∂V∂e的维度是
15
×
9
15 \times 9
15×9。雅可比矩阵由残差对
k
k
k时刻和
k
+
1
k+1
k+1时刻的雅可比组成，形式如下：
[
∂
e
∂
P
k
∂
e
∂
V
k
∂
e
∂
P
k
+
1
∂
e
∂
V
k
+
1
]
\left[\begin{array}{cc} \frac{\partial e}{\partial P_k} & \frac{\partial e}{\partial V_k} & \frac{\partial e}{\partial P_{k+1}} & \frac{\partial e}{\partial V_{k+1}} \end{array} \right]
[∂Pk∂e∂Vk∂e∂Pk+1∂e∂Vk+1∂e]
（2）代码实现
// 15: 维护两帧之间的PVQ + 加速度零偏 + 角速度零偏
// 7: 位姿自由度
// 9: 速度与零偏自由度
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
{
public:
IMUFactor() = delete;
IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration)
{
}
/**
* @brief 使用ceres解析求导，必须重载这个函数
*
* @param[in] parameters 这是一个二维数组，每个参数块都是一个double数组，而一个观测会对多个参数块形成约束
* @param[in] residuals 残差的计算结果，是一个一维数组，残差就是该观测量和约束的状态量通过某种关系形成残差
* @param[in] jacobians 残差对参数块的雅克比矩阵，这是一个二维数组，其中对任意一个参数块的雅克比矩阵都是一个一维数组
* @return true
* @return false
*/
virtual bool Evaluate(double const *const *parameters,
double *residuals,
double **jacobians) const
{
// 便于后续计算，把double类型的参数块都转换成eigen
// imu预积分约束的参数是相邻两帧的位姿 速度和零偏 i表示前一帧, j表示后一帧
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3],
parameters[0][4], parameters[0][5]);
Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);
Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3],
parameters[2][4], parameters[2][5]);
Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);
// 定义残差大小 将double类型的残差映射成15*1的eigen对象
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
// 得到残差(具体实现在下面)
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
// 因为ceres没有g2o设置信息矩阵的接口，因此置信度直接乘在残差上，这里通过LLT分解，相当于将信息矩阵开根号
// 也就是说进行如下转换: 令P = pp^T, 则e^TPe = e^Tpp^Te = (p^Te)^T(p^Te) p^Te作为新的残差
// 其中, P为预积分的协方差矩阵的逆(即信息矩阵)
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
// 这就是带有信息矩阵的残差
residual = sqrt_info * residual;
// 下面进行雅可比的计算
if (jacobians)
{
double sum_dt = pre_integration->sum_dt;
// 预先求出对偏置的雅可比
Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);
if (pre_integration->jacobian.maxCoeff() > 1e8 ||
pre_integration->jacobian.minCoeff() < -1e8)
{
ROS_WARN("numerical unstable in preintegration");
}
if (jacobians[0])
{
// 第0列 —— 第6列(前7列)计算残差对第i时刻位姿的雅可比(平移3个量, 旋转4个量)
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>>
jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
// i时刻平移残差对平移量的雅可比 Qi: q_{b_i}^w
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
// i时刻平移残差对姿态(旋转矩阵R)的雅可比
// R_w^{b_i} * [0.5gdelta_t^2 + Pj - Pi - vi * delta_t]
// Utility::skewSymmetric的作用是将向量转换成反对称矩阵
jacobian_pose_i.block<3, 3>(O_P, O_R)=Utility::skewSymmetric(Qi.inverse()
* (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
#if 0
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).
toRotationMatrix();
#else // 零偏修正后的旋转预积分量: q_{b_j}^{b_i}
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
// Qleft(Qj.inverse() * Qi): [q_w^{b_j} * q_{b_I}^w]_L 左乘矩阵
// Qright(corrected_delta_q): 旋转预积分量corrected_delta_q的右乘矩阵
// bottomRightCorner<3, 3>: 取右下角的3*3矩阵为虚部
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
// i时刻速度残差对姿态(旋转矩阵R)的雅可比 Qi: q_{b_i}^w
jacobian_pose_i.block<3, 3>(O_V, O_R) =
Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
// 求出的雅可比矩阵再乘上前面计算出来的信息矩阵作为权重
jacobian_pose_i = sqrt_info * jacobian_pose_i;
if (jacobian_pose_i.maxCoeff()>1e8 || jacobian_pose_i.minCoeff() < -1e8)
{
ROS_WARN("numerical unstable in preintegration");
}
}
if (jacobians[1])
{
// 第7列 —— 第15列(共9列)计算残差对第i时刻速度v_i、加速度偏置b_{a_i}、角速度偏置b_{w_i}的雅可比(3个3维，共9维)
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>>
jacobian_speedbias_i(jacobians[1]);
jacobian_speedbias_i.setZero();
// j时刻平移残差对速度量的雅可比: -q_w^{b_i} * \delta_t Qi: q_{b_i}^w
jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -
Qi.inverse().toRotationMatrix() * sum_dt;
// j时刻平移残差对加速度偏置的雅可比: -J_{b_a}^{\alpha}
jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
// j时刻平移残差对角速度偏置的雅可比: -J_{b_g}^{\alpha}
jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
#if 0
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
// [(q_{b_j}^w)^{-1} \otimes q_{b_i}^w \otimes q_{b_i}^{b_i}]_L * J_{b_g}^q
// 其中, q_{b_j}^w由Qj表示 q_{b_i}^w由Qi表示
// 预积分量q_{b_i}^{b_i}由pre_integration->delta_q表示
// dq_dbg表示姿态对角速度偏置的雅可比J_{b_g}^q
// bottomRightCorner<3, 3>()取右下角的3*3矩阵，即取虚部
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft
(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif
// i时刻速度残差对速度量的雅可比: -q_w^{b_i}
jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -
Qi.inverse().toRotationMatrix();
// i时刻速度残差对加速度偏置的雅可比: -J_{b_a}^{\beta}
jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
// i时刻速度残差对角速度偏置的雅可比: -J_{b_g}^{\beta}
jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;
// i时刻加速度偏置残差对加速度偏置的雅可比: -I
jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -
Eigen::Matrix3d::Identity();
// i时刻角速度偏置残差对角速度偏置的雅可比: -I
jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -
Eigen::Matrix3d::Identity();
// 求出的雅可比矩阵再乘上前面计算出来的信息矩阵作为权重
jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
}
if (jacobians[2])
{
// 第16列 —— 第22列(共7列)计算残差对第j时刻位姿的雅可比(平移3个量, 旋转4个量)
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>>
jacobian_pose_j(jacobians[2]);
jacobian_pose_j.setZero();
// j时刻平移残差对平移量的雅可比 Qj: q_{b_j}^w
jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();
#if 0
jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
// 零偏修正后的旋转预积分量: q_{b_j}^{b_i}
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
// Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj): [q_{b_i}^{b_j} \otimes q_w^{b_i} \otimes q_{b_j}^w]_L 左乘矩阵
// bottomRightCorner<3, 3>: 取右下角的3*3矩阵为虚部
jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft
(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
jacobian_pose_j = sqrt_info * jacobian_pose_j;
}
if (jacobians[3])
{
// 第23列 —— 第31列(共9列)计算残差对第j时刻速度v_j、加速度偏置b_{a_j}、角速度偏置b_{w_j}的雅可比(3个3维，共9维)
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>>
jacobian_speedbias_j(jacobians[3]);
jacobian_speedbias_j.setZero();
// j时刻速度残差对速度量的雅可比 q_w^{b_i}
jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) =
Qi.inverse().toRotationMatrix();
// j时刻加速度偏置残差对加速度偏置的雅可比
jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) =
Eigen::Matrix3d::Identity();
// j时刻角速度偏置残差对角速度偏置的雅可比
jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) =
Eigen::Matrix3d::Identity();
jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
}
}
return true;
}
};
扫一扫
1111
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
