s-msckf代码笔记（二）

二、VIO

VIO中IMU的状态向量为：
xI=(GIqTbgTGvITbaTGpITCIqTIpCT)T\textbf x_I=\begin{pmatrix} {}_G^I\textbf q^T & \textbf b_g^T & {}^G\textbf v_I^T & \textbf b_a^T & {}^G\textbf p_I^T & {}_C^I\textbf q^T & {}^I\textbf p_C^T \end{pmatrix}^T xI=(GIqTbgTGvITbaTGpITCIqTIpCT)T
误差状态为：
x~I=(GIθ~Tb~gTGv~Tb~aTGp~ITCIθ~TIp~CT)T\tilde{\textbf x}_I=\begin{pmatrix} {}_G^I\tilde\boldsymbol{\theta}^T & \tilde\boldsymbol{b}_g^T & {}^G\tilde\textbf{v}^T & \tilde\textbf{b}_a^T & {}^G\tilde\textbf{p}_I^T & {}_C^I\tilde\boldsymbol{\theta}^T & {}^I\tilde\textbf p_C^T \end{pmatrix}^T x~I=(GIθ~Tb~gTGv~Tb~aTGp~ITCIθ~TIp~CT)T
对于普通的变量，如bg,vI\textbf b_g,\textbf v_Ibg,vI等，满足通用的加减关系，比如Gp~I=GpI−Gp^I{}^G\tilde\textbf p_I={}^G\textbf p_I-{}^G\hat\textbf p_IGp~I=GpI−Gp^I。对于四元数，误差四元数为：
δq=q⊗q^−1≈(12IGθ~T1)T\delta\textbf q=\textbf q\otimes\hat\textbf q^{-1}\\ \approx\begin{pmatrix} \frac{1}{2}{}^G_I\tilde\boldsymbol \theta^T & 1 \end{pmatrix}^T δq=q⊗q^−1≈(21IGθ~T1)T
其中IGθ~T∈R3{}^G_I\tilde\boldsymbol\theta^T\in\mathbb R^3IGθ~T∈R3表示一个微小的旋转，通过这种方式将旋转误差降到了三维。考虑最终的误差状态向量，设有N个相机状态，则最终的误差状态向量为：
x~=(x~ITx~C1…x~CN)T\tilde\textbf x=\begin{pmatrix} \tilde\textbf x_I^T & \tilde\textbf x_{C_1} & \dots & \tilde\textbf x_{C_N} \end{pmatrix}^T x~=(x~ITx~C1…x~CN)T

x~Ci=(GCiθ~TGp~CiT)T\tilde\textbf x_{C_i}=\begin{pmatrix} {}_G^{C_i}\tilde\boldsymbol\theta^T & {}^G\tilde\textbf p_{C_i}^T \end{pmatrix}^T x~Ci=(GCiθ~TGp~CiT)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轴负方向。然后定义惯性系相对于世界坐标系的朝向，这里我看的有点懵，他先求解了一个旋转R\textbf RR,使得：
RgI=−gw\textbf R\textbf g^I=-\textbf g^w RgI=−gw
然后把初始时刻的旋转qiw\textbf q_{iw}qiw定义为:
qiw=quaternion(RT)\textbf q_{iw}=quaternion(R^T) qiw=quaternion(RT)
这一步目前还不是很懂

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

该函数用于构建F\textbf FF矩阵、G\textbf GG矩阵和Φ\boldsymbol\PhiΦ矩阵，更新状态协方差P\textbf PP和状态变量X\textbf XX，其中P\textbf PP存放在state_server.state_cov里。

4.2.1.1 构建F\textbf FF、G\textbf GG和Φ\boldsymbol\PhiΦ

F21×21=(−ω^×−I303×303×303×303×303×303×303×303×3−C(GIq^)Ta^×03×303×3−C(GIq^)T03×303×303×303×303×303×303×303×3I303×303×303×303×303×303×303×303×303×303×303×303×3)\textbf F_{21\times 21}= \begin{pmatrix} -\hat{\boldsymbol\omega}_{\times} & -\textbf I_3 & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ -C({}_G^I\hat{\textbf{q}})^T\hat{\textbf a}_{\times} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & -C({}_G^I\hat{\textbf{q}})^T & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf I_3 & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} \end{pmatrix} F21×21=⎝⎜⎜⎜⎜⎜⎜⎜⎜⎛−ω^×03×3−C(GIq^)Ta^×03×303×303×303×3−I303×303×303×303×303×303×303×303×303×303×3I303×303×303×303×3−C(GIq^)T03×303×303×303×303×303×303×303×303×303×303×3⎠⎟⎟⎟⎟⎟⎟⎟⎟⎞

G21×12=(−I303×303×303×303×3−I303×303×303×303×3−C(GIq^)T03×303×303×303×3I303×303×303×303×303×303×303×303×3)\textbf G_{21\times 12}= \begin{pmatrix} -\textbf I_3 & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & -\textbf I_3 & \textbf 0_{3\times 3} & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & -C({}_G^I\hat{\textbf{q}})^T & \textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf I_3\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} &\textbf 0_{3\times 3}\\ \textbf 0_{3\times 3} & \textbf 0_{3\times 3} & \textbf 0_{3\times 3} &\textbf 0_{3\times 3} \end{pmatrix} G21×12=⎝⎜⎜⎜⎜⎜⎜⎛−I303×303×303×303×303×303×3−I303×303×303×303×303×303×3−C(GIq^)T03×303×303×303×303×303×3I303×303×3⎠⎟⎟⎟⎟⎟⎟⎞

其中
Φ=exp⁡(∫tktk+1F(τ)dτ)\boldsymbol\Phi = \exp(\int_{t_k}^{t_{k+1}}\textbf F(\tau)d\tau) Φ=exp(∫tktk+1F(τ)dτ)
这里采用3阶泰勒展开来取近似
Φ≈I+Fdt+12(Fdt)2+16(Fdt)3\boldsymbol\Phi\approx\textbf I+\textbf Fd_t+\frac{1}{2}(\textbf Fd_t)^2+\frac{1}{6}(\textbf Fd_t)^3 Φ≈I+Fdt+21(Fdt)2+61(Fdt)3

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

RK4积分一般形式
xn+1=xn+Δt6(k1+2k2+2k3+k4)\textbf x_{n+1}=\textbf x_{n}+\frac{\Delta t}{6}(k_1+2k_2+2k_3+k_4) xn+1=xn+6Δt(k1+2k2+2k3+k4)

k1=f(tn,xn)k2=f(tn+12Δt,xn+Δt2k1)k3=f(tn+12Δt,xn+Δt2k2)k4=f(tn+Δt,xn+Δt⋅k3)k_1=f(t_n,\textbf x_n)\\ k_2=f(t_n+\frac{1}{2}\Delta t,\textbf x_n + \frac{\Delta t}{2}k_1)\\ k_3=f(t_n+\frac{1}{2}\Delta t,\textbf x_n+\frac{\Delta t}{2}k_2)\\ k_4=f(t_n+\Delta t,\textbf x_n+\Delta t\cdot k_3) k1=f(tn,xn)k2=f(tn+21Δt,xn+2Δtk1)k3=f(tn+21Δt,xn+2Δtk2)k4=f(tn+Δt,xn+Δt⋅k3)

这一步要对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

首先计算角速度的二范数∥ω∥2\Vert\boldsymbol{\omega}\Vert_2∥ω∥2，然后乘以时间间隔得到旋转角度ϕ\phiϕ。
ϕ=∥ω∥2⋅Δt\phi=\Vert\boldsymbol{\omega}\Vert_2\cdot\Delta t ϕ=∥ω∥2⋅Δt
于是ϕ\phiϕ对应的四元数为
q=(cos⁡(ϕ/2)usin⁡(ϕ/2))=(qwqv)\textbf q= \begin{pmatrix} \cos(\phi/2) \\ \textbf u\sin(\phi/2) \\ \end{pmatrix}= \begin{pmatrix} q_w \\ \textbf q_v \\ \end{pmatrix} q=(cos(ϕ/2)usin(ϕ/2))=(qwqv)

其中u\textbf uu为旋转轴的单位向量。于是更新后的四元数为：
qk+1=q⊗qk=(qwI+(−⌊ω×⌋ω−ωT0))qk=(qwI+Ω(ω))qk\textbf q_{k+1}=\textbf q\otimes\textbf q_k\\ =(q_w\textbf I+\begin{pmatrix} -\left\lfloor\boldsymbol{\omega}\times\right\rfloor & \boldsymbol{\omega}\\ -\boldsymbol{\omega}^T & 0 \end{pmatrix})\textbf q_k\\ =(q_w\textbf I+\boldsymbol{\Omega}(\boldsymbol{\omega}))\textbf q_k qk+1=q⊗qk=(qwI+(−⌊ω×⌋−ωTω0))qk=(qwI+Ω(ω))qk

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

V:
k1v=qk−1⊗ak+gwk2v=d2∗ak+gwk3v=d2∗ak+gwk4v=d1∗ak+gwk_1^v=\textbf q_k^{-1}\otimes\textbf a_k+\textbf g^w\\ k_2^v=d2*\textbf a_k+\textbf g^w\\ k_3^v=d2*\textbf a_k+\textbf g^w\\ k_4^v=d1*\textbf a_k+\textbf g^w k1v=qk−1⊗ak+gwk2v=d2∗ak+gwk3v=d2∗ak+gwk4v=d1∗ak+gw
P:
k1p=vkk2p=vk+12k1vΔtk3p=vk+12k2vΔtk4p=vk+k3vΔtk_1^p=\textbf v_k\\ k_2^p=\textbf v_k+\frac{1}{2}k_1^v\Delta t\\ k_3^p=\textbf v_k+\frac{1}{2}k_2^v\Delta t\\ k_4^p=\textbf v_k+k_3^v\Delta t k1p=vkk2p=vk+21k1vΔtk3p=vk+21k2vΔtk4p=vk+k3vΔt
然后把上面的系数代入RK4积分公式就得到了vk+1,pk+1\textbf v_{k+1},\textbf p_{k+1}vk+1,pk+1，然后更新state_server.imu_state。

4.2.1.3 更新转移矩阵Φ\boldsymbol{\Phi}Φ和协方差矩阵P\textbf{P}P

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

b.P\textbf{P}P的更新
PIIk+1∣k=ΦkPIIk∣kΦkT+Qk\textbf{P}_{II_{k+1|k}}=\boldsymbol{\Phi}_k\textbf{P}_{II_{k|k}}\boldsymbol{\Phi}_k^T+\textbf Q_k PIIk+1∣k=ΦkPIIk∣kΦkT+Qk
其中
Qk=∫tktk+1Φ(tk+1,τ)GQGΦ(tk+1,τ)Tdτ\textbf Q_k = \int_{t_k}^{t_{k+1}}\boldsymbol{\Phi}(t_{k+1},\tau)\textbf{GQG}\boldsymbol{\Phi}(t_{k+1},\tau)^Td\tau Qk=∫tktk+1Φ(tk+1,τ)GQGΦ(tk+1,τ)Tdτ
代码中对Qk\textbf Q_kQk取近似：
Qk≈ΦkGQGΦkTΔt\textbf Q_k\approx\boldsymbol\Phi_{k}\textbf{GQG}\boldsymbol\Phi_{k}^T\Delta t Qk≈ΦkGQGΦkTΔt

Matrix<double, 21, 21> 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和相机之间的外参给相机状态（相对于世界坐标系的平移、旋转）赋值。
(32)GCqˉ^=ICqˉ⊗IGqˉ^{}_G^C\hat{\bar{\textbf q}}={}_I^C\bar{\textbf q}\otimes{}_I^G\hat{\bar{\textbf q}} \tag{32} GCqˉ^=ICqˉ⊗IGqˉ^(32)

(33)Gp^C=Gp^I+Cq^TIpC{}^G\hat{\textbf p}_C={}^G\hat{\textbf p}_I+C_{\hat{\textbf q}}^T{}^I\textbf p_C \tag{33} Gp^C=Gp^I+Cq^TIpC(33)

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

a中得到了一个新的相机位姿，该相机位姿添加到状态向量中，对应的状态协方差矩阵也要进行相应的更新（augmented)，设该相机位姿为第N+1个相机位姿，其对应的误差状态为：
(34)δTCN+1←G=(δθCN+1←GGp~CN+1)\delta T_{C_{N+1}\leftarrow G}=\begin{pmatrix} \delta\boldsymbol{\theta}_{C_{N+1}\leftarrow G} \\ {}^G\tilde{\textbf p}_{C_{N+1}} \end{pmatrix} \tag{34} δTCN+1←G=(δθCN+1←GGp~CN+1)(34)
其中，
(35)exp⁡(δθCN+1)\exp(\delta\boldsymbol{\theta_{C_{N+1}}}) \tag{35} exp(δθCN+1)(35)

(36)J=δTCN+1←GδX~\textbf J=\frac{\delta T_{C_{N+1}\leftarrow G}}{\delta\tilde{\textbf X}} \tag{36} J=δX~δTCN+1←G(36)

更新后的状态协方差为：

Pk∣k=(I6N+21J)Pk∣k(I6N+21J)T=(Pk∣kPJTPJJPJT)\textbf P_{k|k} = \begin{pmatrix} \textbf I_{6N+21} \\ \textbf J \end{pmatrix} \textbf P_{k|k} \begin{pmatrix} \textbf I_{6N+21} \\ \textbf J \end{pmatrix}^T\\ =\begin{pmatrix} \textbf P_{k|k} & \textbf{PJ}^T\\ \textbf{PJ} & \textbf{JPJ}^T \end{pmatrix} Pk∣k=(I6N+21J)Pk∣k(I6N+21J)T=(Pk∣kPJPJTJPJT)

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

(37)δθCN+1=ICqˉδθIN+1\delta\boldsymbol{\theta}_{C_{N+1}} = {}_I^C\bar\textbf{q}\delta\boldsymbol\theta_{I_{N+1}} \tag{37} δθCN+1=ICqˉδθIN+1(37)

(38)Gp~CN+1=Gp~IN+1+GIN+1qˉ^T(IpC)∧δθIN+1{}^G\tilde\textbf p_{C_{N+1}}={}^G\tilde\textbf p_{I_{N+1}}+{}_{G}^{I_{N+1}}\hat{\bar{\textbf q}}^T({}^I\textbf p_C)^{\land}\delta\boldsymbol\theta_{I_{N+1}} \tag{38} Gp~CN+1=Gp~IN+1+GIN+1qˉ^T(IpC)∧δθIN+1(38)

 ⟺ J6×(21+6N)=(JI6×2106×6N)\iff\textbf J_{6\times(21+6N)}=\begin{pmatrix} \textbf J_{I6\times 21} & \textbf 0_{6\times 6N} \end{pmatrix} ⟺J6×(21+6N)=(JI6×2106×6N)

 ⟺ JI=(C(ICqˉ)03×903×3I303×3Cq^T(IpC)∧03×9I303×3I3)\iff\textbf J_I=\begin{pmatrix} C(_I^C\bar\textbf q) & \textbf 0_{3\times 9} & \textbf 0_{3\times 3} & \textbf I_3 & \textbf 0_{3\times 3}\\ C_{\hat\textbf q}^T({}^I\textbf p_C)^\land & \textbf 0_{3\times 9} & \textbf I_3 & \textbf 0_{3\times 3} & \textbf I_3 \end{pmatrix} ⟺JI=(C(ICqˉ)Cq^T(IpC)∧03×903×903×3I3I303×303×3I3)

此处JI\textbf J_IJI跟论文里的推导有点区别，之后深究。
更新状态协方差矩阵后为了确保其对称，与他的转置求和，然后除以二:
P←(P+PT)/2\textbf P\leftarrow(\textbf P+\textbf P^T)/2 P←(P+PT)/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 guessEigen::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算法的非线性优化，优化变量为:
x=(xˉyˉρ)T\textbf x = \begin{pmatrix} \bar x & \bar y & \rho \end{pmatrix}^T x=(xˉyˉρ)T
其中xˉ,yˉ\bar x, \bar yxˉ,yˉ是特征点的归一化坐标，ρ\rhoρ是逆深度。假设该特征点在第j帧图像的观测值的归一化坐标为mj\textbf m_jmj，第j帧相机位姿相对于第一次观测到该特征点的相机的相对位姿为R,t\textbf R,\textbf tR,t，则重投影误差为：
r=Rp+ρt−mj=(a11a12a13a21a22a23a31a32a33)(xˉyˉ1)+ρ(txtytz)−(xˉmjyˉmj1)\textbf r = \textbf{Rp}+\rho\textbf t - \textbf m_j\\ =\begin{pmatrix} a_{11} & a_{12} & a_{13}\\ a_{21} & a_{22} & a_{23}\\ a_{31} & a_{32} & a_{33} \end{pmatrix} \begin{pmatrix} \bar x\\ \bar y\\ 1 \end{pmatrix}+\rho \begin{pmatrix} t_x\\ t_y\\ t_z \end{pmatrix}- \begin{pmatrix} \bar x_{m_j}\\ \bar y_{m_j}\\ 1 \end{pmatrix} r=Rp+ρt−mj=⎝⎛a11a21a31a12a22a32a13a23a33⎠⎞⎝⎛xˉyˉ1⎠⎞+ρ⎝⎛txtytz⎠⎞−⎝⎛xˉmjyˉmj1⎠⎞
这里只取r\textbf rr的前两维r2×1=(rxry)T\textbf r_{2\times 1}=\begin{pmatrix}r_x & r_y\end{pmatrix}^Tr2×1=(rxry)T。=于是雅克比矩阵为：
J=(drxdxˉdrxdyˉdrxdρdrydxˉdrydyˉdrydρ)\textbf J = \begin{pmatrix} \frac{dr_x}{d\bar x} & \frac{dr_x}{d\bar y} & \frac{dr_x}{d\rho}\\ \frac{dr_y}{d\bar x} & \frac{dr_y}{d\bar y} & \frac{dr_y}{d\rho} \end{pmatrix} J=(dxˉdrxdxˉdrydyˉdrxdyˉdrydρdrxdρdry)
其中，令
p′=Rp+ρt=(xˉ′yˉ′zˉ′)T\textbf p' = \textbf{Rp}+\rho\textbf t =\begin{pmatrix} \bar x' & \bar y' & \bar z' \end{pmatrix}^T p′=Rp+ρt=(xˉ′yˉ′zˉ′)T
则：
drxdxˉ=a11zˉ′−a31zˉ′zˉ′2,drxdyˉ=a12zˉ′−a32xˉ′zˉ′2,drxdρ=txzˉ′2−tzxˉ′zˉ′2drydxˉ=a21zˉ′−yˉ′a31zˉ′2,drydyˉ=a22zˉ′−a32yˉ′zˉ′2,drydρ=tyzˉ′−yˉ′tzzˉ′2\frac{dr_x}{d\bar x}=\frac{a_{11}\bar z'-a_{31}\bar z'}{\bar z'^2},\quad \frac{dr_x}{d\bar y}=\frac{a_{12}\bar z'-a_{32}\bar x'}{\bar z'^2},\quad \frac{dr_x}{d\rho}=\frac{t_x\bar z'^2-t_z\bar x'}{\bar z'^2}\\ \frac{dr_y}{d\bar x}=\frac{a_{21}\bar z'-\bar y'a_{31}}{\bar z'^2},\quad \frac{dr_y}{d\bar y}=\frac{a_{22}\bar z'-a_{32}\bar y'}{\bar z'^2},\quad \frac{dr_y}{d\rho}=\frac{t_y\bar z'-\bar y't_z}{\bar z'^2} dxˉdrx=zˉ′2a11zˉ′−a31zˉ′,dyˉdrx=zˉ′2a12zˉ′−a32xˉ′,dρdrx=zˉ′2txzˉ′2−tzxˉ′dxˉdry=zˉ′2a21zˉ′−yˉ′a31,dyˉdry=zˉ′2a22zˉ′−a32yˉ′,dρdry=zˉ′2tyzˉ′−yˉ′tz
代码中对于雅可比的计算位于函数

void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,const Eigen::Vector3d& x, const Eigen::Vector2d& z,Eigen::Matrix<double, 2, 3>& 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);

把上式稍作变换，我们就能得到与代码一致的形式：
(drxdxˉdrxdyˉdrxdρ)=1zˉ′(a11a12tx)−xˉ′zˉ′2(a31a32tz)(drydxˉdrydyˉdrydρ)=1zˉ′(a21a22ty)−yˉ′zˉ′2(a31a32tz)\begin{pmatrix} \frac{dr_x}{d\bar x} & \frac{dr_x}{d\bar y} & \frac{dr_x}{d\rho} \end{pmatrix}=\frac{1}{\bar z'}\begin{pmatrix} a_{11} & a_{12} & t_x \end{pmatrix}- \frac{\bar x'}{\bar z'^2}\begin{pmatrix} a_{31} & a_{32} & t_z \end{pmatrix}\\ \begin{pmatrix} \frac{dr_y}{d\bar x} & \frac{dr_y}{d\bar y} & \frac{dr_y}{d\rho} \end{pmatrix}= \frac{1}{\bar z'}\begin{pmatrix} a_{21} & a_{22} & t_y \end{pmatrix}- \frac{\bar y'}{\bar z'^2}\begin{pmatrix} a_{31} & a_{32} & t_z \end{pmatrix} (dxˉdrxdyˉdrxdρdrx)=zˉ′1(a11a12tx)−zˉ′2xˉ′(a31a32tz)(dxˉdrydyˉdrydρdry)=zˉ′1(a21a22ty)−zˉ′2yˉ′(a31a32tz)
然后根据误差来计算权值，如果误差小于一定值，则权值为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;
elsew = optimization_config.huber_epsilon / (2*e);

然后基于L-M算法构建增量方程：
(A+λI)δx=b(\textbf A+\lambda\textbf I)\delta\textbf x=\textbf b (A+λI)δx=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<StateIDType>& cam_state_ids,Eigen::MatrixXd& H_x, Eigen::VectorXd& r);

该函数放在特征点的循环中执行，每次计算两个雅可比矩阵Hxj,Hfi\textbf H_{x_j}, \textbf H_{f_i}Hxj,Hfi和残差向量rj\textbf r_jrj，然后把雅可比矩阵和残差向量投影到Hfi\textbf H_{f_i}Hfi的零空间。

A. 计算Hxj、Hfj\textbf H_{x_j}、\textbf H_{f_j}Hxj、Hfj和rj\textbf r_jrj
这个过程也是放在一个循环中进行，把单个小的雅克比矩阵拼成一个稍大的雅克比矩阵。计算单个小的雅克比矩阵的函数为MsckfVio::measurementJacobian。它计算的是观测误差关于特征点3D坐标和一对相机状态向量的雅克比矩阵。相机i对特征点j的观测残差为：
rij=zij−z^ij=HCijx~Ci+HfijGp~j+nij\textbf r_i^j = \textbf z_i^j - \hat\textbf z_i^j = \textbf H_{C_i}^j\tilde\textbf x_{C_i} + \textbf H_{f_i}^j{}^G\tilde\textbf p_j + \textbf n_i^j rij=zij−z^ij=HCijx~Ci+HfijGp~j+nij
对应于论文的附录C中。即HCij\textbf H_{C_i}^jHCij和Hfij\textbf H_{f_i}^jHfij。
根据链式法则：
HCij=∂zij∂Ci,1pj⋅∂Ci,1pj∂xCi,1+∂zij∂Ci,2pj⋅∂Ci,2pj∂xCi,1Hfij=∂zij∂Ci,1pj⋅∂Ci,1pj∂Gpj+∂zij∂Ci,2pj⋅∂Ci,2pj∂Gpj\textbf H_{C_i}^j = \frac{\partial\textbf z_i^j}{\partial{}^{C_{i,1}}\textbf p_j}\cdot\frac{\partial{}^{C_{i,1}}\textbf p_j}{\partial\textbf x_{C_{i,1}}} + \frac{\partial\textbf z_i^j}{\partial{}^{C_{i,2}}\textbf p_j}\cdot\frac{\partial{}^{C_{i,2}}\textbf p_j}{\partial\textbf x_{C_{i,1}}}\\\\ \textbf H_{f_i}^j = \frac{\partial\textbf z_i^j}{\partial{}^{C_{i,1}}\textbf p_j}\cdot\frac{\partial{}^{C_{i,1}}\textbf p_j}{\partial{}^G\textbf p_j} + \frac{\partial\textbf z_i^j}{\partial{}^{C_{i,2}}\textbf p_j}\cdot\frac{\partial{}^{C_{i,2}}\textbf p_j}{\partial{}^G\textbf p_j} HCij=∂Ci,1pj∂zij⋅∂xCi,1∂Ci,1pj+∂Ci,2pj∂zij⋅∂xCi,1∂Ci,2pjHfij=∂Ci,1pj∂zij⋅∂Gpj∂Ci,1pj+∂Ci,2pj∂zij⋅∂Gpj∂Ci,2pj
其中，
∂zij∂Ci,1pj=1Ci,1Z^j(10−Ci,1X^jCi,1Z^j01−Ci,1Y^jCi,1Z^j000000)∂zij∂Ci,2pj=1Ci,2Z^j(00000010−Ci,2X^jCi,1Z^j01−Ci,2Y^jCi,1Z^j)∂Ci,1pj∂xCi,1=(⌊Ci,1p^j×⌋−C(GCi,1q^)∂Ci,1pj∂Gpj=C(GCi,1q^)∂Ci,2pj∂xCi,1=C(Ci,2Ci,1q)⊤(⌊Ci,1p^j×⌋−C(GCi,1q^))∂Ci,2pj∂Gpj=C(Ci,2Ci,1q)⊤C(GCi,1q)\frac{\partial\textbf z_i^j}{\partial{}^{C_{i,1}}\textbf p_j} = \frac{1}{{}^{C_{i,1}}\hat Z_j}\begin{pmatrix} 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{pmatrix}\\\\ \frac{\partial\textbf z_i^j}{\partial{}^{C_{i,2}}\textbf p_j} = \frac{1}{{}^{C_{i,2}}\hat Z_j}\begin{pmatrix} 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{pmatrix}\\ \frac{\partial{}^{C_{i,1}}\textbf p_j}{\partial\textbf x_{C_{i,1}}} = \begin{pmatrix} \left\lfloor{}^{C_{i,1}}\hat\textbf p_{j\times}\right\rfloor & -C({}_{G}^{C_{i,1}}\hat\textbf q \end{pmatrix}\\ \frac{\partial{}^{C_{i,1}}\textbf p_j}{\partial{}^G\textbf p_j} = C({}_{G}^{C_{i,1}}\hat\textbf q)\\ \frac{\partial{}^{C_{i,2}}\textbf p_j}{\partial\textbf x_{C_{i,1}}} = C({}_{C_{i,2}}^{C_{i,1}}\textbf q)^\top\begin{pmatrix} \left\lfloor{}^{C_{i,1}}\hat\textbf p_{j\times}\right\rfloor & -C({}_{G}^{C_{i,1}}\hat\textbf q)\end{pmatrix}\\ \frac{\partial{}^{C_{i,2}}\textbf p_j}{\partial{}^G\textbf p_j} = C({}_{C_{i,2}}^{C_{i,1}}\textbf q)^\top C({}_G^{C_{i,1}}\textbf q) ∂Ci,1pj∂zij=Ci,1Z^j1⎝⎜⎜⎜⎜⎛10000100−Ci,1Z^jCi,1X^j−Ci,1Z^jCi,1Y^j00⎠⎟⎟⎟⎟⎞∂Ci,2pj∂zij=Ci,2Z^j1⎝⎜⎜⎜⎜⎛0010000100−Ci,1Z^jCi,2X^j−Ci,1Z^jCi,2Y^j⎠⎟⎟⎟⎟⎞∂xCi,1∂Ci,1pj=(⌊Ci,1p^j×⌋−C(GCi,1q^)∂Gpj∂Ci,1pj=C(GCi,1q^)∂xCi,1∂Ci,2pj=C(Ci,2Ci,1q)⊤(⌊Ci,1p^j×⌋−C(GCi,1q^))∂Gpj∂Ci,2pj=C(Ci,2Ci,1q)⊤C(GCi,1q)

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

// Modifty the measurement Jacobian to ensure
// observability constrain.
Matrix<double, 4, 6> A = H_x;
Matrix<double, 6, 1> u = Matrix<double, 6, 1>::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的雅可比矩阵：
rj=Hxjx~+HfjGp~j+nj\textbf r^j = \textbf H_{\textbf x}^j\tilde\textbf x + \textbf H_f^j{^G}\tilde\textbf p_j + \textbf n^j rj=Hxjx~+HfjGp~j+nj

B. 把雅可比和残差向量投影到Hfi\textbf H_f^iHfi的零空间
由于当前形式的rj\textbf r^jrj无法进行标准EKF更新，需要把它投影到Hfi\textbf H_f^iHfi的零空间,得到
r0j=V⊤rj=V⊤Hxjx~+V⊤nj=Hx,0jx~+n0j\textbf r_0^j = \textbf V^\top\textbf r^j = \textbf V^\top\textbf H_{\textbf x}^j\tilde\textbf x + \textbf V^\top\textbf n^j = \textbf H_{\textbf x,0}^j\tilde\textbf x + \textbf n_0^j r0j=V⊤rj=V⊤Hxjx~+V⊤nj=Hx,0jx~+n0j
V⊤\textbf V^\topV⊤主要通过对Hfi\textbf H_f^iHfi进行SVD分解得到：

// Project the residual and Jacobians onto the nullspace
// of H_fj.
JacobiSVD<MatrixXd> 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向我们解释了原因，为了方便法表示，记以下符号为：
∂zij∂Ci,1pj=(J10),∂zij∂Ci,1pj=(0J2)∂Ci,1pj∂xCi,1=(H1),∂Ci,1pj∂Gpj=H2,C(Ci,2Ci,1q)=R\frac{\partial\textbf z_i^j}{\partial{}^{C_{i,1}}\textbf p_j} = \begin{pmatrix} \textbf J_1\\ \textbf 0 \end{pmatrix}, \frac{\partial\textbf z_i^j}{\partial{}^{C_{i,1}}\textbf p_j} = \begin{pmatrix} \textbf 0\\ \textbf J_2 \end{pmatrix}\\ \frac{\partial{}^{C_{i,1}}\textbf p_j}{\partial\textbf x_{C_{i,1}}} = \begin{pmatrix} \textbf H_1 \end{pmatrix}, \frac{\partial{}^{C_{i,1}}\textbf p_j}{\partial{}^G\textbf p_j} = \textbf H_2, C({}_{C_{i,2}}^{C_{i,1}}\textbf q) = \textbf R ∂Ci,1pj∂zij=(J10),∂Ci,1pj∂zij=(0J2)∂xCi,1∂Ci,1pj=(H1),∂Gpj∂Ci,1pj=H2,C(Ci,2Ci,1q)=R
于是有
HCij=(J1H1J2R⊤H1),Hfij=(j1H2J2R⊤H2)\textbf H_{C_i}^j = \begin{pmatrix} \textbf J_1\textbf H_1\\ \textbf J_2\textbf R^\top\textbf H_1 \end{pmatrix}, \textbf H_{f_i}^j = \begin{pmatrix} \textbf j_1\textbf H_2\\ \textbf J_2\textbf R^\top\textbf H_2 \end{pmatrix} HCij=(J1H1J2R⊤H1),Hfij=(j1H2J2R⊤H2)
设v=(v1⊤,v2⊤)⊤∈R4\textbf v = (\textbf v_1^\top,\textbf v_2^\top)^\top \in \mathbb R^4v=(v1⊤,v2⊤)⊤∈R4是Hfij\textbf H_{f_i}^jHfij的左零空间，于是有
v⊤Hfij=(v1⊤J1+v2⊤J2R⊤)H2=0\textbf v^\top\textbf H_{f_i}^j = (\textbf v_1^\top\textbf J_1 + \textbf v_2^\top\textbf J_2\textbf R^\top)\textbf H_2 = \textbf 0 v⊤Hfij=(v1⊤J1+v2⊤J2R⊤)H2=0
由于H2=C(GCi,1q^)\textbf H_2 = C({}_G^{C_{i,1}}\hat\textbf q)H2=C(GCi,1q^)是一个旋转矩阵，它的秩等于3，因此v1⊤J1+v2⊤J2R⊤=0\textbf v_1^\top\textbf J_1 + \textbf v_2^\top\textbf J_2\textbf R^\top = 0v1⊤J1+v2⊤J2R⊤=0。有这个性质，就可以直接推出v\textbf vv也是HCij\textbf H_{C_i}^jHCij的左零空间：
v⊤HCij=(v1⊤J1+v2⊤J2R⊤)H1=0\textbf v^\top\textbf H_{C_i}^j = (\textbf v_1^\top\textbf J_1 + \textbf v_2^\top\textbf J_2\textbf R^\top)\textbf H_1 = 0 v⊤HCij=(v1⊤J1+v2⊤J2R⊤)H1=0
所以单独的一个双目观测不能用于EKF更新。

4.5.4 EKF更新

在把上一步得到的雅可比矩阵Hx,0j\textbf H_{\textbf x, 0}^jHx,0j和残差向量叠r0j\textbf r_0^jr0j叠加起来后，就得到了一个大的雅可比矩阵Hx,0\textbf H_{x,0}Hx,0和残差向量r0\textbf r_0r0，为了与代码中的变量保持一致，记为Hx\textbf H_xHx和r\textbf rr。
对应算法位于

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

由于Hx\textbf H_xHx的维数较高，为了降低EKF更新的计算量，对Hx\textbf H_xHx进行QR分解：
Hx=(Q1Q2)(TH0)\textbf H_x = \begin{pmatrix} \textbf Q_1 &\textbf Q_2 \end{pmatrix} \begin{pmatrix} \textbf T_H \\ \textbf 0 \end{pmatrix} Hx=(Q1Q2)(TH0)
其中Q1,Q2\textbf Q_1 ,\textbf Q_2Q1,Q2是单位正交列向量组，TH\textbf T_HTH是上三角矩阵，于是
r=(Q1Q2)(TH0)x~+n⇒(Q1⊤rQ2⊤r)=(TH0)x~+(Q1⊤nQ2⊤n)\textbf r = \begin{pmatrix} \textbf Q_1 &\textbf Q_2 \end{pmatrix} \begin{pmatrix} \textbf T_H \\ \textbf 0 \end{pmatrix}\tilde\textbf x + \textbf n\\ \Rightarrow \begin{pmatrix} \textbf Q_1^\top\textbf r\\ \textbf Q_2^\top\textbf r \end{pmatrix} = \begin{pmatrix} \textbf T_H\\ \textbf 0 \end{pmatrix}\tilde\textbf x + \begin{pmatrix} \textbf Q_1^\top\textbf n\\ \textbf Q_2^\top\textbf n \end{pmatrix} r=(Q1Q2)(TH0)x~+n⇒(Q1⊤rQ2⊤r)=(TH0)x~+(Q1⊤nQ2⊤n)
从上式可以很清楚的看出，通过把残差投影到TH\textbf T_HTH的基向量组Q1\textbf Q_1Q1上，可以保留观测的全部有效信息，残差项Q2⊤r\textbf Q_2^\top\textbf rQ2⊤r仅仅是噪声，完全可以忽略，于是可以通过以下残差来进行EKF更新：
r′=Q1⊤r=THx~+n′\textbf r' = \textbf Q_1^\top\textbf r = \textbf T_H\tilde\textbf x + \textbf n' r′=Q1⊤r=THx~+n′
计算卡尔曼增益：
K=PTH⊤(THPTH⊤+R)−1\textbf K = \textbf P\textbf T_H^\top(\textbf T_H\textbf P\textbf T_H^\top + \textbf R)^{-1} K=PTH⊤(THPTH⊤+R)−1
于是状态向量的修正增量为：
Δx=Kr\Delta\textbf x = \textbf K\textbf r Δx=Kr
然后更新协方差：
Pk+1∣k+1=(Iξ−KTH)Pk+1∣k(Iξ−KTH)⊤+KRK⊤\textbf P_{k+1|k+1} = (\textbf I_\xi - \textbf{KT}_H)\textbf P_{k+1|k}(\textbf I_\xi - \textbf{KT}_H)^\top + \textbf{KRK}^\top Pk+1∣k+1=(Iξ−KTH)Pk+1∣k(Iξ−KTH)⊤+KRK⊤
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进行显示。