notes

一步步深入了解VINS-Mono的系统初始化

作者:钟心亮

联系方式:xinliangzhong@foxmail.com

Github:https://github.com/TurtleZhong/vins_notes

一步步深入了解VINS-Mono的系统初始化0.前言0.1 为什么写这个0.2 适合谁读1.概述2. estimator_node节点2.1. main()函数2.2 imu_callback函数2.3. feature_callback函数2.4. VIO线程2.4.1 准备数据measurements2.4.2 预积分部分2.4.3 处理上述IMU对应的视觉feature部分2.4.4 processImage2.4.4.1 marginalization_flag的确定2.4.4.2 构建用于SFM的all_image_frame数据2.4.4.3 标定相机与IMU的旋转2.4.4.4 初始化系统或者非线性滑窗优化3.相机与IMU的Rotation标定3.1 理论推导3.2 代码解释4.初始化的Slide Window操作4.1 MARGIN_OLD4.2 MARGIN_SECOND_NEW5. 初始化5.1 all_image_frame增删5.2 初始化部分initialStructure()5.2.1 IMU激励检测5.2.2 选出足够视差的对应基准帧5.2.3 sfm.construct() 滑窗SFM5.2.3.1 准备工作5.2.3.2 以WINDOW_SIZE-1帧为基准的后半段Pose和地图点恢复5.2.3.3 以l帧为基准的后半段的剩余地图点再恢复5.2.3.4 以l帧为基准的前半段Pose和地图点恢复5.2.3.5 对所有还未三角化出来的地图点进行三角化5.2.3.6 Full BA5.2.4 求解all_image_frame剩下帧的位姿5.2.5 visual-inertial alignment:视觉SFM的结果与IMU预积分结果对齐5.2.5.1 求解陀螺仪bisa误差 \delta b_{g}5.2.5.2 初始化速度，重力向量和尺度因子5.2.5.3 重力向量的精修5.2.6 优化地图点5.2.7 重力与世界坐标系对齐6. 总结参考文献附录A 预积分A.1 IMU测量模型A.2 IMU运动学模型A.3 IMU预积分A.3.1 连续时间下图像帧间IMU状态方程A.3.2离散时间下图像帧间位姿估计}附录B FeatureManager类B.1. Feature管理部分B.2 重要函数解释B.2.1 addFeatureCheckParallaxB.2.1.1 B.2.1.2B.2.1.3 B.2.1.4 B.2.1.5 B.2.2 compensatedParallax2函数附录C 三角化

0.前言

0.1 为什么写这个

泡泡机器人代码组主要的职责是让slam研究爱好者更加容易的上手一个开源SLAM方案。作为泡泡机器人代码组的一员，前段时间（其实挺久了）领了VINS-Mono这个分支的任务，自己也主要负责初始化这一块，其实很早之前基本主体已经写完了，但一直忙着毕业以及实验室的琐碎项目并没有完稿，所以趁着入职前把这部分写完，因为入职之后可能会很忙，更没有时间系统成文了！最后说一下泡泡代码组后续会推出系列文章，包含代码注释，算法的某个模块的细致讲解等等，总之就像标题所说，一步步带你搞懂，如有问题，可以邮箱联系我：xinliangzhong@foxmail.com。

0.2 适合谁读

建议阅读本文的小伙伴至少跑通过VINS-Mono，最好是用自己的传感器跑过而不是EuRoc数据集，因为用自己的传感器跑的过程会涉及到传感器的标定（这里或多或少都会打印一个标定板，并使用过kalibr标定工具等等），以及如何让VINS-Mono系统初始化并成功运行起来等等一系列操作。第二个要求是最好是推导过预积分的公式，至少知道什么是预积分。这里给大家推荐强哥与晨哥的预积分系列推导：成强的博客以及晨哥预积分教程，其中后者是流行上的预积分，与VINS的细节略有不同。当然为了保证完整性，我会在附录中给出大致的预积分流程（其实还算详细，只是略去了冗长的推导部分），具体还请大家参考细致的博客。

1.概述

其实 VINS-Mono 本身作为一个超级优秀的VIO方案基本上已经被各大博主写过了，那我想声明一点就是我这个总结肯定参考了各大博主之前写过的VINS-Mono的材料，当然里边也包含了很多自己看代码以及跟大佬们讨论的一些心血，所以总体来说还是可以一看的。

本文主要针对VINS-Mono的初始化部分进行讲解，讲解之前我们先关注一下VINS-Mono的整体框架，如图所示：

vins-structure

我们可以看到VINS-Mono主要包含四大部分：

(1) 观测数据预处理。视觉部分主要完成特征点的提取与跟踪；而IMU 数据则进行预积分操作。其中预积分部分的公式详见附录A。

(2) 初始化。初始化过程主要完成了相机与IMU 的外参标定，陀螺仪偏置误差的校正，以及整个滑窗内视觉SFM与IMU的对齐操作。

(3) 滑窗VIO非线性优化。整个系统的核心部分，构建了视觉与IMU 紧耦合的非线性优化问题，得到机器人的最优估计。

(4) 回环检测与优化。构建关键帧数据库，利用词袋算法进行回环检测并将整个系统进行回环优化校正，能够得到具有一致性的轨迹，提高整个系统的全局精度。

本文的安排如下：其中第1节简要讲述了VINS-Mono的整体框架以及行文安排；第2节主要讲解VINS-Mono中代码estimator_node节点的大致框架；第3节主要阐述了相机与IMU的Rotation的标定，包含理论推导与代码解析；第4节主要分析了初始化之前的滑窗操作；第5节则结合代码重点分析了VINS-Mono的初始化过程；第6节做了简要的全文总结。另外附录部分主要讲述了预积分、三角化等基本数学原理。

2. estimator_node节点

为了保证行文完整，这里会完整讲解VINS-Mono的框架，但重点依旧放在初始化相关的部分，其中初始化需要两个重要的数据：1.IMU数据；2.特征点数据。所以还算需要讲解一下whole picture。整个系统的算法主节点为estimator_node，所以下面重点讲解一下过程。

2.1. main()函数

这个节点是整个VIO的实现，其中main函数主要实现了多线程的设置，订阅的话题以及发布的话题，基本结构如下


x
 //订阅IMU,feature,image的topic,然后在各自的回调里处理消息
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 20000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    ros::Subscriber sub_raw_image = n.subscribe(IMAGE_TOPIC, 2000, raw_image_callback);
    std::thread measurement_process{process};
    std::thread loop_detection, pose_graph;
    if (LOOP_CLOSURE)
    {
        ROS_WARN("LOOP_CLOSURE true");
        loop_detection = std::thread(process_loop_detection);   
        pose_graph = std::thread(process_pose_graph);
        m_camera = CameraFactory::instance()->generateCameraFromYamlFile(CAM_NAMES);
    }

我们先不分析LOOP_CLOSURE部分只关注VIO部分，那么其实核心线程就是在measurement_process,而这个线程的主要实现函数是process函数当然还有两个重要的call_back函数：imu_callback和feature_callback

2.2 imu_callback函数

我们先来看imu_callback函数


xxxxxxxxxx
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();
    {
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg); /// 纯IMU积分得到的轨迹
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

其中imu_buf是一个队列，数据类型为 queue<sensor_msgs::ImuConstPtr> 那么也就是说上来先把imu的数据塞到一个队列里边，以便于后续的处理。紧接着对imu进行纯imu积分获取轨迹，因为IMU的频率快，所以一般来说imu的轨迹是最新的。


xxxxxxxxxx
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    double dt = t - latest_time;
    latest_time = t;
    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};
    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector3d angular_velocity{rx, ry, rz};
    Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba - tmp_Q.inverse() * estimator.g);
    Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
    tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
    Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba - tmp_Q.inverse() * estimator.g);
    Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
    tmp_V = tmp_V + dt * un_acc;
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

当对IMU进行积分之后，如果说现在的estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR ；其实就是说现在已经初始化完成了并不是出于初始化阶段，而是处于滑窗优化阶段。就发布刚刚积分得到的PVQ。

2.3. feature_callback函数

这个函数主要对特征点进行订阅并塞到一个feature_buf的队列里边，其类型为queue<sensor_msgs::PointCloudConstPtr> feature_buf;

2.4. VIO线程

这个线程主要在process()函数中并且是一直循环的，这里用伪代码的形式概括其主要框架：


xxxxxxxxxx
    while
        1. 准备数据，即取出feature和imu对应的measurements 一张图对应多个imu数据 但同时也会有多个feature
            for measurement：measurements
                2. 先拿出vision对应的多个imu数据进行预积分(send_imu()函数)
                    for imu_msg : measurement.first 
                        send_imu(imu_msg)
                    end
                3. 处理这段数据的视觉信息
                    for point : img_msg->points
                        塞到map<int, vector<pair<int, Vector3d>>>数据结构中
                    end
                4. 处理视觉信息processImage( )
                
                if（LOOP_CLOSURE）
                   D. do somthing
                end if
                发布里程计信息
            end
            if（solver_flag == NON_LINEAR）
                E.更新VIO
     end

我们对照着上述的字母分块讲解，当然其中肯定会遇到细分代码，我们以小标题的形式给出。

2.4.1 准备数据measurements

measurements的数据结构为 std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> 其中图示如下: measurements-data-structure

然后measurements的获取是线程等待获取的，在getMeasurements()函数中实现

其中大while循环里边包含三个if判断和一个小while循环，我们以时间轴为示例

2.4.2 预积分部分

预积分理论部分请参考附录A，这里主要针对代码实现，并且会说明对应代码与理论推导的对应关系。预积分部分的接口函数在2.4部分是send_imu(imu_msg)函数，相当于拿上图绿色圆圈上半部分做循环处理，其中核心实现是在Estimator类里边的processorIMU函数中实现的；

2.4.3 处理上述IMU对应的视觉feature部分

在讲feature部分之前，我们先过一下管理feature的数据结构：Feature管理类

请结合FeatureManager代码部分笔记食用更佳，参考附录B

这里列举几个特别容易混淆的类

类名	含义
FeaturePerFrame	最小单元，表示一个特征
FeaturePerId	同一个特征会有多个观测值
FeatureManager	管理特征的大类，里边用 list 表示滑窗内所有特征点

其中图示如下：

2.4.4 processImage

回到处理视觉信息部分的processImage类

processImage()主要分为一下几大块 {

1. 通过视差判断边缘化策略
1. 构建用于SFM的all_image_frame数据
1. 标定相机与IMU的旋转
1. 初始化系统或者非线性滑窗优化

}

2.4.4.1 marginalization_flag的确定

这部分请参考附录B FeatureManager类里边的 2.1addFeatureCheckParallax 里边有详细的说明。其中这里说明marginalization_flag值的意义。

marginalization_flag=MARGIN_OLD 次新帧被接收为关键帧，需要边缘化掉老的帧
marginalization_flag=MARGIN_SECOND_NEW 当次新帧并不是关键帧，需要边缘化掉它

2.4.4.2 构建用于SFM的all_image_frame数据


xxxxxxxxxx
    ImageFrame imageframe(image, header.stamp.toSec());
    imageframe.pre_integration = tmp_pre_integration;
    ROS_WARN_STREAM("Add frame to all_image_frame for SFM");
    all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe)); // 每读取一帧图像特征点数据，都会存入all_image_frame
    tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

2.4.4.3 标定相机与IMU的旋转

详细解释参考相机与IMU的rotation标定

2.4.4.4 初始化系统或者非线性滑窗优化

这部分对应的主要代码为仔细看其结构是一个if-else语句，判断条件为solver_flag==INITIAL？如果说判断为真，那么说明系统还未初始化，需要先对系统进行初始化，这里包括视觉跟IMU的对齐工作等等。如果判断为假，那么，说明系统已经处于非线性优化阶段。

3.相机与IMU的Rotation标定

3.1 理论推导

相机与IMU之间的外参包括旋转和平移，其中旋转部分的标定非常重要，偏差1°至2°系统的精度就会变得很低。当然相机与IMU的外参可以通过离线工具（如kalibr）进行离线标定，但是这种方法需要离线制作特殊图案的标定板并离线采集数据进行标定。VINS-Mono根据相机观测与IMU预积分之间的约束进行在线标定，并不需要制作特殊的标定图案，只需要保证相机运动且有一定的旋转激励即可完成两者之间的旋转参数标定。

$k$ $k+1$ $t\in [k,k+1]$ 这段时间内分别能获取两帧图像数据以及这段时间内的IMU数据。

calibration_R

$R_{{{c}_{k+1}}}^{{{c}_{k}}}$ $R_{{{b}_{k+1}}}^{{{b}_{k}}}$ $R_{c}^{b}$ $k$ 时刻，我们都有以下约束：

\begin{align} R_{{{b}_{k+1}}}^{{{b}_{k}}}R_{c}^{b} &= R_{c}^{b}R_{{{c}_{k+1}}}^{{{c}_{k}}} \\ q_{{{b}_{k+1}}}^{{{b}_{k}}}\otimes q_{c}^{b} &= q_{c}^{b}\otimes q_{{{c}_{k+1}}}^{{{c}_{k}}}\label{5-1b} \end{align}

其中式(2)为四元数的表示方法，并利用四元数左乘和右乘性质可以转化为，关于四元数可以参考我原先s-msckf笔记中的四元数教程：

\begin{equation} \begin{aligned} ({{Q}_{L}}(q_{{{b}_{k+1}}}^{{{b}_{k}}})-{{Q}_{R}}(q_{{{c}_{k+1}}}^{{{c}_{k}}}))q_{c}^{b}=0 \end{aligned} \end{equation}

将括号内部四元数减法运算进行简化并加上权重可以得到：

\begin{equation} \begin{aligned} w_{k+1}^{k}Q_{k+1}^{k}q_{c}^{b}=0 \end{aligned} \end{equation}

当有N对相对旋转的IMU和视觉测量值时，可以得到过约束线性方程：

\begin{equation} \begin{aligned} \left[ \begin{matrix} w_{1}^{0}Q_{1}^{0} \\ w_{2}^{1}Q_{2}^{1} \\ \vdots \\ w_{N}^{N-1}Q_{N}^{N-1} \\ \end{matrix} \right]q_{c}^{b}=0\label{5-4} \end{aligned} \end{equation}

$Ax=0$ $x$ $x$ $R_{c}^{b}$ $Ax=0$ 的详细解法等可以参考这个博客。

3.2 代码解释

上述IMU与相机的旋转标定代码如图所示：

cali_ex_R

代码第51-57行，这部分判断是否能够满足标定条件。判定条件为A矩阵的第二个奇异值>0.25，这个数值应该能够反映激励程度。另外ric表示相机到imu的旋转，注意代码45行有一个inverse()操作。

4.初始化的Slide Window操作

初始化的过程是需要数据的，因为初始化过程涉及到IMU与相机外参的标定，陀螺仪偏置误差的校正，视觉SFM与IMU的对齐，重力向量的精修等等。这里面比如相机与IMU相机外参的标定，陀螺仪偏置误差的校正等等都需要一定的运动激励，并不是说算法无脑的取一定的Size就取初始化，考虑一个极端的情况就是你一直静止是永远初始化不成功的，而VINS-Mono默认Slide Window的大小为10，也就是后端优化只维护窗口大小为10的一个非线性优化。所以在这里有必要讲解一下初始化阶段，也就是代码中的INITIAL阶段的SlideWindow操作。VINS-Mono的滑窗方式是根据次新帧是否为关键帧来采取不同的marg措施的，其中在附录B中应该有解释如何判断是否为关键帧。这里再次给出结论：

marginalization_flag=MARGIN_OLD 次新帧被接收为关键帧，需要边缘化掉老的帧
marginalization_flag=MARGIN_SECOND_NEW 当次新帧并不是关键帧，需要边缘化掉它

slide_window-1

我们从代码可以很清晰的看到：当相机与IMU的旋转初始标定完成之后且当帧数已经满了滑窗大小之后系统会尝试初始化。当然初始化过程我们在下一节中详细讲解，现在我们先假设初始化失败，从代码来看需要进行滑窗函数即 slideWindow()函数，slideWindow系列还包括slideWindowNew和slideWindowOld。

4.1 MARGIN_OLD

这个时候processImage最开始计算出margination_flag=MARGIN_OLD,这意味着需要marg掉第0帧，我们用图示来表示其整个过程。主要有三个大步骤

1. 因为要将最老帧margin掉，那么需要将后面的往后挪动
1. 剔除掉用于SFM的帧
1. 剔除第0帧的feature

slidewindow-2

4.2 MARGIN_SECOND_NEW

这个时候processImage最开始计算出margination_flag=MARGIN_SECOND_NEW,这意味着需要marg掉倒数第二帧，我们用图示来表示其整个过程。主要有两个大步骤：

1. 第9帧的预积分数据需要再叠加第10帧的IMU数据，世界坐标系下的PVQ也需要置为第十帧的数据。
1. 对feature进行管理

slidewindow-3

5. 初始化

5.1 all_image_frame增删

在讲这里之前其实应该对all_image_frame的增加与删除进行说明，因为这是作为数据输入。下面这张图是以SlideWindow大小为4进行说明的，如果要看SlideWindow的增删则需要参考SlideWindow部分笔记。all_image_frame的数据类型为map<double, ImageFrame>其中key为时间戳，value为ImageFrame数据类型，其包含数据为：

5.2 初始化部分initialStructure()

初始化部分入口在在processImage()的下面位置，其主要入口为initialStruct()；该函数发生在相机与IMU的旋转参数已经给出或者说已经在线标定了才会进行系统初始化对齐工作。其中相机与IMU的标定工作参见第3节。其入口在图片第8行。

整个过程主要分为以下几步

1.确保IMU有足够的excitation
2.检查当前帧（滑动窗口中的最新帧）与滑动窗口中所有图像帧之间的特征点匹配关系，
2.1 选择跟当前帧中有足够多数量的特征点（30个）被跟踪，且有足够视差（20 pixels）的某一帧，利用五点法恢复相对旋转和平移量。
2.2 如果找不到，则在滑动窗口中保留当前帧，然后等待新的图像帧
3.sfm.construct 全局SFM 恢复滑动窗口中所有帧的位姿，以及特特征点三角化
4.利用pnp恢复其他帧, 并将旋转乘以相机与IMU的外参
5.visual-inertial alignment：视觉SFM的结果与IMU预积分结果对齐
6.给滑动窗口中要优化的变量一个合理的初始值以便进行非线性优化

在初始化过程中，各个坐标系的转换，不仔细分析的话可能会弄得很晕眩，所以这里会用图示帮助大家分析，而且指出相应的函数计算的是哪个坐标系与哪个坐标系之间的相互转换。

5.2.1 IMU激励检测

拿出all_image_frame的所有帧，计算各个时间段的平均加速度，然后计算方差，如果说variance<0.25 则激励是不够的。初始化也不会成功。

5.2.2 选出足够视差的对应基准帧

以下部分会频繁涉及到以下数据或变量，下面做出说明，方便对应

变量	数据类型	含义
frame_count	`int`	其实这里等于WINDOW_SIZE,例子中等于4
all_image_frame	`map<double, ImageFrame>`	用于初始化的所有帧，大于等于滑窗内的帧数
l	`int`	基准帧在滑窗中的位置(下面的例子是第2帧，也就是对应timestamp=8)
Q[frame_count + 1]	Eigen::Quaterniond	滑动窗口中每一帧的姿态，以基准帧为参考系
T[frame_count + 1]	Eigen::Vector3d	滑动窗口中每一帧的位置，以基准帧为参考系

函数	参数说明	返回值
`relativePose(Matrix3d &relative_R,Vector3d &relative_T, int &l)`	relative_R：最新帧相对于l帧的旋转；relative_T：最新帧相对于l帧的平移；l：滑窗中与最新帧有足够数量匹配点且足够视差的基准帧编号	能找到基准帧返回真
`GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l, const Matrix3d relative_R, const Vector3d relative_T,vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)`	frame_num：滑窗大小+1；Quaterniond* q：一个数组指针，存放滑窗内所有帧相对于参考帧的旋转；Vector3d* T：数组指针，存放所有帧相对于参考帧的平移；relative_R：最新帧到参考帧的旋转； relative_T 最新帧到参考帧的平移； vector &sfm_f 用于SFM的特征点；map<int, Vector3d> &sfm_tracked_points：所有特征点	初始化成功返回真

我们选取5.1中黑色箭头对应的帧，即此时滑窗状态和all_image_frame的状态对应如下。

然后只对滑窗内的帧进行循环判断，找出与最新帧有足够匹配点对和视差的帧，其中对应函数入口为： relativePose(Matrix3d &relative_R,Vector3d &relative_T, int &l) 其中输出在上面有详细说明。在这个函数的内部调用了opencv的cv::findFundamentalMat()和cv::recoverPose()两个函数，其中第一个函数是找基础矩阵，然后第二是通过基础矩阵恢复R,T，我们注意到opencv得到的旋转与平移关系都是以点为基准的，这里举个例子详细说明，代码中找基础矩阵的对应代码为：


xxxxxxxxxx
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);

这里得到的E=[t]xR,其中R和t分别是将ll点转变到rr的旋转与平移，这与我们常用的以相机为参考正好是相反的，假设我们的ll对应时间戳为8的帧，rr对应最新帧，即第11帧，那么此时通过基础矩阵恢复的R和T其实是：


xxxxxxxxxx
cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);

$R_{8}^{11}, T_{8}^{11}$ ]，但是在程序最后进行了取逆操作：


xxxxxxxxxx
        Rotation = R.transpose();
        Translation = -R.transpose() * T;

$R_{11}^{8}, T_{11}^{8}$ ]，其实就是以8为基准帧并且作为参考系。图示如下：

5.2.3 sfm.construct() 滑窗SFM

5.2.3.1 准备工作

$c_{0}$ 作为参考坐标系，那么这段代码在initial_sfm.cpp里边，在三角化和pnp求解位置之前先做了下面两步操作：

l $c_{0}$
Pose[frame_num+1]是投影矩阵，是从参考坐标系到各个帧的变换，正好跟Q T是逆的关系

注意下面用到的三角化以及PNP都是以Pose即投影矩阵来操作的，也就是说正好于最开始的QT为逆

5.2.3.2 以WINDOW_SIZE-1帧为基准的后半段Pose和地图点恢复

我们看下面的这个步骤，这一步以最新帧为基准，计算出了l到WINDOE_SIZE的所有Pose以及地图点。

其中三角化的数学部分见附录C。

5.2.3.3 以l帧为基准的后半段的剩余地图点再恢复

当以WINDOW_SIZE-1帧为基准算出后半部分的Pose和地图点之后，再以l帧为基准重新对后边的点进行再次三角化，这一步能三角化出更多在l帧观测的地图点，便于下一步对前半部分进行Pose恢复和三角化。

5.2.3.4 以l帧为基准的前半段Pose和地图点恢复

这一步骤和5.2.3.2基本一致，其步骤见图

5.2.3.5 对所有还未三角化出来的地图点进行三角化

5.2.3.6 Full BA

在上述步骤求解完成之后再对滑窗做一次全局BA

5.2.4 求解all_image_frame剩下帧的位姿

滑窗内的位姿都求解完成之后再对all_image_frame帧进行求解，并将RT乘上IMU到相机坐标系的旋转外参，这样得到的RT 分别是相机对应帧的IMU坐标系到c0系的变换，平移依旧是第k帧到c0的平移

$R_{b_{k}}^{c_{0}}$ $T_{c_{k}}^{c_{0}}$

5.2.5 visual-inertial alignment:视觉SFM的结果与IMU预积分结果对齐

这一部分是SFM部分与IMU预积分部分对齐，其中要求解陀螺仪的bias，每一帧的速度，重力向量，视觉尺度因子的计算等等。

$\delta b_{g}$

如图表示了之前的初始化步骤所能得到的数据，其中IMU部分主要是根据附录B计算得到的预积分数据，视觉部分则是根据刚刚提到的视觉PnP求解和三角化操作得到相机的位姿。

solve g

${{b}_{k}}$ ${{b}_{k+1}}$ $q_{{{b}_{k}}}^{w}$ $q_{{{b}_{k+1}}}^{w}$ $\gamma _{{{b}_{k+1}}}^{{{b}_{k}}}$ ，于是可以构建校正陀螺仪偏置的目标函数：

\begin{equation}\label{eq_5_5} \begin{aligned} \underset{\delta {{b}_{\omega }}}{\mathop{\min }}\,{{\sum\limits_{k\in N}{\left\| q{{_{{{b}_{k+1}}}^{w}}^{-1}}\otimes q_{{{b}_{k}}}^{w}\otimes \gamma _{{{b}_{k+1}}}^{{{b}_{k}}} \right\|}}^{2}} \end{aligned} \end{equation}

其中:

\begin{equation}\label{eq_5_6} \begin{aligned} \gamma _{{{b}_{k+1}}}^{{{b}_{k}}}=\hat{\gamma }_{{{b}_{k+1}}}^{{{b}_{k}}}\otimes \left[ \begin{matrix} 1 \\ \frac{1}{2}J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \theta _{{{b}_{k+1}}}^{{{b}_{k}}}}\delta {{b}_{{{\omega }_{k}}}} \\ \end{matrix} \right] \end{aligned} \end{equation}

$N$ $\gamma _{{{b}_{k+1}}}^{{{b}_{k}}}$ $\delta {{b}_{w}}$ 的一阶近似。

等式(6)中目标函数的最小值为单位四元数，所以可以将等式6进一步改写为：

\begin{equation}\label{eq_5_7} \begin{aligned} q{{_{{{b}_{k+1}}}^{w}}^{-1}}\otimes q_{{{b}_{k}}}^{w}\otimes \hat{\gamma }_{{{b}_{k+1}}}^{{{b}_{k}}}\otimes \left[ \begin{matrix} 1 \\ \frac{1}{2}J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \theta _{{{b}_{k+1}}}^{{{b}_{k}}}}\delta {{b}_{{{\omega }_{k}}}} \\ \end{matrix} \right]=\left[ \begin{matrix} 1 \\ 0 \\ 0 \\ 0 \\ \end{matrix} \right] \end{aligned} \end{equation}

于是可以有：

\begin{equation}\label{eq_5_8} \begin{aligned} \left[ \begin{matrix} 1 \\ \frac{1}{2}J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \theta _{{{b}_{k+1}}}^{{{b}_{k}}}}\delta {{b}_{{{\omega }_{k}}}} \\ \end{matrix} \right]=\hat{\gamma }{{_{{{b}_{k+1}}}^{{{b}_{k}}}}^{-1}}\otimes q{{_{{{b}_{k}}}^{w}}^{-1}}q_{{{b}_{k+1}}}^{w}\otimes \left[ \begin{matrix} 1 \\ 0 \\ 0 \\ 0 \\ \end{matrix} \right] \end{aligned} \end{equation}

若只考虑四元数的虚部，则可以进一步简化为：

\begin{equation}\label{eq_5_9} \begin{aligned} J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \theta _{{{b}_{k+1}}}^{{{b}_{k}}}}\delta {{b}_{{{\omega }_{k}}}}=2{{(\hat{\gamma }{{_{{{b}_{k+1}}}^{{{b}_{k}}}}^{-1}}\otimes q{{_{{{b}_{k}}}^{w}}^{-1}}q_{{{b}_{k+1}}}^{w})}_{vec}} \end{aligned} \end{equation}

${{(\centerdot )}_{vec}}$ 表示取四元数的虚部，式子11只是滑动窗口内连续两帧之间的约束方程，如果把滑动窗口内的所有帧进行增广，且假设在窗口内陀螺仪偏置保持不变。于是可以得到如下方程：

\begin{equation}\label{eq_5_10} \begin{aligned} \left[ \begin{matrix} J_{\delta {{b}_{\omega }}}^{\delta \theta _{{{b}_{1}}}^{{{b}_{0}}}} \\ \vdots \\ J_{\delta {{b}_{\omega }}}^{\delta \theta _{{{b}_{N}}}^{{{b}_{N-1}}}} \\ \end{matrix} \right]\delta {{b}_{\omega }}=\left[ \begin{matrix} 2{{(\hat{\gamma }{{_{{{b}_{1}}}^{{{b}_{0}}}}^{-1}}\otimes q{{_{{{b}_{0}}}^{w}}^{-1}}q_{{{b}_{1}}}^{w})}_{vec}} \\ \vdots \\ 2{{(\hat{\gamma }{{_{{{b}_{N}}}^{{{b}_{N-1}}}}^{-1}}\otimes q{{_{{{b}_{N-1}}}^{w}}^{-1}}q_{{{b}_{N}}}^{w})}_{vec}} \\ \end{matrix} \right] \end{aligned} \end{equation}

$Ax=b$ $\delta {{b}_{\omega }}$ 的校正。

其实这一部分的原理就是SFM会计算出一个旋转矩阵，而预积分部分也会计算出一个两帧之间的预积分，这两个旋转矩阵理论上应该是完全相等的，所有可以靠这个来解一个最小二乘问题，而代码部分则是求解Ax=b的问题，这里可以参考数学部分SVD分解与最小二乘关系。

当然在重新得到bg之后，需要对每一帧重新计算预积分.


xxxxxxxxxx
    for (int i = 0; i <= WINDOW_SIZE; i++)
        Bgs[i] += delta_bg;
    for (frame_i = all_image_frame.begin(); next(frame_i) != all_image_frame.end( ); frame_i++)
    {
        frame_j = next(frame_i);
        frame_j->second.pre_integration->repropagate(Vector3d::Zero(), Bgs[0]); //重新预积分
    }

5.2.5.2 初始化速度，重力向量和尺度因子

代码部分对应于VINS-Mono代码initial_aligment.cpp中的LinearAligment()函数。待优化变量为速度V[0:n] Gravity Vectorg,尺度s，总共要优化的量是all_frame_count * 3 + 3 + 1;

待优化的变量为：

\chi_{I}=\left[\begin{array}{c}{v_{b_{0}}^{b_{0}}} \\ {v_{b_{0}}^{b_{0}}} \\ {\vdots} \\ {v_{b_{0}}^{b_{0}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]

$c_{0}$ 系，可得：

\begin{align} \alpha_{b_{k+1}}^{b_{k}} &= R_{c_{0}}^{b_{k}}\left(p_{b_{k+1}}^{c_{0}}-p_{b_{k}}^{c_{0}}-v_{b_{k}}^{c_{0}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right) \\ \beta_{b_{k+1}}^{b_{k}} &= R_{c_{0}}^{b_{k}}\left(v_{b_{k+1}}^{c_{0}}-v_{b_{k}}^{c_{0}}+g^{c_{0}} \Delta t_{k}\right) \end{align}

$p_{b_{k+1}}^{c_{0}}$ $p_{b_{k}}^{c_{0}}$ 可以由视觉SFM得到：

\begin{align} p_{b_{k}}^{c_{0}} &=s \overline{p}_{b_{k}}^{c_{0}} \\ p_{b_{k+1}}^{c_{0}} &=s \overline{p}_{b_{k+1}}^{c_{0}} \end{align}

将其带入式13可得：

\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(s\left(\overline{p}_{b_{k+1}}^{c_{o}}-\overline{p}_{b_{k}}^{c_{0}}\right)-v_{b_{k}}^{c_{0}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right)

将式子14与式17的速度均变换到body系下，则有：

\begin{align} \alpha_{b_{k+1}}^{b_{k}}&=R_{c_{0}}^{b_{k}}\left(s\left(\overline{p}_{b_{k+1}}^{c_{0}}-\overline{p}_{b_{k}}^{c_{0}}\right)-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right) \\ \beta_{b_{k+1}}^{b_{k}}&=R_{c_{0}}^{b_{k}}\left(R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}}+g^{c_{0}} \Delta t_{k}\right) \end{align}

$s \overline{p}_{b_{k}}^{c_{0}}=s \overline{p}_{c_{k}}^{c_{0}}-R_{b_{k}}^{c_{0}} p_{c}^{b}$ $s \overline{p}_{b_{k+1}}^{c_{0}}=s \overline{p}_{c_{k+1}}^{c_{0}}-R_{b_{k+1}}^{c_{0}} p_{c}^{b}$ $Ax=b$ 的形式：

\left[\begin{array}{ccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2}} & {R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)} \\ {-I} & {R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}}} & {R_{c_{0}}^{b_{k}} \Delta t_{k}} & {0}\end{array}\right]\left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]=\left[\begin{array}{c}{\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}} \\ {\beta_{b_{k+1}}^{b_{k}}}\end{array}\right]

但实际上是并不会相等的，而且等式右边为预积分的值，所以其中最后也是算一个Ax=b的最小二乘问题。

程序里面，会把关于尺度求导得到的雅克比除以100，这就意味着，尺度这个变量对残差的影响力减弱了100倍。最终为了能消去残差，优化后的尺度会比实际的大100倍。得到后，要再除以100。这么做的目的，应该是要让尺度的精度更高。

5.2.5.3 重力向量的精修

在上一个步骤中，我们能得到一个初始的重力向量，这个步骤的目的是让重力向量在正切空间中微调。对应函数为initial_aligment.cpp中的RefineGravity()函数。这种将3个自由度变为2个自由度的方式其实也在后端优化中有所体现，这边的话先看一个示意图：

unit_g

当g值得模长固定适，那么其自由度由3变成了2，则可以将g重新参数化得到：

\begin{align} \hat{g} &=\|g\| \overline{\hat{g}}+w_{1} b_{1}+w_{2} b_{2} \\ &=\|g\| \overline{g}+\left[b_{1} \quad b_{2}\right]\left[\begin{array}{c}{w_{1}} \\ {w_{2}}\end{array}\right] \\ &=\|g\| \overline{g}+b w \end{align}

$\overline{\hat{g}}$ $c_{0}$ $b_{1}$ $b_{2}$ $w_{1}$ $w_{2}$ 则为控制参数。

$Ax=b$ 的形式可得：

\left[\begin{array}{cccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2} b} & {R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)} \\ {-I} & {R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}}} & {R_{c_{0}}^{b_{k}} \Delta t_{k} b} & {0}\end{array}\right]\left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {w} \\ {s}\end{array}\right] \approx \\ \left[\begin{array}{c}{\hat{\alpha}_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{b}+1}^{c_{0}} p_{c}^{b}-p_{c}^{b}-\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2}\|g\| \overline{\hat{g}}} \\ {\hat{\beta}_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}} \Delta t_{k}\|g\| \hat{g}}\end{array}\right]

$Ax=b$ 的问题一切变得简单了起来。

5.2.6 优化地图点

在步骤5.2.3.6中对滑窗内的所有位姿和地图点都进行了优化，但是之前步骤三角化出来的地图点都是用的相邻帧的位姿三角化出来的，然而一个地图点会被多帧观测到，所有理论上能构建更多的约束。这一步利用的是1.4做完全局BA且经过修正陀螺仪偏移误差之后的位姿，并且对每个特征点的观测都构建三角化约束，但是对特征点也有要求，需要满足 it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2) 其实也就是说它的观测至少要大于2，且至少是在倒数第4帧能看到的特征点才会被三角化，三角话原理部分跟之前其实是一致的，可以参考附录C三角化；此部分得出得深度是为第一次观测到该帧的相机下边的深度。对应代码为：

5.2.7 重力与世界坐标系对齐

得到c0系下的重力 g 之后，即可根据其与方向向量[0,0,1]之间的旋转，得到世界坐标系与第一帧相机坐标系之间旋转，从而可以将所有图像帧的旋转转换到世界坐标系下。其次根据得到的尺度s，对地图点坐标和平移进行缩放。这部分代码在void visualInitialAlign()函数的后半部分。

6. 总结

全文以VINS-Mono为依托, 主要对VINS-Mono的初始化部分理论基础和实现原理及细节进行了讲述，后期会放出代码的注释版本. 由于笔者水平有限,有些地方理解难免不到位,最后希望望读者批评指正文中不足的地方。

参考文献

(0) VINS-Mono(https://arxiv.org/pdf/1708.03852.pdf)

(1) Quaternion kinematics for the error-state Kalman filter

(2) Indirect Kalman Filter for 3D Attitude Estimation-A Tutorial for Quaternion Algebra

(3) 视觉SLAM十四讲

(4) 高成强(强哥)的预积分

(5) 邱笑晨(晨哥)的预积分

(6) 18年写过的一步步S-msckf入坑指南

附录A 预积分

A.1 IMU测量模型

IMU是由加速度计和陀螺仪组成的惯性测量器件，其中加速度和陀螺仪分别可以测量自身的三轴加速度和三轴角速度。但是对于加速度计和陀螺仪的测量数据都受到偏置和噪声影响，另外由于加速度计的测量原理，它敏感到的加速度实际上并不是纯加速度，而是包含了反向重力加速度的比力。同样的角速度也会受到因地球自转而引起的可是加速度的影响，但是我们一般忽略这个影响，最终我们可以对IMU的测量数据建模为三部分的叠加：

\begin{align} {a}_{t} &= {{a}_{t(real)}}+{{b}_{{{a}_{t}}}}+R_{w}^{t}{{g}^{w}}+{{n}_{a}} \\ {{\omega }_{t}} &= {{\omega }_{t(real)}}+{{b}_{{{w}_{t}}}}+{{n}_{\omega }} \end{align}

${{a}_{t(real)}}$ ${{b}_{{{a}_{t}}}}$ ${{n}_{a}}$ ${g}^{w}$ ${a}_{t}$ ${\omega }_{t(real)}$ ${b}_{{{w}_{t}}}$ ${n}_{\omega }$ ${\omega }_{t}$ 。

加速度和陀螺仪的偏置我们将其建模为随机游走，也就是由一个高斯白噪声驱动的值。即：

{{\dot{b}}_{{{a}_{t}}}} = {{n}_{{{b}_{a}}}} \\ {{\dot{b}}_{{{\omega }_{t}}}} = {{n}_{{{\omega }_{t}}}}

${n}_{{{b}_{a}}}$ ${n}_{{{\omega }_{t}}}$ 服从高斯分布：

{{n}_{{{b}_{a}}}}\sim N(0,\sigma _{{{b}_{a}}}^{2}) \\ {{n}_{{{b}_{\omega }}}}\sim N(0,\sigma _{{{b}_{\omega }}}^{2})

$\sigma _{{{b}_{a}}}$ $\sigma _{{{b}_{\omega }}}$ 为随机游走的噪声强度，可以查阅对应IMU型号的数据手册或者通过标定工具获取。

对于加速度和陀螺仪噪声同样建模为高斯分布：

{{n}_{{{b}_{a}}}}\sim N(0,\sigma _{{{a}}}^{2}) \\ {{n}_{{{b}_{\omega }}}}\sim N(0,\sigma _{{{\omega }}}^{2})

$\sigma {_{{{a}}}}$ $\sigma {_{{{\omega}}}}$ 为高斯噪声强度，同样可以通过查阅对应的IMU型号的数据手册获取或者通过标定工具获取。IMU的随机游走噪声强度和噪声强度通过开源工具 $imu\_utils$ 获得。

A.2 IMU运动学模型

A.2 中介绍了IMU传感器的误差模型，这一小节着重介绍IMU的运动学模型。惯性导航的核心原理是基于牛顿第二定律，即位置的导数等于速度，速度的导数等于加速度，同样的角度的导数为角速度。而IMU正好能以200Hz以上的固定频率得到本体坐标系下的加速度和角速度。假设参考坐标系下载体的初始速度和初始姿态已知，利用载体运动过程中参考系下的加速度和角速度信息，可以不断通过积分运算去更新实时的位置和姿态。

imu_coor_with_data

$b_{k}$ $b_{k+1}$ $t_{k}$ $t_{k+1}$ $[t_{k},t_{k+1}]$ 时间间隔内的IMU测量值对系统的位置、速度和旋转进行积分传播，其中旋转我们采用四元数来表示旋转：

\begin{align} p_{{{b}_{k+1}}}^{w} &= p_{{{b}_{k}}}^{w}+v_{{{b}_{k}}}^{w}\vartriangle {{t}_{k}}+\iint_{_{t\in [{{t}_{k}},{{t}_{k+1}}]}}{(R_{t}^{w}({{a}_{t}}-{{b}_{{{a}_{t}}}}-{{n}_{a}})-{{g}^{w}})d{{t}^{2}}} \\ v_{{{b}_{k+1}}}^{w} &= v_{{{b}_{k}}}^{w}+\int_{t\in [{{t}_{k}},{{t}_{k+1}}]}{(R_{t}^{w}({{a}_{t}}-{{b}_{{{a}_{t}}}}-{{n}_{a}})-{{g}^{w}})dt} \\ q_{{{b}_{k+1}}}^{w} &= q_{{{b}_{k}}}^{w}\otimes \int_{t\in [{{t}_{k}},{{t}_{k+1}}]}{\frac{1}{2}q_{t}^{{{b}_{k}}}\otimes ({{\omega }_{t}}-{{b}_{{{\omega }_{t}}}}-{{n}_{\omega }})dt} \\ &= q_{{{b}_{k}}}^{w}\otimes \int_{t\in [{{t}_{k}},{{t}_{k+1}}]}{\frac{1}{2}\Omega ({{\omega }_{t}}-{{b}_{{{\omega }_{t}}}}-{{n}_{\omega }})q_{t}^{{{b}_{k}}}dt} \end{align}

式中：

\Omega (\omega ) = \left[ \begin{matrix} -{{[\omega ]}_{\times }} & \omega \\ -{{\omega }^{T}} & 0 \\ \end{matrix} \right],{{[\omega ]}_{\times }} = \left[ \begin{matrix} 0 & -{{\omega }_{z}} & {{\omega }_{y}} \\ {{\omega }_{z}} & 0 & -{{\omega }_{x}} \\ -{{\omega }_{y}} & {{\omega }_{x}} & 0 \\ \end{matrix} \right]

$\vartriangle {{t}_{k}}$ $[{{t}_{k}},{{t}_{k+1}}]$ $R_{t}^{w}$ $t$ $q_{t}^{{{b}_{k}}}$ $t$ 时刻下从IMU坐标系到世界坐标系的旋转。于是我们得到了连续时间上IMU模型的运动学方程，当已知初始状态和IMU的自身偏置以及噪声时可以通过递推的方式来计算下一时刻系统的位置、速度和旋转。

A.3 IMU预积分

${{b}_{k}}$ $p_{{{b}_{k}}}^{w}$ $v_{{{b}_{k}}}^{w}$ $q_{b_{k}}^{w}$ ，当这些起始状态发生改变时，就需要重新进行状态传播。而在基于优化的算法中，每个关键帧时刻的状态需要频繁调整，所以就需要频繁地重新积分，这样会浪费大量的计算资源。IMU 预积分就是为了避免这种计算资源上的浪费。

${{b}_{k}}$ 。如图所示，虚线绿色框表示的是关键帧对应的IMU数据，深蓝色框为对应的预积分结果。

imu_preintegration

A.3.1 连续时间下图像帧间IMU状态方程

$R_{w}^{{{b}_{k}}}$ ，我们将得到以下等式：

\begin{align} R_{w}^{{{b}_{k}}}p_{{{b}_{k+1}}}^{w} &= R_{w}^{{{b}_{k}}}(p_{{{b}_{k}}}^{w}+v_{{{b}_{k}}}^{w}\vartriangle {{t}_{k}}-\frac{1}{2}{{g}^{w}}\vartriangle t_{k}^{2})+\alpha _{{{b}_{k+1}}}^{{{b}_{k}}} \\ R_{w}^{{{b}_{k}}}v_{{{b}_{k+1}}}^{w} &= R_{w}^{{{b}_{k}}}(v_{{{b}_{k}}}^{w}-{{g}^{w}}\vartriangle {{t}_{k}})+\beta _{{{b}_{k+1}}}^{{{b}_{k}}} \\ q_{w}^{{{b}_{k}}}\otimes q_{{{b}_{k+1}}}^{w} &= \gamma _{{{b}_{k+1}}}^{{{b}_{k}}} \end{align}

其中：

\begin{align} \alpha _{{{b}_{k+1}}}^{{{b}_{k}}} &= \iint_{_{t\in [{{t}_{k}},{{t}_{k+1}}]}}{(R_{t}^{{{b}_{k}}}({{a}_{t}}-{{b}_{{{a}_{t}}}}-{{n}_{a}}))d{{t}^{2}}} \\ \beta _{{{b}_{k+1}}}^{{{b}_{k}}} &= \int_{t\in [{{t}_{k}},{{t}_{k+1}}]}{(R_{t}^{w}({{a}_{t}}-{{b}_{{{a}_{t}}}}-{{n}_{a}}))dt} \\ \gamma _{{{b}_{k+1}}}^{{{b}_{k}}} &= \int_{t\in [{{t}_{k}},{{t}_{k+1}}]}{\frac{1}{2}\Omega ({{\omega }_{t}}-{{b}_{{{\omega }_{t}}}}-{{n}_{\omega }})\gamma _{t}^{{{b}_{k}}}dt} \end{align}

$b_{k}$ $b_{k+1}$ $b_{k}$ 的相对运动量，即使在优化过程中调整了视觉关键帧的位置、速度和旋转等状态进行调整，也并不会对积分项产生影响，从而通过这种方法避免重复积分。

$\alpha _{t}^{{{b}_{k}}}$ $\beta _{t}^{{{b}_{k}}}$ $\gamma _{t}^{{{b}_{k}}}$ ${{b}_{{{a}_{t}}}}$ ${{b}_{{{\omega }_{t}}}}$ $\hat{\beta }_{t}^{{{b}_{k}}}$ $\hat{\beta }_{t}^{{{b}_{k}}}$ $\hat{\gamma }_{t}^{{{b}_{k}}}$ ${{\hat{b}}_{{{a}_{t}}}}$ ${{\hat{b}}_{{{\omega }_{t}}}}$ $\delta \alpha _{t}^{{{b}_{k}}}$ $\delta \beta _{t}^{{{b}_{k}}}$ $\delta \gamma _{t}^{{{b}_{k}}}$ $\delta {{b}_{{{a}_{t}}}}$ $\delta {{b}_{{{\omega }_{t}}}}$ 表示，其中噪声是误差主要来源，其累计过程也是由误差状态方程来描述。真实状态、标称状态与误差状态之间的关系有如下关系：

\begin{align} \alpha_{t}^{b_{k}}&=\hat{\alpha}_{t}^{b_{k}}+\delta \alpha_{t}^{b_{k}} \\ \beta_{t}^{b_{k}}&=\hat{\beta}_{t}^{b_{k}}+\delta \beta_{t}^{b_{k}} \\ \gamma_{t}^{b_{k}}&=\hat{\gamma}_{t}^{b_{k}} \otimes \delta \gamma_{t}^{b_{k}} \\ &=\hat{\gamma}_{t}^{b_{k}} \otimes \left[ \begin{array}{c}{1} \\ {\frac{1}{2} \delta \theta_{t}^{b_{k}}}\end{array}\right] \\ b_{a_{t}}&=\hat{b}_{a_{t}}+\delta b_{a_{t}} \\ b_{\omega_{t}}&=\hat{b}_{\omega_{t}}+\delta b_{\omega_{t}} \end{align}

其中真实状态的状态方程可以表示为：

\begin{align} \dot{\alpha }_{t}^{{{b}_{k}}} &= \beta _{t}^{{{b}_{k}}} \\ \dot{\beta }_{t}^{{{b}_{k}}} &= R_{t}^{w}({{a}_{t}}-{{b}_{{{a}_{t}}}}-{{n}_{a}}) \\ \dot{\gamma }_{t}^{{{b}_{k}}} &= \frac{1}{2}\gamma _{t}^{{{b}_{k}}}\otimes ({{\omega }_{t}}-{{b}_{{{\omega }_{t}}}}-{{n}_{\omega }}) \\ &= \frac{1}{2}\Omega ({{\omega }_{t}}-{{b}_{{{\omega }_{t}}}}-{{n}_{\omega }})\gamma _{t}^{{{b}_{k}}} \\ {{\dot{b}}_{{{a}_{t}}}} &= {{n}_{{{a}_{t}}}} \\ {{\dot{b}}_{{{\omega }_{t}}}} &= {{n}_{{{\omega }_{t}}}} \end{align}

同样的我们可以得到标称状态下的状态方程:

\begin{align} \dot{\hat{\alpha }}_{t}^{{{b}_{k}}} &= \hat{\beta }_{t}^{{{b}_{k}}} \\ \dot{\hat{\beta }}_{t}^{{{b}_{k}}} &= \hat{R}_{t}^{{{b}_{k}}}({{a}_{t}}-{{\hat{b}}_{{{a}_{t}}}}) \\ \dot{\hat{\gamma }}_{t}^{{{b}_{k}}} &= \frac{1}{2}\hat{\gamma }_{t}^{{{b}_{k}}}\otimes ({{\omega }_{t}}-{{\hat{b}}_{{{\omega }_{t}}}}) \\ &= \frac{1}{2}\Omega ({{\omega }_{t}}-{{\hat{b}}_{{{\omega }_{t}}}})\hat{\gamma }_{t}^{{{b}_{k}}} \\ {{\dot{\hat{b}}}_{{{a}_{t}}}} &= \mathbf{0} \\ {{\dot{\hat{b}}}_{{{\omega }_{t}}}} &= \mathbf{0}\label{eq-bwt} \end{align}

其中上面两个等式表明了，在我们使用IMU信息时，一般认为加速度计和陀螺仪的偏置是保持不变的，只有在得到优化时，它的值才会改变，并且改变之后要重新进行预积分。误差状态方程表示的是小信号的传播，并且不会过参数化，参数的数量正好和系统的自由度是一致的，这里主要针对四元数进行了参数化，并且误差状态基本是在零点附近进行传播，这能保证系统不会出现奇异性，并且具有良好的线性化性质。下面给出误差状态的状态方程：

\begin{align} \delta \dot{\alpha }_{t}^{{{b}_{k}}} &= \delta \beta _{t}^{{{b}_{k}}} \\ \delta \dot{\beta }_{t}^{{{b}_{k}}} &= -\hat{R}_{t}^{{{b}_{k}}}{{[{{a}_{t}}-{{\hat{b}}_{{{a}_{t}}}}]}_{\times }}\delta \theta _{t}^{{{b}_{k}}}-\hat{R}_{t}^{{{b}_{k}}}\delta {{b}_{{{a}_{t}}}}-\hat{R}_{t}^{{{b}_{k}}}{{n}_{a}} \\ \delta \dot{\theta }_{t}^{{{b}_{k}}} &= -{{[{{\omega }_{t}}-{{\hat{b}}_{{{\omega }_{t}}}}]}_{\times }}\delta \theta _{t}^{{{b}_{k}}}-\delta {{b}_{{{\omega }_{t}}}}-{{n}_{\omega }} \\ \delta {{\dot{b}}_{{{a}_{t}}}} &= {{n}_{{{b}_{a}}}} \\ \delta {{\dot{b}}_{{{\omega }_{t}}}} &= {{n}_{{{b}_{\omega }}}} \end{align}

将误差状态写成矩阵的形式如下，

\begin{align} \left[ \begin{matrix} \delta \dot{\alpha }_{t}^{{{b}_{k}}} \\ \delta \dot{\beta }_{t}^{{{b}_{k}}} \\ \delta \dot{\theta }_{t}^{{{b}_{k}}} \\ \delta {{{\dot{b}}}_{{{a}_{t}}}} \\ \delta {{{\dot{b}}}_{{{\omega }_{t}}}} \\ \end{matrix} \right] &= \left[ \begin{matrix} 0 & I & 0 & 0 & 0 \\ 0 & 0 & -\hat{R}_{t}^{{{b}_{k}}}{{[{{a}_{t}}-{{{\hat{b}}}_{{{a}_{t}}}}]}_{\times }} & \hat{R}_{t}^{{{b}_{k}}} & 0 \\ 0 & 0 & -{{[{{\omega }_{t}}-{{{\hat{b}}}_{{{\omega }_{t}}}}]}_{\times }} & 0 & -I \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ \end{matrix} \right]\left[ \begin{matrix} \delta \alpha _{t}^{{{b}_{k}}} \\ \delta \beta _{t}^{{{b}_{k}}} \\ \delta \theta _{t}^{{{b}_{k}}} \\ \delta {{b}_{{{a}_{t}}}} \\ \delta {{b}_{{{\omega }_{t}}}} \\ \end{matrix} \right]+\left[ \begin{matrix} 0 & 0 & 0 & 0 \\ -\hat{R}_{t}^{{{b}_{k}}} & 0 & 0 & 0 \\ 0 & -I & 0 & 0 \\ 0 & 0 & I & 0 \\ 0 & 0 & 0 & I \\ \end{matrix} \right]\left[ \begin{matrix} {{n}_{a}} \\ {{n}_{\omega }} \\ {{n}_{{{b}_{a}}}} \\ {{n}_{{{b}_{\omega }}}} \\ \end{matrix} \right] \\ &= {{F}_{t}}\delta z_{t}^{{{b}_{k}}}+{{G}_{t}}{{n}_{t}} \end{align}

至此我们得到了IMU在连续状态下的误差状态方程：

\begin{align} \delta \dot{z}_{t}^{{{b}_{k}}}=F_{t}^{15\times 15}\delta z_{t}^{{{b}_{k}}}+G_{t}^{15\times 12}{{n}_{t}} \end{align}

A.3.2离散时间下图像帧间位姿估计}

$\delta \dot{z}_{t}^{{{b}_{k}}}=\underset{\Delta t\to 0}{\mathop{\lim }}\,\frac{\delta z_{t+\Delta t}^{{{b}_{k}}}-\delta z_{t}^{{{b}_{k}}}}{\Delta t}$ ，展开可得到：

\begin{align} \delta z_{t+\Delta t}^{{{b}_{k}}}=\delta z_{t}^{{{b}_{k}}}+\delta \dot{z}_{t}^{{{b}_{k}}}\Delta t=(I+F_{t}^{15\times 15}\Delta t)\delta z_{t}^{{{b}_{k}}}+(G_{t}^{15\times 12}\Delta t){{n}_{t}} \end{align}

上式的意义是表示下一个时刻的IMU的测量误差与上一时刻的成线性关系，这样就可以根据当前时刻的值，预测出下一时刻的均值和协方差，等式给出的是均值预测，其中协方差预测公式如下：

\begin{align} P_{t+\Delta t}^{{{b}_{k}}}=(I+F_{t}^{15\times 15}\Delta t)P_{t}^{{{b}_{k}}}{{(I+F_{t}^{15\times 15}\Delta t)}^{T}}+(G_{t}^{15\times 12}\Delta t)Q{{(G_{t}^{15\times 12}\Delta t)}^{T}} \end{align}

$P_{t}^{{{b}_{k}}}$ $Q$ 表示噪声项的对角协方差矩阵：

\begin{align} {{Q}^{12\times 12}}=\left[ \begin{matrix} \sigma _{a}^{2} & 0 & 0 & 0 \\ 0 & \sigma _{\omega }^{2} & 0 & 0 \\ 0 & 0 & \sigma _{{{b}_{a}}}^{2} & 0 \\ 0 & 0 & 0 & \sigma _{{{b}_{\omega }}}^{2} \\ \end{matrix} \right] \end{align}

另外根据等式可以获得误差项的Jacobian的迭代公式：

\begin{align} J_{t+\Delta t}^{{{b}_{k}}}=(I+F_{t}^{15\times 15}\Delta t)J_{t}^{{{b}_{k}}} \end{align}

$J_{t}^{{{b}_{k}}}=I$ 。

${{b}_{k+1}}$ ${{J}^{{{b}_{k+1}}}}$ ${{P}^{{{b}_{k+1}}}}$ $\alpha _{{{b}_{k+1}}}^{{{b}_{k}}}$ $\beta _{{{b}_{k+1}}}^{{{b}_{k}}}$ $\gamma _{{{b}_{k+1}}}^{{{b}_{k}}}$ ${{b}_{k+1}}$ $\delta {{b}_{{{\omega }_{k}}}}$ 和陀螺仪偏置误差的一阶近似可以近似表示为：

\begin{align} \alpha_{b_{k+1}}^{b_{k}}&=\hat{\alpha}_{b_{k+1}}^{b_{k}}+J_{\delta b_{a_{k}}}^{\delta \alpha_{b_{k+1}}^{b_{k+1}}} \delta b_{a_{k}}+J_{\delta b_{\omega_{k}}}^{\delta \alpha_{b_{k+1}}^{b_{k}}} \delta b_{\omega_{k}} \\ \beta_{b_{k+1}}^{b_{k}}&=\hat{\beta}_{b_{k+1}}^{b_{k}}+J_{\delta b_{a_{k}}}^{\delta \beta_{b_{k+1}}^{b_{k+1}}} \delta b_{a_{k}}+J_{\delta b_{\omega_{k}}}^{\delta \alpha_{b_{k+1}}^{b_{k}}} \delta b_{\omega_{k}} \\ \gamma_{b_{k+1}}^{b_{k}}&=\hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes \left[ \begin{array}{c}{1} \\ {\frac{1}{2} J_{\delta b_{v_{k}}}^{\delta b_{b_{k}}^{k_{k+1}}} \delta b_{w_{k}}}\end{array}\right] \label{eq_4_19c} \end{align}

$J_{\delta {{b}_{{{a}_{k}}}}}^{\delta \alpha _{{{b}_{k+1}}}^{{{b}_{k}}}}$ $J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \alpha _{{{b}_{k+1}}}^{{{b}_{k}}}}$ $J_{\delta {{b}_{{{a}_{k}}}}}^{\delta \beta _{{{b}_{k+1}}}^{{{b}_{k}}}}$ $J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \theta _{{{b}_{k+1}}}^{{{b}_{k}}}}$ $J_{\delta {{b}_{{{\omega }_{k}}}}}^{\delta \theta _{{{b}_{k+1}}}^{{{b}_{k}}}}$ ${{J}^{{{b}_{k+1}}}}$ 的子块矩阵，可以直接从对应的位置获取，当加速度和陀螺仪的偏置发生微小的变化时，可以根据上式对预积分进行修正，从而避免了重复积分。至此，可以写出预积分形势下的IMU测量模型：

\begin{align} \left[ \begin{matrix} \hat{\alpha }_{{{b}_{k+1}}}^{{{b}_{k}}} \\ \hat{\gamma }_{{{b}_{k+1}}}^{{{b}_{k}}} \\ \hat{\beta }_{{{b}_{k+1}}}^{{{b}_{k}}} \\ 0 \\ 0 \\ \end{matrix} \right]=\left[ \begin{matrix} R_{w}^{{{b}_{k}}}(p_{{{b}_{k+1}}}^{w}-p_{{{b}_{k}}}^{w}-v_{{{b}_{k}}}^{w}\vartriangle {{t}_{k}}+\frac{1}{2}{{g}^{w}}\vartriangle t_{k}^{2}) \\ {{(q_{w}^{{{b}_{k}}})}^{-1}}\otimes q_{{{b}_{k+1}}}^{w} \\ R_{w}^{{{b}_{k}}}(v_{{{b}_{k+1}}}^{w}-v_{{{b}_{k}}}^{w}+{{g}^{w}}\vartriangle {{t}_{k}}) \\ {{b}_{a{{b}_{k+1}}}}-{{b}_{a{{b}_{k}}}} \\ {{b}_{\omega {{b}_{k+1}}}}-{{b}_{\omega {{b}_{k}}}} \\ \end{matrix} \right] \end{align}

这个模型将为后端优化提供IMU信息的优化目标函数。

附录B FeatureManager类

B.1. Feature管理部分


xxxxxxxxxx
#ifndef FEATURE_MANAGER_H
#define FEATURE_MANAGER_H
#include <list>
#include <algorithm>
#include <vector>
#include <numeric>
using namespace std;
#include <eigen3/Eigen/Dense>
using namespace Eigen;
#include <ros/console.h>
#include <ros/assert.h>
#include "parameters.h"
class FeaturePerFrame // 每个路标点在一张图像中的信息
{
  public:
    FeaturePerFrame(const Vector3d &_point)
    {
        z = _point(2);
        point = _point / z;
    }
    Vector3d point;
    double z;
    bool is_used;
    double parallax;
    MatrixXd A;
    VectorXd b;
    double dep_gradient;
};
class FeaturePerId //每个路标点由连续多帧观测到，但都有独立的ID
{
  public:
    const int feature_id;
    int start_frame;
    vector<FeaturePerFrame> feature_per_frame;
    int used_num;
    bool is_outlier;
    bool is_margin;
    double estimated_depth;
    int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;
    Vector3d gt_p;
    FeaturePerId(int _feature_id, int _start_frame)
        : feature_id(_feature_id), start_frame(_start_frame),
          used_num(0), estimated_depth(-1.0), solve_flag(0)
    {
    }
    int endFrame();
};
class FeatureManager //特征管理类
{
  public:
    FeatureManager(Matrix3d _Rs[]);
    void setRic(Matrix3d _ric[]);
    void clearState();
    int getFeatureCount();
    bool addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Vector3d>>> &image);
    void debugShow();
    vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r);
    //void updateDepth(const VectorXd &x);
    void setDepth(const VectorXd &x);
    void removeFailures();
    void clearDepth(const VectorXd &x);
    VectorXd getDepthVector();
    void triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]);
    void removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P);
    void removeBack();
    void removeFront(int frame_count);
    void removeOutlier();
    list<FeaturePerId> feature; //滑动窗口内的所有特征点
    int last_track_num;
  private:
    double compensatedParallax2(const FeaturePerId &it_per_id, int frame_count);
    const Matrix3d *Rs;
    Matrix3d ric[NUM_OF_CAM];
};
#endif

B.2 重要函数解释

B.2.1 addFeatureCheckParallax

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Vector3d>>> &image)

参数	含义
frame_count	相机帧数，它不大于滑窗最大数量
image	特征点集合： key: feature_id ； value: vector< key: camera_id, value: point>
返回值	1：是关键帧；0：非关键帧

伪代码如下：

last_track_num=0; frame_count；parallax_num

1.遍历当前帧特征并将其归类到feature中，feature为list类型，这里主要判断新老特征。
2.判断当前帧数和last_track_num, 如果说帧数不足或者跟踪效果太差则返回true
3.遍历跟踪较久的特征计算视差 compensatedParallax2
4.如果总视差为0，返回真
5.判断视差是否满足最小视差要求，如果视差足够说明为关键帧

下面分别对以上做出说明

B.2.1.1

第10-13行其实是对list查找，如果这个特征id已经存在了，那么说明是一个老特征，那么直接对vector追加就好了，并且last_track_num++,这个数值表示对上一帧的跟踪情况，越大越好；如果是新的特征，那么需要创建一个新的FeaturePerId的实例，并将其填充。

B.2.1.2

判断当前帧数和last_track_num, 如果说帧数不足或者跟踪效果太差则返回true。从代码来看可以说明前两帧肯定是关键帧。


xxxxxxxxxx
    /// 帧数不足或者说跟踪效果太差
    if (frame_count < 2 || last_track_num < 20)
        return true;

B.2.1.3

遍历跟踪较久的特征计算视差 compensatedParallax2.

计算的是倒数第二帧和倒数第三帧的视差，这个就是直接计算uv向量值


xxxxxxxxxx
    //计算视差，second last和third last
    for (auto &it_per_id : feature)
    {
        /// 其中判断条件选择的是跟踪较为久的特征, 至少要能在上一帧存在
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            ROS_DEBUG_STREAM("start_frame: " << it_per_id.start_frame << " tracking times: " <<
                           int(it_per_id.feature_per_frame.size()) << " frame_count: " << frame_count);
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;
        }
    }

这里用图示来表示：其实应该是倒数第二帧和倒数第三帧都存在才会被选作用来计算视差的。

B.2.1.4

如果总视差为0，返回真，绝对静止情况下也作为新的关键帧。

B.2.1.5

判断视差是否满足最小视差要求，如果视差足够说明为关键帧。

B.2.2 compensatedParallax2函数

这个函数在B.2.1addFeatureCheckParallax使用了，主要是计算两帧的视差，其实计算的是倒数第三帧和倒数第二帧的uv的模长。

附录C 三角化

三角化是通过多帧相机对同一个点的观测计算出特征点在世界坐标系下的绝对3D坐标的一个过程，简单说就是相机位姿已知，需要求解特征点的3D位置。

triangulation

${{P}^{W}}={{\left[ \begin{matrix} X & Y & Z & 1 \\ \end{matrix} \right]}^{T}}$ ${{x}_{1}},{{x}_{2}},{{x}_{3}}$ $\mathbf{M}$ 矩阵：

\begin{equation} \begin{aligned} \mathbf{M}_{W}^{{{C}_{i}}}=[R_{W}^{{{C}_{i}}}|t_{W}^{{{C}_{i}}}] \end{aligned} \end{equation}

$i$ $i$ ${{\mathbf{M}}_{i}}$ 。为简化推导过程，我们仅以两帧为例，根据投影矩阵可知：

\begin{align} {{x}_{1}}={{\mathbf{M}}_{1}}{{P}^{W}} \\ {{x}_{2}}={{\mathbf{M}}_{1}}{{P}^{W}} \end{align}

将上述两式对自身进行叉乘计算，由于向量对自身叉乘结果为0，于是得到：

\begin{align} {{[{{x}_{1}}]}_{\times }}{{\mathbf{M}}_{1}}{{P}^{W}}=0 \\ {{[{{x}_{2}}]}_{\times }}{{\mathbf{M}}_{2}}{{P}^{W}}=0 \end{align}

${{\mathbf{M}}_{1}}{{P}^{W}}$ 按行展开，可以得到：

\begin{equation} \begin{aligned} \left[ \begin{matrix} 0 & -1 & {{v}_{1}} \\ 1 & 0 & -{{u}_{1}} \\ -{{v}_{1}} & {{u}_{1}} & 0 \\ \end{matrix} \right]\left[ \begin{matrix} \mathbf{M}_{1}^{1}{{P}^{W}} \\ \mathbf{M}_{1}^{2}{{P}^{W}} \\ \mathbf{M}_{1}^{3}{{P}^{W}} \\ \end{matrix} \right]=0 \end{aligned} \end{equation}

展开可得：

\begin{align} ({{u}_{1}}\mathbf{M}_{1}^{3}-\mathbf{M}_{1}^{1}){{P}^{W}} &= 0 \label{3-40a} \\ ({{v}_{1}}\mathbf{M}_{1}^{3}-\mathbf{M}_{1}^{2}){{P}^{W}} &= 0 \label{3-40b} \\ ({{u}_{1}}\mathbf{M}_{1}^{2}-{{v}_{1}}\mathbf{M}_{1}^{1}){{P}^{W}} &= 0 \label{3-40c} \end{align}

其中式子(61)(62)与(63)是线性相关的，所以其自由度只有2个，我们取前两个并将相机2下的投影合并可以得到：

\begin{equation} \begin{aligned} \left[ \begin{matrix} {{u}_{1}}\mathbf{M}_{1}^{3}-\mathbf{M}_{1}^{1} \\ {{v}_{1}}\mathbf{M}_{1}^{3}-\mathbf{M}_{1}^{2} \\ {{u}_{2}}\mathbf{M}_{2}^{3}-\mathbf{M}_{2}^{1} \\ {{v}_{2}}\mathbf{M}_{2}^{3}-\mathbf{M}_{2}^{2} \\ \end{matrix} \right]{{P}^{W}}=0 \end{aligned} \end{equation}

$Ax=0$ ${{P}^{W}}$ 。