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}$$