s-msckf代码笔记（二）

二、VIO

VIO中IMU的状态向量为：
$${\text{x}}_I={\left(\begin{array}{ccccccc}{}_G^I{\text{q}}^T& {\text{b}}_g^T& {}^G{\text{v}}_I^T& {\text{b}}_a^T& {}^G{\text{p}}_I^T& {}_C^I{\text{q}}^T& {}^I{\text{p}}_C^T\end{array}\right)}^T$$
误差状态为：
$${\tilde{\text{x}}}_I={\left(\begin{array}{ccccccc}{}_G^I{\tilde{\mathbf{\theta }}}^T& {\tilde{\mathbf{b}}}_g^T& {}^G{\tilde{\text{v}}}^T& {\tilde{\text{b}}}_a^T& {}^G{\tilde{\text{p}}}_I^T& {}_C^I{\tilde{\mathbf{\theta }}}^T& {}^I{\tilde{\text{p}}}_C^T\end{array}\right)}^T$$
对于普通的变量，如${\text{b}}_g,{\text{v}}_I$等，满足通用的加减关系，比如${}^G{\tilde{\text{p}}}_I={}^G{\text{p}}_I-{}^G{\hat{\text{p}}}_I$。对于四元数，误差四元数为：
$$\begin{gathered} \delta \text{q}=\text{q}\otimes {\hat{\text{q}}}^{-1} \\ \approx {\left(\begin{array}{cc}\frac{1}{2}{}_I^G{\tilde{\mathbf{\theta }}}^T& 1\end{array}\right)}^T \end{gathered}$$
其中${}_I^G{\tilde{\mathbf{\theta }}}^T\in {\mathbb{R}}^3$表示一个微小的旋转，通过这种方式将旋转误差降到了三维。考虑最终的误差状态向量，设有N个相机状态，则最终的误差状态向量为：
$$\tilde{\text{x}}={\left(\begin{array}{cccc}{\tilde{\text{x}}}_I^T& {\tilde{\text{x}}}_{C_1}& \dots & {\tilde{\text{x}}}_{C_N}\end{array}\right)}^T$$

$${\tilde{\text{x}}}_{C_i}={\left(\begin{array}{cc}{}_G^{C_i}{\tilde{\mathbf{\theta }}}^T& {}^G{\tilde{\text{p}}}_{C_i}^T\end{array}\right)}^T$$

3 VIO初始化

(1) 初始化imu各噪声项，存在state_server.continuous_noise_cov中；

double IMUState::gyro_noise = 0.001;
double IMUState::acc_noise = 0.01;
double IMUState::gyro_bias_noise = 0.001;
double IMUState::acc_bias_noise = 0.01;

continuous_noise_cov为12x12的对角矩阵

(2) 初始化卡方检验表，置信度为0.95(不知道做什么用)，存在chi_squared_test_table中。
(3) 创建ROS IO接口，订阅imu、features、mocap_odom，发布reset、gt_odom。

imuCallback
接收到的imu数据放在缓存imu_msg_buffer中，如果数据个数大于200，初始化重力和偏置。由于s-msckf默认从静止状态初始化，所以用加速度平均值初始化重力，角速度平均值初始化角速度偏置。把is_gravity_set设置为true。

得到重力向量后构建世界坐标系，令重力方向为z轴负方向。然后定义惯性系相对于世界坐标系的朝向，这里我看的有点懵，他先求解了一个旋转$\text{R}$,使得：
$$\text{R}{\text{g}}^I=-{\text{g}}^w$$
然后把初始时刻的旋转${\text{q}}_{iw}$定义为:
$${\text{q}}_{iw}=quaternion(R^T)$$
这一步目前还不是很懂

double gravity_norm = gravity_imu.norm();
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);
Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
    gravity_imu, -IMUState::gravity);
state_server.imu_state.orientation =
    rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());

4 featureCallback

4.1 一些预设置

首先判断重力是否已经设置，即判断is_gravity_set是否为true;再判断是否为第一张图像，如果是，把is_first_img置为false，把state_server.imu_state.time置为该图像时间。

4.2 batchImuProcessing

该函数主要用于积分上一次积分时间到当前图像时间的imu数据，也就是积分相邻两个图像时间内的imu数据，并且构建协方差矩阵。

4.2.1 processModel

该函数用于构建$\text{F}$矩阵、$\text{G}$矩阵和$\mathbf{\Phi }$矩阵，更新状态协方差$\text{P}$和状态变量$\text{X}$，其中$\text{P}$存放在state_server.state_cov里。

4.2.1.1 构建$\text{F}$、$\text{G}$和$\mathbf{\Phi }$

$${\text{F}}_{21\times 21}=\left(\begin{array}{ccccc}-{\hat{\mathbf{\omega }}}_{\times }& -{\text{I}}_3& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ -C({}_G^I\hat{\text{q}}{)}^T{\hat{\text{a}}}_{\times }& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& -C({}_G^I\hat{\text{q}}{)}^T& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{I}}_3& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\end{array}\right)$$

$${\text{G}}_{21\times 12}=\left(\begin{array}{cccc}-{\text{I}}_3& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& -{\text{I}}_3& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& -C({}_G^I\hat{\text{q}}{)}^T& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{I}}_3\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\\ {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}& {\text{0}}_{3\times 3}\end{array}\right)$$

其中
$$\mathbf{\Phi }=\exp ({\int }_{t_k}^{t_{k+1}}\text{F}(\tau )d\tau )$$
这里采用3阶泰勒展开来取近似
$$\mathbf{\Phi }\approx \text{I}+\text{F}{d}_t+\frac{1}{2}(\text{F}{d}_t{)}^2+\frac{1}{6}(\text{F}{d}_t{)}^3$$

4.2.1.2 predictNewState: 利用RK4积分预测状态

RK4积分一般形式
$${\text{x}}_{n+1}={\text{x}}_n+\frac{\Delta t}{6}(k_1+2k_2+2k_3+k_4)$$

$$\begin{gathered} k_1=f(t_n,{\text{x}}_n) \\ {k}_2=f(t_n+\frac{1}{2}\Delta t,{\text{x}}_n+\frac{\Delta t}{2}{k}_1) \\ {k}_3=f(t_n+\frac{1}{2}\Delta t,{\text{x}}_n+\frac{\Delta t}{2}{k}_2) \\ {k}_4=f(t_n+\Delta t,{\text{x}}_n+\Delta t\cdot k_3) \end{gathered}$$

这一步要对PVQ的状态进行更新，其中对PV的更新用的是RK4积分，对于Q的更新是角速度乘以间隔时间。PVQ保存在state_server.imu_state里面。

Vector4d& q = state_server.imu_state.orientation;
Vector3d& v = state_server.imu_state.velocity;
Vector3d& p = state_server.imu_state.position;

a.更新Q

首先计算角速度的二范数$\Vert \mathbf{\omega }{\Vert }_2$，然后乘以时间间隔得到旋转角度$\varphi$。
$$\varphi =\Vert \mathbf{\omega }{\Vert }_2\cdot \Delta t$$
于是$\varphi$对应的四元数为
$$\text{q}=\left(\begin{array}{c}\cos (\varphi /2)\\ \text{u}\sin (\varphi /2)\end{array}\right)=\left(\begin{array}{c}{q}_w\\ {\text{q}}_v\end{array}\right)$$

其中$\text{u}$为旋转轴的单位向量。于是更新后的四元数为：
$$\begin{gathered} {\text{q}}_{k+1}=\text{q}\otimes {\text{q}}_k \\ =(q_w\text{I}+\left(\begin{array}{cc}-\lfloor \mathbf{\omega }\times \rfloor & \mathbf{\omega }\\ -{\mathbf{\omega }}^T& 0\end{array}\right)){\text{q}}_k \\ =(q_w\text{I}+\mathbf{\Omega }(\mathbf{\omega })){\text{q}}_k \end{gathered}$$

b.PV的更新( 这部分d1和d2在代码里没完全弄明白，待深究 )

V:
$$\begin{gathered} k_1^v={\text{q}}_k^{-1}\otimes {\text{a}}_k+{\text{g}}^w \\ {k}_2^v=d2\ast {\text{a}}_k+{\text{g}}^w \\ {k}_3^v=d2\ast {\text{a}}_k+{\text{g}}^w \\ {k}_4^v=d1\ast {\text{a}}_k+{\text{g}}^w \end{gathered}$$
P:
$$\begin{gathered} k_1^p={\text{v}}_k \\ {k}_2^p={\text{v}}_k+\frac{1}{2}{k}_1^v\Delta t \\ {k}_3^p={\text{v}}_k+\frac{1}{2}{k}_2^v\Delta t \\ {k}_4^p={\text{v}}_k+k_3^v\Delta t \end{gathered}$$
然后把上面的系数代入RK4积分公式就得到了${\text{v}}_{k+1},{\text{p}}_{k+1}$，然后更新state_server.imu_state。

4.2.1.3 更新转移矩阵$\mathbf{\Phi }$和协方差矩阵$\text{P}$

a.$\mathbf{\Phi }$的更新（ 感觉像是某种trick，论文里面没有详述，待深究)

b.$\text{P}$的更新
$${\text{P}}_{I{I}_{k+1|k}}={\mathbf{\Phi }}_k{\text{P}}_{I{I}_{k|k}}{\mathbf{\Phi }}_k^T+{\text{Q}}_k$$
其中
$${\text{Q}}_k={\int }_{t_k}^{t_{k+1}}\mathbf{\Phi }(t_{k+1},\tau )\text{GQG}\mathbf{\Phi }(t_{k+1},\tau {)}^{T}d\tau$$
代码中对${\text{Q}}_k$取近似：
$${\text{Q}}_k\approx {\mathbf{\Phi }}_k\text{GQG}{\mathbf{\Phi }}_k^T\Delta t$$

Matrix Q = Phi*G*state_server.continuous_noise_cov*
	G.transpose()*Phi.transpose()*dtime;
state_server.state_cov.block<21, 21>(0, 0) =
    Phi*state_server.state_cov.block<21, 21>(0, 0)*Phi.transpose() + Q;

c.对相机状态协方差的更新（之后补上)

4.2.2 更新状态ID，删除已经积分的imu数据

4.3 stateAugmentation : 添加新的相机状态，更新状态协方差

a.根据imu数据的积分值和imu和相机之间的外参给相机状态（相对于世界坐标系的平移、旋转）赋值。
$${}_G^C\widehat{\bar{\text{q}}}={}_I^C\bar{\text{q}}\otimes {}_I^G\widehat{\bar{\text{q}}}\text{\hspace{1em}(32)}$$

$${}^G{\hat{\text{p}}}_C={}^G{\hat{\text{p}}}_I+C_{\hat{\text{q}}}^T{}^I{\text{p}}_C\text{\hspace{1em}(33)}$$

b.更新状态协方差矩阵(待详细推导)

a中得到了一个新的相机位姿，该相机位姿添加到状态向量中，对应的状态协方差矩阵也要进行相应的更新（augmented)，设该相机位姿为第N+1个相机位姿，其对应的误差状态为：
$$\delta T_{C_{N+1}\leftarrow G}=\left(\begin{array}{c}\delta {\mathbf{\theta }}_{C_{N+1}\leftarrow G}\\ {}^G{\tilde{\text{p}}}_{C_{N+1}}\end{array}\right)\text{\hspace{1em}(34)}$$
其中，
$$\exp (\delta {\mathbf{\theta }}_{{\mathbf{C}}_{\mathbf{N}+1}})\text{\hspace{1em}(35)}$$

$$\text{J}=\frac{\delta T_{C_{N+1}\leftarrow G}}{\delta \tilde{\text{X}}}\text{\hspace{1em}(36)}$$

更新后的状态协方差为：

$$\begin{gathered} {\text{P}}_{k|k}=\left(\begin{array}{c}{\text{I}}_{6N+21}\\ \text{J}\end{array}\right){\text{P}}_{k|k}{\left(\begin{array}{c}{\text{I}}_{6N+21}\\ \text{J}\end{array}\right)}^T \\ =\left(\begin{array}{cc}{\text{P}}_{k|k}& {\text{PJ}}^T\\ \text{PJ}& {\text{JPJ}}^T\end{array}\right) \end{gathered}$$

相机的误差状态与IMU的误差状态存在以下关系：

$$\delta {\mathbf{\theta }}_{C_{N+1}}={}_I^C\bar{\text{q}}\delta {\mathbf{\theta }}_{I_{N+1}}\text{\hspace{1em}(37)}$$

$${}^G{\tilde{\text{p}}}_{C_{N+1}}={}^G{\tilde{\text{p}}}_{I_{N+1}}+{}_G^{I_{N+1}}{\widehat{\bar{\text{q}}}}^T({}^I{\text{p}}_C{)}^{\wedge }\delta {\mathbf{\theta }}_{I_{N+1}}\text{\hspace{1em}(38)}$$

$$⟺{\text{J}}_{6\times (21+6N)}=\left(\begin{array}{cc}{\text{J}}_{I6\times 21}& {\text{0}}_{6\times 6N}\end{array}\right)$$

$$⟺{\text{J}}_I=\left(\begin{array}{ccccc}C{(}_I^C\bar{\text{q}})& {\text{0}}_{3\times 9}& {\text{0}}_{3\times 3}& {\text{I}}_3& {\text{0}}_{3\times 3}\\ C_{\hat{\text{q}}}^T({}^I{\text{p}}_C{)}^{\wedge }& {\text{0}}_{3\times 9}& {\text{I}}_3& {\text{0}}_{3\times 3}& {\text{I}}_3\end{array}\right)$$

此处${\text{J}}_I$跟论文里的推导有点区别，之后深究。
更新状态协方差矩阵后为了确保其对称，与他的转置求和，然后除以二:
$$\text{P}\leftarrow (\text{P}+{\text{P}}^T)/2$$

4.4 addFeatureObservations

添加新特征、跟踪特征。

4.5 removeLostFeatures

移除已经不再跟踪的特征点、进行measurement update
如果特征点仍然在跟踪，但是尚未初始化，就根据checkMotion和initializePosition来判断是否为无效点。

4.5.1 检测相机运动

bool Feature::checkMotion(const CamStateServer& cam_states);

checkMotion主要是check第一次观测到某特征点时的相机位置和最后一次观测到该特征点时的相机位置（当前相机位置）是否存在足够的平移，主要判断下图红色线段长度是否超过阈值。

如图所示，Cm是第一次观测到该特征点时的相机位姿，Cn是最后一次观测到该特征点的相机位姿，如果orthogonal_translation超过阈值，则返回真，否则返回假。

4.5.2 初始化特征点3D位置

bool Feature::initializePosition(const CamStateServer& cam_states);

如果checkMotion返回真，则对特征点位置进行初始化。
算法中把计算特征点的3D坐标构建成了一个最小二乘问题，利用Levenberg-Marquart算法进行求解。再进行优化之前，首先要得到一个初始估计(initial guess)。

void Feature::generateInitialGuess(
    const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
    const Eigen::Vector2d& z2, Eigen::Vector3d& p)

// Generate initial guess
  Eigen::Vector3d initial_position(0.0, 0.0, 0.0);
  generateInitialGuess(cam_poses[cam_poses.size()-1], measurements[0],
      measurements[measurements.size()-1], initial_position);
  Eigen::Vector3d solution(
      initial_position(0)/initial_position(2),
      initial_position(1)/initial_position(2),
      1.0/initial_position(2));

在计算初始估计时，取观测到该特征点的第一帧的左相机归一化坐标和观测到该特征点的最后一帧的右相机归一化坐标进行三角化来估计深度，通过足够大的平移来保证一个较高的精度。
接下来就是基于L-M算法的非线性优化，优化变量为:
$$\text{x}={\left(\begin{array}{ccc}\bar{x}& \bar{y}& \rho \end{array}\right)}^T$$
其中$\bar{x},\bar{y}$是特征点的归一化坐标，$\rho$是逆深度。假设该特征点在第j帧图像的观测值的归一化坐标为${\text{m}}_j$，第j帧相机位姿相对于第一次观测到该特征点的相机的相对位姿为$\text{R},\text{t}$，则重投影误差为：
$$\begin{gathered} \text{r}=\text{Rp}+\rho \text{t}-{\text{m}}_j \\ =\left(\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right)\left(\begin{array}{c}\bar{x}\\ \bar{y}\\ 1\end{array}\right)+\rho \left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)-\left(\begin{array}{c}{\bar{x}}_{m_j}\\ {\bar{y}}_{m_j}\\ 1\end{array}\right) \end{gathered}$$
这里只取$\text{r}$的前两维${\text{r}}_{2\times 1}={\left(\begin{array}{cc}{r}_x& r_y\end{array}\right)}^T$。=于是雅克比矩阵为：
$$\text{J}=\left(\begin{array}{ccc}\frac{d{r}_x}{d\bar{x}}& \frac{d{r}_x}{d\bar{y}}& \frac{d{r}_x}{d\rho }\\ \frac{d{r}_y}{d\bar{x}}& \frac{d{r}_y}{d\bar{y}}& \frac{d{r}_y}{d\rho }\end{array}\right)$$
其中，令
$${\text{p}}^{\prime }=\text{Rp}+\rho \text{t}={\left(\begin{array}{ccc}{\bar{x}}^{\prime }& {\bar{y}}^{\prime }& {\bar{z}}^{\prime }\end{array}\right)}^T$$
则：
$$\begin{gathered} \frac{d{r}_x}{d\bar{x}}=\frac{a_{11}{\bar{z}}^{\prime }-a_{31}{\bar{z}}^{\prime }}{{\bar{z}}^{\prime 2}},\frac{d{r}_x}{d\bar{y}}=\frac{a_{12}{\bar{z}}^{\prime }-a_{32}{\bar{x}}^{\prime }}{{\bar{z}}^{\prime 2}},\frac{d{r}_x}{d\rho }=\frac{t_x{\bar{z}}^{\prime 2}-t_z{\bar{x}}^{\prime }}{{\bar{z}}^{\prime 2}} \\ \frac{d{r}_y}{d\bar{x}}=\frac{a_{21}{\bar{z}}^{\prime }-{\bar{y}}^{\prime }{a}_{31}}{{\bar{z}}^{\prime 2}},\frac{d{r}_y}{d\bar{y}}=\frac{a_{22}{\bar{z}}^{\prime }-a_{32}{\bar{y}}^{\prime }}{{\bar{z}}^{\prime 2}},\frac{d{r}_y}{d\rho }=\frac{t_y{\bar{z}}^{\prime }-{\bar{y}}^{\prime }{t}_z}{{\bar{z}}^{\prime 2}} \end{gathered}$$
代码中对于雅可比的计算位于函数

void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
    const Eigen::Vector3d& x, const Eigen::Vector2d& z,
    Eigen::Matrix& J, Eigen::Vector2d& r,
    double& w) const

其中计算雅克比部分的代码为

// Compute the Jacobian.
Eigen::Matrix3d W;
W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
W.rightCols<1>() = T_c0_ci.translation();

J.row(0) = 1/h3*W.row(0) - h1/(h3*h3)*W.row(2);
J.row(1) = 1/h3*W.row(1) - h2/(h3*h3)*W.row(2);

把上式稍作变换，我们就能得到与代码一致的形式：
$$\begin{gathered} \left(\begin{array}{ccc}\frac{d{r}_x}{d\bar{x}}& \frac{d{r}_x}{d\bar{y}}& \frac{d{r}_x}{d\rho }\end{array}\right)=\frac{1}{{\bar{z}}^{\prime }}\left(\begin{array}{ccc}{a}_{11}& a_{12}& t_x\end{array}\right)-\frac{{\bar{x}}^{\prime }}{{\bar{z}}^{\prime 2}}\left(\begin{array}{ccc}{a}_{31}& a_{32}& t_z\end{array}\right) \\ \left(\begin{array}{ccc}\frac{d{r}_y}{d\bar{x}}& \frac{d{r}_y}{d\bar{y}}& \frac{d{r}_y}{d\rho }\end{array}\right)=\frac{1}{{\bar{z}}^{\prime }}\left(\begin{array}{ccc}{a}_{21}& a_{22}& t_y\end{array}\right)-\frac{{\bar{y}}^{\prime }}{{\bar{z}}^{\prime 2}}\left(\begin{array}{ccc}{a}_{31}& a_{32}& t_z\end{array}\right) \end{gathered}$$
然后根据误差来计算权值，如果误差小于一定值，则权值为1.0，若大于该值，根据huber核函数计算权值：

r = z_hat - z;
// Compute the weight based on the residual.
double e = r.norm();
if (e <= optimization_config.huber_epsilon)
  w = 1.0;
else
  w = optimization_config.huber_epsilon / (2*e);

然后基于L-M算法构建增量方程：
$$(\text{A}+\lambda \text{I})\delta \text{x}=\text{b}$$
$\lambda$的选择逻辑
代码中对于$\lambda$取值十分清晰，直接把代码搬上来：

if (new_cost < total_cost) 
{
  is_cost_reduced = true;
  solution = new_solution;
  total_cost = new_cost;
  lambda = lambda/10 > 1e-10 ? lambda/10 : 1e-10;
} 
else 
{
  is_cost_reduced = false;
  lambda = lambda*10 < 1e12 ? lambda*10 : 1e12;
}

其中$\lambda$的初始值是1e-3。

4.5.3 计算雅可比矩阵和残差向量

该过程在一个循环中计算与每个特征点相关的雅可比矩阵和残差向量，然后拼成一个大的雅可比矩阵和残差向量，用于下一步的观测更新(measurement update)。

// This function computes the Jacobian of all measurements viewed
// in the given camera states of this feature.
void featureJacobian(const FeatureIDType& feature_id,
                     const std::vector& cam_state_ids,
                     Eigen::MatrixXd& H_x, Eigen::VectorXd& r);

该函数放在特征点的循环中执行，每次计算两个雅可比矩阵${\text{H}}_{x_j},{\text{H}}_{f_i}$和残差向量${\text{r}}_j$，然后把雅可比矩阵和残差向量投影到${\text{H}}_{f_i}$的零空间。

A. 计算${\text{H}}_{x_j}、{\text{H}}_{f_j}$和${\text{r}}_j$
这个过程也是放在一个循环中进行，把单个小的雅克比矩阵拼成一个稍大的雅克比矩阵。计算单个小的雅克比矩阵的函数为MsckfVio::measurementJacobian。它计算的是观测误差关于特征点3D坐标和一对相机状态向量的雅克比矩阵。相机i对特征点j的观测残差为：
$${\text{r}}_i^j={\text{z}}_i^j-{\hat{\text{z}}}_i^j={\text{H}}_{C_i}^j{\tilde{\text{x}}}_{C_i}+{\text{H}}_{f_i}^j{}^G{\tilde{\text{p}}}_j+{\text{n}}_i^j$$
对应于论文的附录C中。即${\text{H}}_{C_i}^j$和${\text{H}}_{f_i}^j$。
根据链式法则：
$$\begin{gathered} {\text{H}}_{C_i}^j=\frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,1}}{\text{p}}_j}\cdot \frac{\partial {}^{C_{i,1}}{\text{p}}_j}{\partial {\text{x}}_{C_{i,1}}}+\frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,2}}{\text{p}}_j}\cdot \frac{\partial {}^{C_{i,2}}{\text{p}}_j}{\partial {\text{x}}_{C_{i,1}}} \\ {\text{H}}_{f_i}^j=\frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,1}}{\text{p}}_j}\cdot \frac{\partial {}^{C_{i,1}}{\text{p}}_j}{\partial {}^G{\text{p}}_j}+\frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,2}}{\text{p}}_j}\cdot \frac{\partial {}^{C_{i,2}}{\text{p}}_j}{\partial {}^G{\text{p}}_j} \end{gathered}$$
其中，
$$\begin{gathered} \frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,1}}{\text{p}}_j}=\frac{1}{{}^{C_{i,1}}{\hat{Z}}_j}\left(\begin{array}{ccc}1& 0& -\frac{{}^{C_{i,1}}{\hat{X}}_j}{{}^{C_{i,1}}{\hat{Z}}_j}\\ 0& 1& -\frac{{}^{C_{i,1}}{\hat{Y}}_j}{{}^{C_{i,1}}{\hat{Z}}_j}\\ 0& 0& 0\\ 0& 0& 0\end{array}\right) \\ \frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,2}}{\text{p}}_j}=\frac{1}{{}^{C_{i,2}}{\hat{Z}}_j}\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 1& 0& -\frac{{}^{C_{i,2}}{\hat{X}}_j}{{}^{C_{i,1}}{\hat{Z}}_j}\\ 0& 1& -\frac{{}^{C_{i,2}}{\hat{Y}}_j}{{}^{C_{i,1}}{\hat{Z}}_j}\end{array}\right) \\ \frac{\partial {}^{C_{i,1}}{\text{p}}_j}{\partial {\text{x}}_{C_{i,1}}}=\left(\begin{array}{cc}\lfloor {}^{C_{i,1}}{\hat{\text{p}}}_{j\times }\rfloor & -C({}_G^{C_{i,1}}\hat{\text{q}}\end{array}\right) \\ \frac{\partial {}^{C_{i,1}}{\text{p}}_j}{\partial {}^G{\text{p}}_j}=C({}_G^{C_{i,1}}\hat{\text{q}}) \\ \frac{\partial {}^{C_{i,2}}{\text{p}}_j}{\partial {\text{x}}_{C_{i,1}}}=C({}_{C_{i,2}}^{C_{i,1}}\text{q}{)}^{\top }\left(\begin{array}{cc}\lfloor {}^{C_{i,1}}{\hat{\text{p}}}_{j\times }\rfloor & -C({}_G^{C_{i,1}}\hat{\text{q}})\end{array}\right) \\ \frac{\partial {}^{C_{i,2}}{\text{p}}_j}{\partial {}^G{\text{p}}_j}=C({}_{C_{i,2}}^{C_{i,1}}\text{q}{)}^{\top }C({}_G^{C_{i,1}}\text{q}) \end{gathered}$$

在代码里，为了保证观测约束，对雅可比进行了一定的调整：
这步目前还没整明白

// Modifty the measurement Jacobian to ensure
// observability constrain.
Matrix A = H_x;
Matrix u = Matrix::Zero();
u.block<3, 1>(0, 0) = quaternionToRotation(
    cam_state.orientation_null) * IMUState::gravity;
u.block<3, 1>(3, 0) = skewSymmetric(
    p_w-cam_state.position_null) * IMUState::gravity;
H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
H_f = -H_x.block<4, 3>(0, 3);

// Compute the residual.
r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
    p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));

通过把对同一个特征点的观测叠加起来，可以得到特征点j的雅可比矩阵：
$${\text{r}}^j={\text{H}}_{\text{x}}^j\tilde{\text{x}}+{\text{H}}_f^j{}^G{\tilde{\text{p}}}_j+{\text{n}}^j$$

B. 把雅可比和残差向量投影到${\text{H}}_f^i$的零空间
由于当前形式的${\text{r}}^j$无法进行标准EKF更新，需要把它投影到${\text{H}}_f^i$的零空间,得到
$${\text{r}}_0^j={\text{V}}^{\top }{\text{r}}^j={\text{V}}^{\top }{\text{H}}_{\text{x}}^j\tilde{\text{x}}+{\text{V}}^{\top }{\text{n}}^j={\text{H}}_{\text{x},0}^j\tilde{\text{x}}+{\text{n}}_0^j$$
${\text{V}}^{\top }$主要通过对${\text{H}}_f^i$进行SVD分解得到：

// Project the residual and Jacobians onto the nullspace
// of H_fj.
JacobiSVD svd_helper(H_fj, ComputeFullU | ComputeThinV);
MatrixXd A = svd_helper.matrixU().rightCols(
    jacobian_row_size - 3);

H_x = A.transpose() * H_xj;
r = A.transpose() * r_j;

那么这里有个问题，如果对单个双目观测的误差关于状态向量的雅可比进行投影，计算量更小，为什么不这样做呢？
论文的附录D向我们解释了原因，为了方便法表示，记以下符号为：
$$\begin{gathered} \frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,1}}{\text{p}}_j}=\left(\begin{array}{c}{\text{J}}_1\\ \text{0}\end{array}\right),\frac{\partial {\text{z}}_i^j}{\partial {}^{C_{i,1}}{\text{p}}_j}=\left(\begin{array}{c}\text{0}\\ {\text{J}}_2\end{array}\right) \\ \frac{\partial {}^{C_{i,1}}{\text{p}}_j}{\partial {\text{x}}_{C_{i,1}}}=\left(\begin{array}{c}{\text{H}}_1\end{array}\right),\frac{\partial {}^{C_{i,1}}{\text{p}}_j}{\partial {}^G{\text{p}}_j}={\text{H}}_2,C({}_{C_{i,2}}^{C_{i,1}}\text{q})=\text{R} \end{gathered}$$
于是有
$${\text{H}}_{C_i}^j=\left(\begin{array}{c}{\text{J}}_1{\text{H}}_1\\ {\text{J}}_2{\text{R}}^{\top }{\text{H}}_1\end{array}\right),{\text{H}}_{f_i}^j=\left(\begin{array}{c}{\text{j}}_1{\text{H}}_2\\ {\text{J}}_2{\text{R}}^{\top }{\text{H}}_2\end{array}\right)$$
设$\text{v}=({\text{v}}_1^{\top },{\text{v}}_2^{\top }{)}^{\top }\in {\mathbb{R}}^4$是${\text{H}}_{f_i}^j$的左零空间，于是有
$${\text{v}}^{\top }{\text{H}}_{f_i}^j=({\text{v}}_1^{\top }{\text{J}}_1+{\text{v}}_2^{\top }{\text{J}}_2{\text{R}}^{\top }){\text{H}}_2=\text{0}$$
由于${\text{H}}_2=C({}_G^{C_{i,1}}\hat{\text{q}})$是一个旋转矩阵，它的秩等于3，因此${\text{v}}_1^{\top }{\text{J}}_1+{\text{v}}_2^{\top }{\text{J}}_2{\text{R}}^{\top }=0$。有这个性质，就可以直接推出$\text{v}$也是${\text{H}}_{C_i}^j$的左零空间：
$${\text{v}}^{\top }{\text{H}}_{C_i}^j=({\text{v}}_1^{\top }{\text{J}}_1+{\text{v}}_2^{\top }{\text{J}}_2{\text{R}}^{\top }){\text{H}}_1=0$$
所以单独的一个双目观测不能用于EKF更新。

4.5.4 EKF更新

在把上一步得到的雅可比矩阵${\text{H}}_{\text{x},0}^j$和残差向量叠${\text{r}}_0^j$叠加起来后，就得到了一个大的雅可比矩阵${\text{H}}_{x,0}$和残差向量${\text{r}}_0$，为了与代码中的变量保持一致，记为${\text{H}}_x$和$\text{r}$。
对应算法位于

void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r)

由于${\text{H}}_x$的维数较高，为了降低EKF更新的计算量，对${\text{H}}_x$进行QR分解：
$${\text{H}}_x=\left(\begin{array}{cc}{\text{Q}}_1& {\text{Q}}_2\end{array}\right)\left(\begin{array}{c}{\text{T}}_H\\ \text{0}\end{array}\right)$$
其中${\text{Q}}_1,{\text{Q}}_2$是单位正交列向量组，${\text{T}}_H$是上三角矩阵，于是
$$\begin{gathered} \text{r}=\left(\begin{array}{cc}{\text{Q}}_1& {\text{Q}}_2\end{array}\right)\left(\begin{array}{c}{\text{T}}_H\\ \text{0}\end{array}\right)\tilde{\text{x}}+\text{n} \\ \Rightarrow \left(\begin{array}{c}{\text{Q}}_1^{\top }\text{r}\\ {\text{Q}}_2^{\top }\text{r}\end{array}\right)=\left(\begin{array}{c}{\text{T}}_H\\ \text{0}\end{array}\right)\tilde{\text{x}}+\left(\begin{array}{c}{\text{Q}}_1^{\top }\text{n}\\ {\text{Q}}_2^{\top }\text{n}\end{array}\right) \end{gathered}$$
从上式可以很清楚的看出，通过把残差投影到${\text{T}}_H$的基向量组${\text{Q}}_1$上，可以保留观测的全部有效信息，残差项${\text{Q}}_2^{\top }\text{r}$仅仅是噪声，完全可以忽略，于是可以通过以下残差来进行EKF更新：
$${\text{r}}^{\prime }={\text{Q}}_1^{\top }\text{r}={\text{T}}_H\tilde{\text{x}}+{\text{n}}^{\prime }$$
计算卡尔曼增益：
$$\text{K}=\text{P}{\text{T}}_H^{\top }({\text{T}}_H\text{P}{\text{T}}_H^{\top }+\text{R}{)}^{-1}$$
于是状态向量的修正增量为：
$$\Delta \text{x}=\text{K}\text{r}$$
然后更新协方差：
$${\text{P}}_{k+1|k+1}=({\text{I}}_{\xi }-{\text{KT}}_H){\text{P}}_{k+1|k}({\text{I}}_{\xi }-{\text{KT}}_H{)}^{\top }+{\text{KRK}}^{\top }$$
trick: 代码并没有严格按照公式来，而是做了一些调整

// Update state covariance.
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
//state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
//  K*K.transpose()*Feature::observation_noise;
state_server.state_cov = I_KH*state_server.state_cov;

// Fix the covariance to be symmetric
MatrixXd state_cov_fixed = (state_server.state_cov +
    state_server.state_cov.transpose()) / 2.0;
state_server.state_cov = state_cov_fixed;

4.6 删除冗余相机位姿

对应算法位于MsckfVio::pruneCamStateBuffer()。

4.6.1 查找冗余相机位姿

对应代码位于MsckfVio::findRedundantCamStates。冗余相机位姿为与关键相机位姿相差小的位姿或者相机状态向量中靠前的位姿。把冗余相机位姿的id放在一个vector中并进行排序，输出。

4.6.2 关于冗余相机位姿的测量更新

如果查找到2个冗余相机位姿，则遍历这2个位姿产生共视的所有特征点构建特征雅可比矩阵，删除冗余相机位姿的观测，关于这些共视点和2个相机位姿进行EKF更新。
最后从相机状态向量里删除冗余相机位姿，删除状态协方差里关于冗余相机位姿的矩阵块。

4.7 发布里程计

位姿发布给ros进行显示。