VINS-Fusion代码阅读（二）
前情回顾：
在上一篇中，我们主要读了两个类Estimator和IntegrationBase。忽然发现，如果本次再将IntegrationBase::evaluate()这个成员函数介绍完，整个IntegrationBase类就基本都介绍了。
总结一下：
IntegrationBase类在integration_base.h文件中，包含的成员函数共有5个分别为：
（已介绍）push_back，midPointIntegration，propagate；
（待介绍）evaluate，repropagate
evaluate成员函数的功能
传入参数：
Vector3d &Pi, Quaterniond &Qi, Vector3d &Vi, Vector3d &Bai, Vector3d &Bgi,
Vector3d &Pj, Quaterniond &Qj, Vector3d &Vj, Vector3d &Baj, Vector3d &Bgj
和解析上11页，IMU约束小节中列出的优化变量对应
[ p b k w , q b k w ] , [ v b k w , b a k , b ω k ] [p^w_{b_k},q^w_{b_k}], [v^w_{b_k}, b_{a_k}, b_{\omega_k}] [pbkw,qbkw],[vbkw,bak,bωk]
[ p b k + 1 w , q b k + 1 w ] , [ v b k + 1 w , b a k + 1 , b ω k + 1 ] [p^w_{b_{k+1}},q^w_{b_{k+1}}], [v^w_{b_{k+1}}, b_{a_{k+1}}, b_{\omega_{k+1}}] [pbk+1w,qbk+1w],[vbk+1w,bak+1,bωk+1]
传出参数：Eigen::Matrix
原文中，各增量关于bias的Jacobian可以从 大Jacobian的相应位置获得，体现在了代码中。如：
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
此处得到的Jacobian主要用于修正？
解析6页
α b k + 1 b k ≈ α ^ b k + 1 b k + J b a α δ b a + J b ω α δ b ω \alpha^{b_k}_{b_{k+1}}\approx \hat{\alpha}^{b_k}_{b_{k+1}} +J^\alpha_{b_a}\delta b_a+J^\alpha_{b_\omega}\delta b_\omega αbk+1bk≈α^bk+1bk+Jbaαδba+Jbωαδbω
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
β b k + 1 b k ≈ β ^ b k + 1 b k + J b a β δ b a + J b ω β δ b ω \beta^{b_k}_{b_{k+1}}\approx \hat{\beta}^{b_k}_{b_{k+1}} +J^\beta_{b_a}\delta b_a+J^\beta_{b_\omega}\delta b_\omega βbk+1bk≈β^bk+1bk+Jbaβδba+Jbωβδbω
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
γ b k + 1 b k ≈ γ ^ b k + 1 b k ⊗ [ 1 1 2 J b ω γ δ b ω ] \gamma^{b_k}_{b_{k+1}}\approx \hat{\gamma}^{b_k}_{b_{k+1}}\otimes \begin{bmatrix}1 \\ \frac{1}{2}J^\gamma_{b_\omega}\delta b_\omega \end{bmatrix} γbk+1bk≈γ^bk+1bk⊗[121Jbωγδbω]
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
（代码稍微有些不同，是Eigen库帮忙完成了计算么？）
最后，残差的计算：
解析中10页公式（20）
r B 15 × 1 ( z ^ b k + 1 b k , X ) = [ δ α b k + 1 b k δ θ b k + 1 b k δ β b k + 1 b k δ b a δ b g ] {r_B}^{15\times 1}(\hat{z}^{b_k}_{b_{k+1}},X)=\begin{bmatrix} \delta\alpha^{b_k}_{b_{k+1}} \\ \delta\theta^{b_k}_{b_{k+1}} \\ \delta\beta^{b_k}_{b_{k+1}} \\ \delta b_a \\ \delta b_g \end{bmatrix} rB15×1(z^bk+1bk,X)=⎣⎢⎢⎢⎢⎢⎡δαbk+1bkδθbk+1bkδβbk+1bkδbaδbg⎦⎥⎥⎥⎥⎥⎤
δ α b k + 1 b k = R w b k ( p b k + 1 w − p b k w − v b k w Δ t k + 1 2 g w Δ t k 2 ) − α b k + 1 b k \delta\alpha^{b_k}_{b_{k+1}} = R^{b_k}_w(p^w_{b_{k+1}}-p^w_{b_k}-v^w_{b_k}\Delta t_k+\frac{1}{2}g^w\Delta t^2_k)-\alpha^{b_k}_{b_{k+1}} δαbk+1bk=Rwbk(pbk+1w−pbkw−vbkwΔtk+21gwΔtk2)−αbk+1bk
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
δ θ b k + 1 b k = 2 [ γ b k + 1 b k − 1 ⊗ ( q b k w − 1 ⊗ q b k + 1 w ) ] \delta\theta^{b_k}_{b_{k+1}} = 2[{\gamma^{b_k}_{b_{k+1}}}^{-1}\otimes ({q^w_{b_k}}^{-1}\otimes {q^w_{b_{k+1}}})] δθbk+1bk=2[γbk+1bk−1⊗(qbkw−1⊗qbk+1w)]
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
δ β b k + 1 b k = R w b k ( v b k + 1 w − v b k w + g w Δ t k ) − β b k + 1 b k \delta\beta^{b_k}_{b_{k+1}}=R^{b_k}_w(v^w_{b_{k+1}}-v^w_{b_k}+g^w\Delta t_k)-\beta^{b_k}_{b_{k+1}} δβbk+1bk=Rwbk(vbk+1w−vbkw+gwΔtk)−βbk+1bk
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
δ b a = b a b k + 1 − b a b k \delta b_a=b_{a_{b_{k+1}}}-b_{a_{b_{k}}} δba=babk+1−babk
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
δ b g = b ω b k + 1 − b ω b k \delta b_g=b_{\omega_{b_{k+1}}}-b_{\omega_{b_{k}}} δbg=bωbk+1−bωbk
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
【注】：其中，O_P，O_R，O_V，O_BA，O_BG在estimator/parameters.h文件中定义，分别为：
enum StateOrder
{
O_P = 0,
O_R = 3,
O_V = 6,
O_BA = 9,
O_BG = 12
};
Eigen::Matrix 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 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;
}