VINS-FUSION 前端后端代码全详解（一）

本次工作

我首先参照网络上的文档整理了全部的代码，并对于C++和OpenCV的一些操作也进行了详细的注释，并写了这篇的博客进行全部的讲解，其中1-4章节是前端VIO信息，5章节是后端DBOW词袋回环，6-7章节是GPS与VIO融合，8章节是参考文献。

1. 程序入口rosNodeTest.cpp

1.1 定义内容

运行程序时，首先进入的是主程序vins_estimator/src/estimator/rosNodeTest.cpp
里边主要定义了 估计器、 缓存器 、 获取传感器数据的函数 和 一个主函数

// 获得左目的message
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 获得右目的message
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 从msg中获取图片，返回值cv::Mat，输入是当前图像msg的指针
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
// 从两个主题中提取具有相同时间戳的图像
// 并将图像输入到估计器中
void sync_process()
// 输入imu的msg信息，进行解算并把imu数据输入到estimator
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
// 把特征点的点云msg输入到estimator
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
// 是否重启estimator，并重新设置参数
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
// 是否使用IMU
void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
// 相机的开关
void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
int main(int argc, char **argv)

1.2 程序执行

1.2.1 获取参数并设置参数

具体的方法在函数

主函数中，主要是执行以下各个步骤订阅ROS信息，然后进行处理

string config_file = argv[1];
printf("config_file: %s\n", argv[1]);
//config_file: 4VINS_test/0config/yaml_mynt_s1030/mynt_stereo_imu_config.yaml

readParameters(config_file);// 读取参数
estimator.setParameter();// 设置参数

/*
    ros::Subscriber subscribe (const std::string &topic, uint32_t queue_size, void(*fp)(M), const TransportHints &transport_hints=TransportHints())
    第一个参数是订阅话题的名称；
    第二个参数是订阅队列的长度；（如果收到的消息都没来得及处理，那么新消息入队，旧消息就会出队）；
    第三个参数是回调函数的指针，指向回调函数来处理接收到的消息！
    第四个参数：似乎与延迟有关系，暂时不关心。（该成员函数有13重载）
    */
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
    ros::Subscriber sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);
    ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
    ros::Subscriber sub_imu_switch = n.subscribe("/vins_imu_switch", 100, imu_switch_callback);
    ros::Subscriber sub_cam_switch = n.subscribe("/vins_cam_switch", 100, cam_switch_callback);

    std::thread sync_thread{sync_process}; //创建sync_thread线程，指向sync_process，这里边处理了processMeasurements的线程
    ros::spin(); // 用于触发topic, service的响应队列
    // 如果你的程序写了相关的消息订阅函数，那么程序在执行过程中，除了主程序以外，ROS还会自动在后台按照你规定的格式，接受订阅的消息，但是所接到的消息并不是
    // 立刻就被处理，而是必须要等到ros::spin()或ros::spinOnce()执行的时候才被调用，这就是消息回到函数的原理

其中有几个比较重要的函数，在下方进行说明。

1.2.2 imu_callback

其中imu_callback中订阅imu信息，并将器填充到accBuf和gyrBuf中，之后执行了vins_estimator/src/estimator/estimator.cpp中inputIMU函数的fastPredictIMU、pubLatestOdometry函数

fastPredictIMU使用上一时刻的姿态进行快速的imu预积分，这个信息根据processIMU的最新数据Ps[frame_count]、Rs[frame_count]、Vs[frame_count]、Bas[frame_count]、Bgs[frame_count]来进行预积分，从而保证信息能够正常发布。

// 使用上一时刻的姿态进行快速的imu预积分
// 用来预测最新P,V,Q的姿态
// -latest_p,latest_q,latest_v,latest_acc_0,latest_gyr_0 最新时刻的姿态。这个的作用是为了刷新姿态的输出，但是这个值的误差相对会比较大，是未经过非线性优化获取的初始值。
void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)

其中latest_Ba、latest_Bb是在预积分处就已经计算完毕的

// 其中包含了检测关键帧,估计外部参数,初始化,状态估计,划窗等等。
/**
 * 处理一帧图像特征
 * 1、提取前一帧与当前帧的匹配点
 * 2、在线标定外参旋转
 *     利用两帧之间的Camera旋转和IMU积分旋转，构建最小二乘问题，SVD求解外参旋转
 *     1) Camera系，两帧匹配点计算本质矩阵E，分解得到四个解，根据三角化成功点比例确定最终正确解R、t，得到两帧之间的旋转R
 *     2) IMU系，积分计算两帧之间的旋转
 *     3) 根据旋转构建最小二乘问题，SVD求解外参旋转
 * 3、系统初始化
 * 4、3d-2d Pnp求解当前帧位姿
 * 5、三角化当前帧特征点
 * 6、滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
 * 7、剔除outlier点
 * 8、用当前帧与前一帧位姿变换，估计下一帧位姿，初始化下一帧特征点的位置
 * 9、移动滑窗，更新特征点的观测帧集合、观测帧索引（在滑窗中的位置）、首帧观测帧和深度值，删除没有观测帧的特征点
 * 10、删除优化后深度值为负的特征点
 * @param image  图像帧特征
 * @param header 时间戳
*/
void Estimator::processImage
(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)

经过fastPredictIMU函数后我们可以拿到latest_P, latest_Q, latest_V三个变量，之后即可通过pubLatestOdometry广播odom信息

//构建一个odometry的msg并发布
void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, double t)

1.2.3 feature_callback

feature_callback的作用是获取点云数据，之后填充featureFrame，并把featureFrame通过inputFeature输入到estimator，且填充了featureBuf

/**
 * 订阅一帧跟踪的特征点，包括3D坐标、像素坐标、速度，交给estimator处理
*/
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    for (unsigned int i = 0; i < feature_msg->points.size(); i++)
    {
        int feature_id = feature_msg->channels[0].values[i];
        int camera_id = feature_msg->channels[1].values[i];
        double x = feature_msg->points[i].x;
        double y = feature_msg->points[i].y;
        double z = feature_msg->points[i].z;
        double p_u = feature_msg->channels[2].values[i];
        double p_v = feature_msg->channels[3].values[i];
        double velocity_x = feature_msg->channels[4].values[i];
        double velocity_y = feature_msg->channels[5].values[i];
        if(feature_msg->channels.size() > 5)
        {
            double gx = feature_msg->channels[6].values[i];
            double gy = feature_msg->channels[7].values[i];
            double gz = feature_msg->channels[8].values[i];
            pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);
            //printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
        }
        ROS_ASSERT(z == 1);
        Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
        xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
        featureFrame[feature_id].emplace_back(camera_id,  xyz_uv_velocity);
    }
    double t = feature_msg->header.stamp.toSec();
    estimator.inputFeature(t, featureFrame);
    return;
}

1.2.4 sync_process

之后通过

   // 从两个图像队列中取出最早的一帧，并从队列删除，双目要求两帧时差不得超过0.003s
std::thread sync_thread{sync_process}; //创建sync_thread线程，指向sync_process，这里边处理了measurementpross的线程

进入sync_process进行处理

/**
 * 从两个图像队列中取出最早的一帧，并从队列删除，双目要求两帧时差不得超过0.003s
*/
void sync_process()
{
    while(1)
    {
        if(STEREO)
        {
            cv::Mat image0, image1;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if (!img0_buf.empty() && !img1_buf.empty())
            {
                double time0 = img0_buf.front()->header.stamp.toSec();
                double time1 = img1_buf.front()->header.stamp.toSec();
                // 双目相机左右图像时差不得超过0.003s
                if(time0 < time1 - 0.003)
                {
                    img0_buf.pop();
                    printf("throw img0\n");
                }
                else if(time0 > time1 + 0.003)
                {
                    img1_buf.pop();
                    printf("throw img1\n");
                }
                else
                {
                    // 提取缓存队列中最早一帧图像，并从队列中删除
                    time = img0_buf.front()->header.stamp.toSec();
                    header = img0_buf.front()->header;
                    image0 = getImageFromMsg(img0_buf.front());
                    img0_buf.pop();
                    image1 = getImageFromMsg(img1_buf.front());
                    img1_buf.pop();
                }
            }
            m_buf.unlock();
            if(!image0.empty())
                estimator.inputImage(time, image0, image1);
        }
        else
        {
            cv::Mat image;
            std_msgs::Header header;
            double time = 0;
            m_buf.lock();
            if(!img0_buf.empty())
            {
                time = img0_buf.front()->header.stamp.toSec();
                header = img0_buf.front()->header;
                image = getImageFromMsg(img0_buf.front());
                img0_buf.pop();
            }
            m_buf.unlock();
            if(!image.empty())
                estimator.inputImage(time, image);
        }

        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
}

该函数中，首先对是否双目进行判断。
如果是双目，需要检测同步问题。对双目的时间进行判断，时间间隔小于0.003s的话则使用getImageFromMsg将其输入到image0和image1变量之中。之后estimator.inputImage。
如果是单目，则直接estimator.inputImage

2. 图像输入estimator.cpp

2.1 inputImage

/**
 * 输入一帧图像
 * 1、featureTracker，提取当前帧特征点
 * 2、添加一帧特征点，processMeasurements处理
*/
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
    inputImageCnt++;
    // 特征点id，(x,y,z,pu,pv,vx,vy)
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
    TicToc featureTrackerTime;

    /**
     * 跟踪一帧图像，提取当前帧特征点
     * 1、用前一帧运动估计特征点在当前帧中的位置，如果特征点没有速度，就直接用前一帧该点位置
     * 2、LK光流跟踪前一帧的特征点，正反向，删除跟丢的点；如果是双目，进行左右匹配，只删右目跟丢的特征点
     * 3、对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩阵，用极线约束进一步剔除outlier点（代码注释掉了）
     * 4、如果特征点不够，剩余的用角点来凑；更新特征点跟踪次数
     * 5、计算特征点归一化相机平面坐标，并计算相对与前一帧移动速度
     * 6、保存当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
     * 7、展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
    */
    if(_img1.empty())
        featureFrame = featureTracker.trackImage(t, _img);
    else
        featureFrame = featureTracker.trackImage(t, _img, _img1);
    //printf("featureTracker time: %f\n", featureTrackerTime.toc());

    // 发布跟踪图像
    if (SHOW_TRACK)
    {
        cv::Mat imgTrack = featureTracker.getTrackImage();
        pubTrackImage(imgTrack, t);
    }
    
    // 添加一帧特征点，处理
    if(MULTIPLE_THREAD)  
    {     
        if(inputImageCnt % 2 == 0)
        {
            mBuf.lock();
            featureBuf.push(make_pair(t, featureFrame));
            mBuf.unlock();
        }
    }
    else
    {
        mBuf.lock();
        featureBuf.push(make_pair(t, featureFrame));
        mBuf.unlock();
        TicToc processTime;
        processMeasurements();
        printf("process time: %f\n", processTime.toc());
    }
    
}

当中需要先设置参数，并开启processMeasurements线程

setParameter();

然后追踪图像上的特征trackImage，之后会进行详解,其中得到了featureFrame

if(_img1.empty())
    featureFrame = featureTracker.trackImage(t, _img);// 追踪单目
else
    featureFrame = featureTracker.trackImage(t, _img, _img1);// 追踪双目

然后，getTrackImage对特征到跟踪的图像进行一些处理。并把追踪的图片imgTrack发布出去.

   if (SHOW_TRACK)//这个应该是展示轨迹 
    {
        cv::Mat imgTrack = featureTracker.getTrackImage();
        pubTrackImage(imgTrack, t);
    }

然后，填充featureBuf
最后执行processMeasurements，这是处理全部量测的线程,IMU的预积分,特征点的处理等等都在这里进行.

2.2 trackImage

根据2.1的讲述，我们发现当中用到了trackImage这个函数，这个函数在
vins_estimator/src/featureTracker/feature_tracker.cpp

/**
 * 跟踪一帧图像，提取当前帧特征点
 * 1、用前一帧运动估计特征点在当前帧中的位置
 * 2、LK光流跟踪前一帧的特征点，正反向，删除跟丢的点；如果是双目，进行左右匹配，只删右目跟丢的特征点
 * 3、对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩阵，用极线约束进一步剔除outlier点（代码注释掉了）
 * 4、如果特征点不够，剩余的用角点来凑；更新特征点跟踪次数
 * 5、计算特征点归一化相机平面坐标，并计算相对与前一帧移动速度
 * 6、保存当前帧特征点数据（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
 * 7、展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
*/
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)

图像处理可以添加图像处理的部分，比如直方图均衡等等方法。

   {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//createCLAHE 直方图均衡
        clahe->apply(cur_img, cur_img);
        if(!rightImg.empty())
            clahe->apply(rightImg, rightImg);
    }

2.2.1 hasPrediction

会对上一帧的特征点进行处理，计算出上一帧运动估计特征点在当前帧中的位置。对于前后帧用LK光流跟踪到的匹配特征点，计算基础矩。

if(hasPrediction)
        {
            cur_pts = predict_pts;
            // LK光流跟踪两帧图像特征点，金字塔为1层
            cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 1, 
            cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
            
            // 跟踪到的特征点数量
            int succ_num = 0;
            for (size_t i = 0; i < status.size(); i++)
            {
                if (status[i])
                    succ_num++;
            }
            // 特征点太少，金字塔调整为3层，再跟踪一次
            if (succ_num < 10)
               cv::calcOpticalFlowPyrLK(prev_img, cur_img, prev_pts, cur_pts, status, err, cv::Size(21, 21), 3);
        }

2.2.2 if(SHOW_TRACK)

画出追踪情况，就是在图像上的特征点位置出画圈圈，如果是双目的话就连线。

// 展示，左图特征点用颜色区分跟踪次数（红色少，蓝色多），画个箭头指向前一帧特征点位置，如果是双目，右图画个绿色点
    if(SHOW_TRACK)
        drawTrack(cur_img, rightImg, ids, cur_pts, cur_right_pts, prevLeftPtsMap);

2.2.3 setMask

在已跟踪到角点的位置上，将mask对应位置上设为0,
意为在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
进行操作时在该点不再重复进行角点检测，这样可以使角点分布更加均匀
具体详情见开源的注释代码。

/**
 * 特征点画个圈（半径MIN_DIST）存mask图，同时特征点集合按跟踪次数从大到小重排序
*/
void FeatureTracker::setMask()

2.2.4 goodFeaturesToTrack

如果当前图像的特征点cur_pts数目小于规定的最大特征点数目MAX_CNT，则进行提取。
提取使用的cv::goodFeaturesToTrack。将点保存到n_pts

/* goodFeaturesToTrack
_image：8位或32位浮点型输入图像，单通道
_corners：保存检测出的角点
maxCorners：角点数目最大值，如果实际检测的角点超过此值，则只返回前maxCorners个强角点
qualityLevel：角点的品质因子
minDistance：对于初选出的角点而言，如果在其周围minDistance范围内存在其他更强角点，则将此角点删除
_mask：指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
blockSize：计算协方差矩阵时的窗口大小
useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
harrisK：Harris角点检测需要的k值 */
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
// mask 这里肯定是指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI

之后将n_pts保存到cur_pts之中

2.2.5 undistortedPts

将像素座标系下的座标，转换为归一化相机座标系下的座标 即un_pts为归一化相机座标系下的座标。

/**
 * 像素点计算归一化相机平面点，带畸变校正
*/
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)
{
    vector<cv::Point2f> un_pts;
    for (unsigned int i = 0; i < pts.size(); i++)
    {
        // 特征点像素坐标
        Eigen::Vector2d a(pts[i].x, pts[i].y);
        Eigen::Vector3d b;
        // 像素点计算归一化相机平面点，带畸变校正
        cam->liftProjective(a, b);
        // 归一化相机平面点
        un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
    }
    return un_pts;
}

2.2.6 cam->liftProjective(a, b);

这个函数是对鱼眼相机模型的标定及去畸变过程

/**
 * \brief Lifts a point from the image plane to its projective ray
 * \param p image coordinates
 * \param P coordinates of the projective ray
 * 这个函数是对鱼眼相机模型的标定及去畸变过程
 */
void
PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const

之后通过ptsVelocity计算当前帧相对于前一帧 特征点沿x,y方向的像素移动速度

/**
 * 计算当前帧归一化相机平面特征点在x、y方向上的移动速度
 * @param pts 当前帧归一化相机平面特征点
*/
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts, 
                                            map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)
{
    vector<cv::Point2f> pts_velocity;
    cur_id_pts.clear();
    for (unsigned int i = 0; i < ids.size(); i++)
    {
        cur_id_pts.insert(make_pair(ids[i], pts[i]));
    }

    // caculate points velocity
    if (!prev_id_pts.empty())
    {
        double dt = cur_time - prev_time;
        
        // 遍历当前帧归一化相机平面特征点
        for (unsigned int i = 0; i < pts.size(); i++)
        {
            std::map<int, cv::Point2f>::iterator it;
            it = prev_id_pts.find(ids[i]);
            if (it != prev_id_pts.end())
            {
                // 计算点在归一化相机平面上x、y方向的移动速度
                double v_x = (pts[i].x - it->second.x) / dt;
                double v_y = (pts[i].y - it->second.y) / dt;
                pts_velocity.push_back(cv::Point2f(v_x, v_y));
            }
            else
                pts_velocity.push_back(cv::Point2f(0, 0));

        }
    }
    else
    {
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    return pts_velocity;
}

2.3 双目摄像头

如果是双目相机，那么在右目上追踪左目的特征点。使用的函数是calcOpticalFlowPyrLK

/*光流跟踪是在左右两幅图像之间进行cur left ---- cur right
prevImg	第一幅8位输入图像 或 由buildOpticalFlowPyramid()构造的金字塔。
nextImg	第二幅与preImg大小和类型相同的输入图像或金字塔。
prevPts	光流法需要找到的二维点的vector。点坐标必须是单精度浮点数。
nextPts	可以作为输入，也可以作为输出。包含输入特征在第二幅图像中计算出的新位置的二维点（单精度浮点坐标）的输出vector。当使用OPTFLOW_USE_INITIAL_FLOW 标志时，nextPts的vector必须与input的大小相同。
status	输出状态vector(类型：unsigned chars)。如果找到了对应特征的流，则将向量的每个元素设置为1；否则，置0。
err	误差输出vector。vector的每个元素被设置为对应特征的误差，可以在flags参数中设置误差度量的类型；如果没有找到流，则未定义误差（使用status参数来查找此类情况）。
winSize	每级金字塔的搜索窗口大小。
maxLevel	基于最大金字塔层次数。如果设置为0，则不使用金字塔（单级）；如果设置为1，则使用两个级别，等等。如果金字塔被传递到input，那么算法使用的级别与金字塔同级别但不大于MaxLevel。
criteria	指定迭代搜索算法的终止准则（在指定的最大迭代次数标准值（criteria.maxCount）之后，或者当搜索窗口移动小于criteria.epsilon。）
flags 操作标志，可选参数：
OPTFLOW_USE_INITIAL_FLOW：使用初始估计，存储在nextPts中；如果未设置标志，则将prevPts复制到nextPts并被视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本征值作为误差度量（见minEigThreshold描述）；如果未设置标志，则将原始周围的一小部分和移动的点之间的 L1 距离除以窗口中的像素数，作为误差度量。
minEigThreshold	
算法所计算的光流方程的2x2标准矩阵的最小本征值（该矩阵称为[Bouguet00]中的空间梯度矩阵）÷ 窗口中的像素数。如果该值小于MinEigThreshold，则过滤掉相应的特征，相应的流也不进行处理。因此可以移除不好的点并提升性能。 */
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);

2.3.1 if(FLOW_BACK)

如果这个打开，就想前边的左右目图像的位置换一下，在进行一次特征跟踪，目的是反向跟踪，得到左右目都匹配到的点

// 反向LK光流计算一次
        if(FLOW_BACK)
        {
            vector<uchar> reverse_status;
            vector<cv::Point2f> reverse_pts = prev_pts;
            cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 1, 
            cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 30, 0.01), cv::OPTFLOW_USE_INITIAL_FLOW);
            //cv::calcOpticalFlowPyrLK(cur_img, prev_img, cur_pts, reverse_pts, reverse_status, err, cv::Size(21, 21), 3); 
            
            // 正向、反向都匹配到了，且用正向匹配点反向匹配回来，与原始点距离不超过0.5个像素，认为跟踪到了
            for(size_t i = 0; i < status.size(); i++)
            {
                if(status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5)
                {
                    status[i] = 1;
                }
                else
                    status[i] = 0;
            }
        }

之后和单目一样执行undistortedPts 和 ptsVelocity

2.3.2 制作featureFrame

// 添加当前帧特征点（归一化相机平面坐标，像素坐标，归一化相机平面移动速度）
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;

其中，camera_id = 0为左目上的点，camera_id = 1，为右目上的点。

3 IMU处理estimator.cpp

3.1 判断IMU数据是否可用

if ((!USE_IMU  || IMUAvailable(feature.first + td)))//如果不用imu或者超时

其中

// 判断输入的时间t时候的imu是否可用
bool Estimator::IMUAvailable(double t)

3.2 获得accVector和gyrVector

对imu的时间进行判断，讲队列里的imu数据放入到accVector和gyrVector中

// 对imu的时间进行判断，讲队列里的imu数据放入到accVector和gyrVector中，完成之后返回true
bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, 
                                vector<pair<double, Eigen::Vector3d>> &gyrVector)

3.3 初始化IMU的姿态

initFirstIMUPose,其实很简单,就是求一个姿态角,然后把航向角设为0

//第一帧IMU姿态初始化
// 用初始时刻加速度方向对齐重力加速度方向，得到一个旋转，使得初始IMU的z轴指向重力加速度方向
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)

3.4 处理IMU数据,运行processIMU

/**
 * 处理一帧IMU，积分
 * 用前一图像帧位姿，前一图像帧与当前图像帧之间的IMU数据，积分计算得到当前图像帧位姿
 * Rs，Ps，Vs
 * @param t                     当前时刻
 * @param dt                    与前一帧时间间隔
 * @param linear_acceleration   当前时刻加速度
 * @param angular_velocity      当前时刻角速度
*/
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)

其中frame_count是值窗内的第几帧图像

下边是新建一个预积分项目

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

预积分

pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        // push_back重载的时候就已经进行了预积分

其中的push_back

    void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        propagate(dt, acc, gyr);
    }

其中的propagate

    // IMU预积分传播方程 
    // 积分计算两个关键帧之间IMU测量的变化量
    // 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
    /**
     * IMU中值积分传播
     * 1、前一时刻状态计算当前时刻状态，PVQ，Ba，Bg
     * 2、计算当前时刻的误差Jacobian，误差协方差 todo
    */
    void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
    {
        dt = _dt;
        acc_1 = _acc_1;
        gyr_1 = _gyr_1;
        Vector3d result_delta_p;
        Quaterniond result_delta_q;
        Vector3d result_delta_v;
        Vector3d result_linearized_ba;
        Vector3d result_linearized_bg;

        /**
         * 中值积分
         * 1、前一时刻状态计算当前时刻状态，PVQ，其中Ba，Bg保持不变
         * 2、计算当前时刻的误差Jacobian，误差协方差 todo
        */
        midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                            linearized_ba, linearized_bg,
                            result_delta_p, result_delta_q, result_delta_v,
                            result_linearized_ba, result_linearized_bg, 1);

        //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
        //                    linearized_ba, linearized_bg);
        delta_p = result_delta_p;
        delta_q = result_delta_q;
        delta_v = result_delta_v;
        linearized_ba = result_linearized_ba;
        linearized_bg = result_linearized_bg;
        delta_q.normalize();
        sum_dt += dt;
        acc_0 = acc_1;
        gyr_0 = gyr_1;  
     
    }

其中的midPointIntegration.这里边就涉及到了IMU的传播方针和协方差矩阵.雅克比矩阵等等.哪里不懂可以VIO的理论知识。

// 中值积分递推Jacobian和Covariance
// _acc_0上次测量加速度 _acc_1本次测量加速度 delta_p上一次的位移 result_delta_p位置变化量计算结果 update_jacobian是否更新雅克比基本方法就涉及到了IMU的创博方针和器方差矩阵的窗哦sdf
void midPointIntegration(double _dt, 
                        const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                        const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                        const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                        const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                        Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                        Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)

之后计算对应绝对坐标系下的位置等

    // Rs Ps Vs是frame_count这一个图像帧开始的预积分值,是在绝对坐标系下的.
    int j = frame_count;         
    Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//移除了偏执的加速度
    Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];//移除了偏执的gyro
    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;

4. 图像处理estimator.cpp

4.1 processImage

函数入口processMeasurements中的processImage(feature.second, feature.first);输入的之后关键帧和时间

/* addFeatureCheckParallax
对当前帧与之前帧进行视差比较，如果是当前帧变化很小，就会删去倒数第二帧，如果变化很大，就删去最旧的帧。并把这一帧作为新的关键帧
这样也就保证了划窗内优化的,除了最后一帧可能不是关键帧外,其余的都是关键帧
VINS里为了控制优化计算量，在实时情况下，只对当前帧之前某一部分帧进行优化，而不是全部历史帧。局部优化帧的数量就是窗口大小。
为了维持窗口大小，需要去除旧的帧添加新的帧，也就是边缘化 Marginalization。到底是删去最旧的帧（MARGIN_OLD）还是删去刚
刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)
如果大于最小像素,则返回true */
/**
 * 添加特征点记录，并检查当前帧是否为关键帧
 * @param frame_count   当前帧在滑窗中的索引
 * @param image         当前帧特征（featureId，cameraId，feature）
*/
bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)

判断之后,确定marg掉那个帧

// 检测关键帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
{
    marginalization_flag = MARGIN_OLD;//新一阵将被作为关键帧!
    //printf("keyframe\n");
}
else
{
    marginalization_flag = MARGIN_SECOND_NEW;
    //printf("non-keyframe\n");
}

4.2 估计相机和IMU的外参

// 当前帧预积分（前一帧与当前帧之间的IMU预积分）
imageframe.pre_integration = tmp_pre_integration;
all_image_frame.insert(make_pair(header, imageframe));
// 重置预积分器
tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

// 估计一个外部参,并把ESTIMATE_EXTRINSIC置1,输出ric和RIC
if(ESTIMATE_EXTRINSIC == 2)

首先找到对应的图像

vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            // 这个里边放的是新图像和上一帧

使用CalibrationExRotation计算参数

/* CalibrationExRotation
当外参完全不知道的时候，可以在线对其进行初步估计,然后在后续优化时，会在optimize函数中再次优化。
输入是新图像和上一阵图像的位姿 和二者之间的imu预积分值,输出旋转矩阵
对应VIO课程第七讲中对外参矩阵的求解 */
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)

4.3 如果没有初始化,那么就先进行初始化

4.3.1 单目+IMU的初始化

 // monocular + IMU initilization
 // 单目+IMU系统初始化
 if (!STEREO && USE_IMU)
 {
     // 要求滑窗满
     if (frame_count == WINDOW_SIZE)
     {
         bool result = false;
         // 如果上次初始化没有成功，要求间隔0.1s
         if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
         {
             /**
              * todo
              * 系统初始化
              * 1、计算滑窗内IMU加速度的标准差，用于判断移动快慢
              * 2、在滑窗中找到与当前帧具有足够大的视差，同时匹配较为准确的一帧，计算相对位姿变换
              *   1) 提取滑窗中每帧与当前帧之间的匹配点（要求点在两帧之间一直被跟踪到，属于稳定共视点），超过20个则计算视差
              *   2) 两帧匹配点计算本质矩阵E，恢复R、t
              *   3) 视差超过30像素，匹配内点数超过12个，则认为符合要求，返回当前帧
              * 3、以上面找到的这一帧为参考系，Pnp计算滑窗每帧位姿，然后三角化所有特征点，构建BA（最小化点三角化前后误差）优化每帧位姿
              *   1) 3d-2d Pnp求解每帧位姿
              *   2) 对每帧与l帧、当前帧三角化
              *   3) 构建BA，最小化点三角化前后误差，优化每帧位姿
              *   4) 保存三角化点
              * 4、对滑窗中所有帧执行Pnp优化位姿
              * 5、Camera与IMU初始化，零偏、尺度、重力方向
             */
             result = initialStructure();
             initial_timestamp = header;   
         }
         if(result)
         {
             // 滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
             optimization();
             // 用优化后的当前帧位姿更新IMU积分的基础位姿，用于展示IMU轨迹
             updateLatestStates();
             solver_flag = NON_LINEAR;
             // 移动滑窗，更新特征点的观测帧集合、观测帧索引（在滑窗中的位置）、首帧观测帧和深度值，删除没有观测帧的特征点
             slideWindow();
             ROS_INFO("Initialization finish!");
         }
         else
             // 移动滑窗，更新特征点的观测帧集合、观测帧索引（在滑窗中的位置）、首帧观测帧和深度值，删除没有观测帧的特征点
             slideWindow();
     }
 }

其中的initialStructure还含有很多函数.

// 视觉和惯性的对其,对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的visualInitialAlign
/* visualInitialAlign
很具VIO课程第七讲:一共分为5步:
1估计旋转外参. 2估计陀螺仪bias 3估计中立方向,速度.尺度初始值 4对重力加速度进一步优化 5将轨迹对其到世界坐标系 */
bool Estimator::visualInitialAlign()


// 视觉IMu的对其
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)


// 更新得到新的陀螺仪漂移Bgs
// 对应视觉IMU对其的第二部分
// 对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的公式31-34
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)


// 初始化速度、重力和尺度因子
// 对应 https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw 中的公式34-36
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

成功初始化之后则执行，optimization()、 updateLatestStates() 、slideWindow()

if(result)//如果初始化成功
{
    optimization();//先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
    updateLatestStates();
    solver_flag = NON_LINEAR;
    slideWindow();//滑动窗口
    ROS_INFO("Initialization finish!");
}
else//滑掉这一窗
    slideWindow();

updateLatestStates() 和slideWindow()这两个函数比较简单

// 让此时刻的值都等于上一时刻的值,用来更新状态
void Estimator::updateLatestStates()

// 滑动窗口法
void Estimator::slideWindow()
// 道理很简单,就是把前后元素交换

optimization()为基于滑动窗口的紧耦合的非线性优化，残差项的构造和求解

/**
 * 滑窗执行Ceres优化，边缘化，更新滑窗内图像帧的状态（位姿、速度、偏置、外参、逆深度、相机与IMU时差）
*/
void Estimator::optimization()