【SLAM】VINS-MONO解析——vins_estimator流程

5.vins_estimator

基本上VINS里面绝大部分功能都在这个package下面，包括IMU数据的处理(前端)，初始化(我觉得可能属于是前端)，滑动窗口(后端)，非线性优化(后端)，关键帧的选取(部分内容)(前端)。我第一次看的时候，总是抱有一个疑问，就是为什么把这么多内容全都放在这一个node里面。为了回答这个问题，那么首先先搞清楚vins_estimator里面分别具体都是什么，为什么要有这些数据结构/函数，这些函数是怎样工作的。

这个package下面主要以下文件：
factor——主要用于非线性优化对各个参数块和残差块的定义，VINS采用的是ceres，所以这部分需要对一些状态量和因子进行继承和重写。
initial——主要用于初始化，VINS采用的初始化策略是先SfM进行视觉初始化，再与IMU进行松耦合。
estimator.cpp——vins_estimator需要的所有函数都放在这里，是一个鸿篇巨制。
estimator_node.cpp——vins_estimator的入口，是一个ROS的node，实际上运行的是这个cpp文件。
feature_manager.cpp——负责管理滑窗内的所有特征点。
parameters.cpp——读取参数，是一个辅助函数。
utility——里面放着用于可视化的函数和tictok计时器。

然后就是要注意CmakeLists.txt和package.xml文件的写法，这两个文件也是相当于套公式，写错了就不能实现正常的ROS功能了。

各个部分的讲解如下链接：

【SLAM】VINS-MONO解析——综述

【SLAM】VINS-MONO解析——feature_tracker

【SLAM】VINS-MONO解析——IMU预积分

【SLAM】VINS-MONO解析——vins_estimator

【SLAM】VINS-MONO解析——初始化(理论部分)

【SLAM】VINS-MONO解析——初始化(代码部分)

【SLAM】VINS-MONO解析——后端优化(理论部分)

【SLAM】VINS-MONO解析——后端优化(代码部分)

【SLAM】VINS-MONO解析——sliding window

【SLAM】VINS-MONO解析——回环检测

【SLAM】VINS-Fusion解析——流程

【SLAM】VINS-MONO解析——对vins-mono的修改使流程逻辑更清晰

【SLAM】VINS-MONO解析——基于vins-mono的slam系统开发

5.1 estimator_node

5.1.1 main()主入口

1、ROS初始化、设置句柄

ros::init(argc, argv, "vins_estimator");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

2、读取参数，设置状态估计器参数

readParameters(n);
estimator.setParameter();

这个estimator.setParameter()需要注意一下，它在estimator.cpp里面：

void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

它读取了每一个相机到IMU坐标系的旋转/平移外参数和非线性优化的重投影误差部分的信息矩阵。

3、发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出

registerPub(n);

这个函数定义在utility/visualization.cpp里面：void registerPub(ros::NodeHandle &n)。

4、订阅IMU_TOPIC，执行imu_callback

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());

这个回调函数比较重要，如下代码所示：

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();
    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();
    last_imu_t = imu_msg->header.stamp.toSec();
    {
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg);
        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_callback主要干了3件事，
第一件事就是往imu_buf里放IMU数据，缓存起来；
第二件事就是IMU预积分获得当前时刻的PVQ，见4.1.2；
第三件事就是如果当前处于非线性优化阶段的话，需要把第二件事计算得到的PVQ发布到rviz里去，见utility/visualization.cpp的pubLatestOdometry()函数。

在这个函数里，第一次出现了上锁操作，因为这个程序是多线程，需要考虑线程安全问题，对imu_buf的操作是一个生产者-消费者模型，加入和读取的时候不可以中断，必须加锁以保证数据安全，后续部分有多个地方都有上锁操作。
这部分出现了几个新的数据结构，解释如下：

数据结构: 
tmp_Q，tmp_P，tmp_V：当前时刻的PVQ
header：当前时刻时间戳        

5、订阅/feature_tracker/feature，执行feature_callback

ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息，见3.3.6.。
feature_callback就只干了一件事，就是把cur帧的所有特征点放到feature_buf里，同样需要上锁。注意，cur帧的所有特征点都是整合在一个数据里的，也就是sensor_msgs::PointCloudConstPtr &feature_msg。

6、订阅/feature_tracker/restart，执行restart_callback

ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);

restart_callback干了一件事，就是把所有状态量归零，把buf里的数据全部清空。

7、订阅/pose_graph/match_points，执行relocalization_callback

ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

8、创建VIO主线程process()(VINS核心！)

std::thread measurement_process{process};

这一部分是最重要的，包含了VINS绝大部分内容和最难的内容。

5.1.2 process()主线程
1、对imu和图像数据进行对齐并配对
这一部分卡了我很久很久，终于最近彻底弄明白了。

 std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
              
        std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]{return (measurements = getMeasurements()).size() != 0; });
        lk.unlock();
        

一上来就是一个非常复杂的数据结构measurements！在这里先解释一波。

数据结构: measurements
1、首先，measurements他自己就是一个vector；
2、对于measurements中的每一个measurement，又由2部分组成；
3、第一部分，由sensor_msgs::ImuConstPtr组成的vector；
4、第二部分，一个sensor_msgs::PointCloudConstPtr；
5、这两个sensor_msgs见3.1-6部分介绍。
6、为什么要这样配对(一个PointCloudConstPtr配上若干个ImuConstPtr)？
因为IMU的频率比视觉帧的发布频率要高，所以说在这里，需要把一个视觉帧和之前的一串IMU帧的数据配对起来。 
                                                    

容器measurements有了，接下来就是配对IMU和视觉帧的数据，放到容器里去。配对过程也需要上锁，而且是一个条件锁。作者在这里用了一个lambda表达式，也就是说，在return里面的部分是false时，保持上锁状态，继续配对数据；如果return里面时true，说明配对完成，释放锁，measurements完成了，以供后续使用。

接下来分析一下measurements()具体的功能：

//这个函数的作用就是 把图像帧 和 对应的IMU数据们 配对起来,而且IMU数据时间是在图像帧的前面
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
    while (true)
    {   //边界判断：数据取完了，说明配对完成
        if (imu_buf.empty() || feature_buf.empty())
            return measurements;
//边界判断：IMU buf里面所有数据的时间戳都比img buf第一个帧时间戳要早，说明缺乏IMU数据，需要等待IMU数据
        if (!(imu_buf.back()->header.stamp.toSec() > feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            sum_of_wait++;//统计等待的次数
            return measurements;
        }
        
        //边界判断：IMU第一个数据的时间要大于第一个图像特征数据的时间(说明图像帧有多的)
        if (!(imu_buf.front()->header.stamp.toSec() < feature_buf.front()->header.stamp.toSec() + estimator.td))
        {
            feature_buf.pop();
            continue;
        }
        //核心操作：装入视觉帧信息
        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();
        feature_buf.pop();
        //核心操作：转入IMU信息
        std::vector<sensor_msgs::ImuConstPtr> IMUs;       
        while (imu_buf.front()->header.stamp.toSec() < img_msg->header.stamp.toSec() + estimator.td)
        {
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }
        //注意：把最后一个IMU帧又放回到imu_buf里去了
        //原因：最后一帧IMU信息是被相邻2个视觉帧共享的
        IMUs.emplace_back(imu_buf.front());
 
        measurements.emplace_back(IMUs, img_msg);
    }
    return measurements;
}

2、对IMU数据的处理

m_estimator.lock(); 
for (auto &measurement : measurements)//2、对measurements中的每一个measurement (IMUs,IMG)组合进行操作
{//2.1、对于measurement中的每一个imu_msg，计算dt并执行processIMU()
    auto img_msg = measurement.second;
    double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0;
   
    for (auto &imu_msg : measurement.first)
    {
        double t = imu_msg->header.stamp.toSec();
        double img_t = img_msg->header.stamp.toSec() + estimator.td;//相机和IMU同步校准得到的时间差
        //对于大多数情况，IMU的时间戳都会比img的早，此时直接选取IMU的数据就行
        if (t <= img_t)  //http://wiki.ros.org/sensor_msgs
        {   
            if (current_time < 0)
                current_time = t;
            double dt = t - current_time;
            ROS_ASSERT(dt >= 0);
            current_time = t;
            dx = imu_msg->linear_acceleration.x;
            dy = imu_msg->linear_acceleration.y;
            dz = imu_msg->linear_acceleration.z;
            rx = imu_msg->angular_velocity.x;
            ry = imu_msg->angular_velocity.y;
            rz = imu_msg->angular_velocity.z;
  //这里干了2件事，IMU粗略地预积分，然后把值传给一个新建的IntegrationBase对象
            estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//进行IMU预积分
        }
        
        //对于处于边界位置的IMU数据，是被相邻两帧共享的，而且对前一帧的影响会大一些，在这里，对数据线性分配
        else//每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的(出现次数少)
        {
            double dt_1 = img_t - current_time; //current_time < img_time < t
            double dt_2 = t - img_t;
            current_time = img_t;
            ROS_ASSERT(dt_1 >= 0);
            ROS_ASSERT(dt_2 >= 0);
            ROS_ASSERT(dt_1 + dt_2 > 0);
			 //以下操作其实就是简单的线性分配
            double w1 = dt_2 / (dt_1 + dt_2);
            double w2 = dt_1 / (dt_1 + dt_2);
            dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
            dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
            dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
            rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
            ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
            rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
            estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
        }
        

这个if-else的操作技巧非常值得学习！这一部分的核心代码是processIMU()，它在estomator.cpp里面，它的作用就是IMU预积分，对应着4.1.2的内容。

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
  /*这一段作用就是就是给以下数据提供初始值/初始化：
   * 
   *pre_integrations[frame_count]
   *dt_buf[frame_count]
   *linear_acceleration_buf[frame_count]
   *angular_velocity_buf[frame_count]
   *Rs[frame_count]
   *PS[frame_count]
   *Vs[frame_count] 
   * 
   * TODO 关于frame_count的更新，目前只在process_img里的solver_flag == INITIAL这里看到?
   * 
   */
    //边界判断：如果当前帧不是第一帧IMU，那么就把它看成第一个IMU，而且把他的值取出来作为初始值
    if (!first_imu)//force to be first_IMU
    {
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }
    //边界判断：如果当前IMU帧没有构造IntegrationBase，那就构造一个，后续会用上
    if (!pre_integrations[frame_count])
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    //核心操作
    if (frame_count != 0)
    {
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;
        
        //注意啊，这块对j的操作看似反智，是因为j时刻的值都拷贝了j-1时刻的值！！
        //第一次使用实际上就是使用的是j-1时刻的值，所以在这些地方写上j-1是没有关系的！
        //noise是zero mean Gauss，在这里忽略了
        //TODO 把j改成j-1，看看效果是一样
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//world    
        //下面都采用的是中值积分的传播方式，noise被忽略了
        //TODO 把j改成j-1，看看效果是一样
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        //PPT 1-27
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;
    }
    //数据传递
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}

关于IMU预积分部分，见4.1.2。这里出现了几个新的数据结构，分析如下：

数据结构:
1、Rs[frame_count]，Ps[frame_count]，Vs[frame_count]:是从IMU系转到world系的PVQ，数据是由IMU预积分得到的，目前在这里存放的是没有用bias修正过的值。
2、frame_count:这个值让我很疑惑，它只在processImage()里有过++操作，而且在estimator.hpp声明的时候，没有加上static关键字。它是在h文件中声明，在cpp文件里初始化的，后续需要再关注一下。
3、dt_buf，linear_acceleration_buf，angular_velocity_buf：帧数和IMU测量值的缓存，而且它们是对齐的。
3、pre_integrations[frame_count]，它是IntegrationBase的一个实例，在factor/integration_base.h中定义，它保存着frame_count帧中所有跟IMU预积分相关的量，包括F矩阵，Q矩阵，J矩阵等。    

3、重定位/回环检测操作
回到estimator_node.cpp的process()函数。接下来的代码的作用是，在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()

        // set relocalization frame
        sensor_msgs::PointCloudConstPtr relo_msg = NULL;
   //2.2、在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
        while (!relo_buf.empty())
        {
            relo_msg = relo_buf.front();
            relo_buf.pop();
        }
        
        if (relo_msg != NULL)
        {
            vector<Vector3d> match_points;
            double frame_stamp = relo_msg->header.stamp.toSec();
            for (unsigned int i = 0; i < relo_msg->points.size(); i++)
            {
                Vector3d u_v_id;
                u_v_id.x() = relo_msg->points[i].x;
                u_v_id.y() = relo_msg->points[i].y;
                u_v_id.z() = relo_msg->points[i].z;
                match_points.push_back(u_v_id);
            }
            Vector3d relo_t(relo_msg->channels[0].values[0], relo_msg->channels[0].values[1], relo_msg->channels[0].values[2]);
            Quaterniond relo_q(relo_msg->channels[0].values[3], relo_msg->channels[0].values[4], relo_msg->channels[0].values[5], relo_msg->channels[0].values[6]);
            Matrix3d relo_r = relo_q.toRotationMatrix();
            int frame_index;
            frame_index = relo_msg->channels[0].values[7];
            estimator.setReloFrame(frame_stamp, frame_index, match_points, relo_t, relo_r);//设置重定位帧
        }
        

4、对img信息进行处理(核心！)
(1)一开始，就定义了一个新的数据结构，解释一下：

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;

数据结构: map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
1、虽然它叫image，但是这个容器里面存放的信息是每一个特征点的！
2、索引值是feature_id；
3、value值是一个vector，如果系统是多目的，那么同一个特征点在不同摄像头下会有不同的观测信息，那么这个vector，就是存储着某个特征点在所有摄像头上的信息。对于VINS-mono来说，value它不是vector，仅仅是一个pair，其实是可以的。
4、接下来看这个vector里面的每一pair。int对应的是camera_id，告诉我们这些数据是当前特征点在哪个摄像头上获得的。
5、Matrix<double, 7, 1>是一个7维向量，依次存放着当前feature_id的特征点在camera_id的相机中的归一化坐标，像素坐标和像素运动速度，这些信息都是在feature_tracker_node.cpp中获得的。   

(2)这个数据结构定义完之后，接下来就是往这个容器中放数据。

//遍历img_msg里面的每一个特征点的归一化坐标
            for (unsigned int i = 0; i < img_msg->points.size(); i++)
            {
                //把img的信息提取出来放在image容器里去，通过这里，可以理解img信息里面装的都是些什么
                int v = img_msg->channels[0].values[i] + 0.5; //channels[0].values[i]==id_of_point
               //hash
                int feature_id = v / NUM_OF_CAM;
                int camera_id = v % NUM_OF_CAM;
           
                double x = img_msg->points[i].x;
                double y = img_msg->points[i].y;
                double z = img_msg->points[i].z;
                double p_u = img_msg->channels[1].values[i];
                double p_v = img_msg->channels[2].values[i];
                double velocity_x = img_msg->channels[3].values[i];
                double velocity_y = img_msg->channels[4].values[i];
           
                Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
                xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
                image[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
            }
            

(3) 处理图像processImage() (核心！)
这一部分的内容非常多，将在5.2部分一层层剥开讲解！这里实现了视觉与IMU的初始化以及非线性优化的紧耦合。

estimator.processImage(image, img_msg->header);//处理图像帧：初始化，紧耦合的非线性优化

5、可视化

向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系，这些函数都定义在中utility/visualization.cpp里，都是ROS相关代码。

pubOdometry(estimator, header);
pubKeyPoses(estimator, header);
pubCameraPose(estimator, header);
pubPointCloud(estimator, header);
pubTF(estimator, header);
pubKeyframe(estimator);
if (relo_msg != NULL)
    pubRelocalization(estimator);
    

6、IMU的PVQ信息更新

更新IMU参数[P,Q,V,ba,bg,a,g]，需要上锁，注意线程安全。这里对应的知识点是4.1.1最后一个公式。

m_buf.lock();
m_state.lock();
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
    update();
m_state.unlock();
m_buf.unlock();

update()的内容如下：

void update()//这个函数在非线性优化时才会在process()中被调用
{//1、从估计器中得到滑动窗口中最后一个图像帧的imu更新项[P,Q,V,ba,bg,a,g]
    TicToc t_predict;
    latest_time = current_time;
    tmp_P = estimator.Ps[WINDOW_SIZE];
    tmp_Q = estimator.Rs[WINDOW_SIZE];
    tmp_V = estimator.Vs[WINDOW_SIZE];
    tmp_Ba = estimator.Bas[WINDOW_SIZE];
    tmp_Bg = estimator.Bgs[WINDOW_SIZE];
    acc_0 = estimator.acc_0;
    gyr_0 = estimator.gyr_0;
//2、对imu_buf中剩余的imu_msg进行PVQ递推（因为imu的频率比图像频率要高很多，在getMeasurements(）将图像和imu时间对齐后，imu_buf中还会存在imu数据）
    queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf;
    for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop())
        predict(tmp_imu_buf.front());
}

这里有一个问题，就是为什么要update()？
因为在进行完一个process()循环后，当前的PVQ的状态和循环开始的状态是不一样的。所以说我们需要再根据当前的数据，更新当前的PVQ状态，也就是tmp_X。同样，得上锁。

5.2 处理图像processImage()

这个函数在estimator.cpp里。先看函数名，它传入的参数分别是当前帧上的所有特征点和当前帧的时间戳。

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)

1、关键帧判断
VINS滑动窗口采取的是这样的策略，它判断当前帧是不是关键帧，如果是关键帧，滑窗的时候marg掉最老帧；如果不是关键帧，则marg掉上一帧。

//true：上一帧是关键帧，marg_old; false:上一帧不是关键帧marg_second_new
//TODO frame_count指的是次新帧还是最新帧？
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    marginalization_flag = MARGIN_OLD;
else
    marginalization_flag = MARGIN_SECOND_NEW;
    

这里出现了一个新的数据结构f_manager和一个新的函数addFeatureCheckParallax()。

数据结构: f_manager
f_manager是FeatureManager的一个对象。它定义在utility/feature_manager.h里。这个h文件里定义了3个类，借用崔神的一个神图来表示如下：

f_manager可以看作为一个存放着滑窗内所有特征点信息的容器，其中最关键的部分是list<FeaturePerId> feature。其中每一个特征点，可以看作是一个FeaturePerId的对象，它存放着一个特征点在滑窗中的所有信息，其中最关键的部分是vector<FeaturePerFrame> feature_per_frame。其中一个特征点在一个帧中的信息，可以看作是一个FeaturePerFrame的对象，它存放着一个特征点在滑窗里一个帧里面的信息，包括归一化坐标，像素坐标，像素速度等。
套娃套了三层。

接下来看看addFeatureCheckParallax()函数里面是什么逻辑，这个函数定义在feature_manager.cpp里。

//关键帧判断
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    ROS_DEBUG("input feature: %d", (int)image.size());
    ROS_DEBUG("num of feature: %d", getFeatureCount());
    //总平行度
    double parallax_sum = 0;
    //平行特征点数
    int parallax_num = 0;
    //统计在滑窗中的特征点有多少个在当前帧中继续被追踪到了
    last_track_num = 0;
    
    //遍历当前帧的每一个特征点
    for (auto &id_pts : image)
    {
        //把当前特征点封装成一个FeaturePerFrame对象
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        //获取当前帧的feature_id
        int feature_id = id_pts.first;
        //在滑窗的所有特征点中，看看能不能找到当前这个特征点
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {//是find的一个谓词判断版本，它利用返回布尔值的谓词判断pred，检查迭代器区间[first, last)上的每一个元素，如果迭代器iter满足pred(*iter) == true，表示找到元素并返回迭代器值iter；未找到元素，则返回last。
            return it.feature_id == feature_id;
                          });
        //如果这个特征点是一个新的特征(在特征点库里没有找到),那么就把它加入到滑窗的特征点库里
        if (it == feature.end())
        {
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        //如果这个特征在滑窗中已经被观测到过，那么就补充上这个特征点在当前帧的数据，并且把共视点统计数+1
        else if (it->feature_id == feature_id)
        {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++;
        }
    }
    //如果总共2帧，或者说共视点<20，那么说明次新帧是关键帧，marg_old
    if (frame_count < 2 || last_track_num < 20)
        return true;
    //遍历滑窗中的每一个特征点
    for (auto &it_per_id : feature)
    {   //如果当前特征点在当前帧-2以前出现过而且至少在当前帧-1还在，那么他就是平行特征点
        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)
        {
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;//平行特征点数
        }
    }

    if (parallax_num == 0)//判断标准1:平行特征点数为0
    { return true; }
    else//判断标准2:平均平行度小于threshold
    { return parallax_sum / parallax_num >= MIN_PARALLAX; }
}

这一部分代码完全在秦通大神的IV.A部分体现了。秦神在这部分里写了2个判断关键帧的判断指标，第一个是“the average parallax apart from the previous keyframe”，对应着代码中parallax_num和parallax_sum / parallax_num；第二个是“If the number of tracked features goes below a certain threshold, we treat this frame as a new keyframe”，对应着代码里的last_track_num。注意，这部分里还有一个函数是compensatedParallax2()，用来计算当前特征点的视差。

2、将图像数据、时间、临时预积分值存到图像帧类中

ImageFrame imageframe(image, header.stamp.toSec());
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));

这里出现了一个新的数据结构，分析如下：

数据结构: ImageFrame imageframe
imageframe是ImageFrame的一个实例，定义在initial/initial_alignment.h里。顾名思义，它是用于融合IMU和视觉信息的数据结构，包括了某一帧的全部信息:位姿，特征点信息，预积分信息，是否是关键帧等。

3、更新临时预积分初始值

tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

4、如果需要标定外参，则标定

if(ESTIMATE_EXTRINSIC == 2)////如果没有外参则进行标定
{
    ROS_INFO("calibrating extrinsic param, rotation movement is needed");
    if (frame_count != 0)
    {
        vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
        Matrix3d calib_ric;
        if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
        {
            ROS_WARN("initial extrinsic rotation calib success");
            ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
            ric[0] = calib_ric;
            RIC[0] = calib_ric;
            ESTIMATE_EXTRINSIC = 1;
        }
    }
}

5、初始化
这是一个重要模块，见下一章节介绍。一般初始化只进行一次。

6、非线性优化
这是一个重要模块，见下下章节介绍。一般非线性优化是稳定状态下VINS系统一直要循环的部分。