VINS-MONO源码学习（二）：状态估计器流程processImage

在上一篇文章的预积分完成之后，VINS-MONO执行了estimator::processImage这个函数，实现了视觉与IMU的初始化以及非线性优化的紧耦合，本篇先记录一下这个函数的流程，初始化和非线性优化的内容较多，会在后面写。
1、首先搞清楚这个函数的输入image是个什么东西：
estimator.processImage(image, img_msg->header);
measurement是一组观测（一帧图像和若干IMU数据），数据类型是std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>，这pair的一项就是imu_msg，第二项就是img_msg。
传到processImage的参数Image的生成过程如下，img_msg是指向sensor_msgs/PointCloud的指针，PointCloud消息中包括了点云的3D坐标和对应点的一些属性channels

geometry_msgs/Point32[] points
ChannelFloat32[] channels

里面的内容可以在feature_tracker_node.cpp的img_callback中找到，可见，channels中存的是对应3D点的id、图像中的u坐标和v坐标。

feature_points->points.push_back(p);
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);
v_of_point.values.push_back(cur_pts[j].y);

feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);

这样看来下面代码中的v就是点云的索引（虽说加0.5是为了四舍五入，但我觉得这些量都是整型，不知道会不会出现需要四舍五入的情况）。所以image就是一帧图像中的特征点的集合。

map<int, vector<pair<int, Vector3d>>> image;
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
    int v = img_msg->channels[0].values[i] + 0.5;
    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;
    ROS_ASSERT(z == 1);
    image[feature_id].emplace_back(camera_id, Vector3d(x, y, z));
}

2、在开始看processImage的代码之前，首先需要弄清feature_manager类。

FeatureManager类含有成员变量list< FeaturePerId > feature，是滑窗中的所有路标点；列表中的每个路标点会在多个frame中出现 ，按照frame分组组成vector< FeaturePerFrame > feature_per_frame；feature_per_frame中存放了每个路标点在一张图像中的信息。
在FeatureManager::addFeatureCheckParallax函数中，把图像特征点放入list< FeaturePerId > feature中。
对于一帧中的每个特征点，建一个feature_per_frame，如果这个特征点中，与目前的滑窗中已有的特征点相同的数量小于20，就认为这是一个关键帧，如果大于20，则计算视差来判断倒数第二帧是不是关键帧（来了最新一帧之后，才把前一帧和这两帧之间的IMU数据合并起来作为一组数据，因此现在判断的是倒数第二帧）。计算倒数第二帧和倒数第三帧的视差：这里的视差相同的特征点在这两帧图像中的像素距离（坐标差平方之和开方）。滑窗中所有的特征点的平均视差如果大于阈值就认为这个倒数第二帧是关键帧。
判断关键帧之后，创建ImageFrame对象，把图像、时间戳、预积分的值存进去，然后把所有的产生的imageFrame都存到名为all_image_frame的map中。重新为预积分指针tmp_pre_integration分配空间（重新预积分）。
3、标定外参：如果需要标定外参（相机和IMU之间的位移和旋转），调用CalibrationExRotation（）函数进行标定。
4、初始化：
当还没进行初始化，并且有足够的帧填满滑窗，并且具有外参，并且当前的帧的时间戳-初始时间戳>0.1，则开始初始化。初始化分为两步：纯视觉的sfm、视觉惯导联合，这些内容在下一篇文章中介绍Estimator::initialStructure()：

result = initialStructure();
initial_timestamp = header.stamp.toSec();

如果初始化成功则进行非线性优化，否则执行滑窗操作。非线性优化solveOdometry和滑窗操作slideWindow以后再说，挖个坑。

if(result)
            {
                solver_flag = NON_LINEAR;
                solveOdometry();
                slideWindow();
                f_manager.removeFailures();
                ROS_INFO("Initialization finish!");
                last_R = Rs[WINDOW_SIZE];
                last_P = Ps[WINDOW_SIZE];
                last_R0 = Rs[0];
                last_P0 = Ps[0];
                
            }
            else
                slideWindow();