R3Live系列学习（三）R2Live源码阅读（1）

上一篇FAST-LIO的文章写的不是很好，我经过几个月的反思和学习，现在厚着脸皮继续写，我会进一步分析ESKF的行为，弥补上一篇的一些问题。这次阅读的源码是R2Live，论文为《R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual tightly-coupled state Estimator and mapping》。它的工程看起来像是FAST-LIO与Vins-Fusion的缝合，它告别了之前系列的简单粗暴的风格，转向了学院风格。

这次我们依旧边看论文边看代码。从系统框图可以知道两点：

1、R2Live并不是简单地将LIO与VIO的位姿结果做KF更新，而是估计整个系统状态变量，分别用点云特征和视觉特征做观测量更新，紧耦合的程度更深。

2、R2Live建立的是lidar点云地图，而视觉地图并未给点云地图提供信息，而是作为视觉特征的约束存在，也就是虽然状态在一起估计，但各自的地图并未建立实质的联系，而R3Live对视觉地图的利用将有大的飞跃。

那么，从系统框图上又有了一些疑惑与期待：

1、imu积分肯定是做系统的先验，但雷达特征和视觉特征又以不同频率进入观测，我们将怎么处理不同时刻的观测量从而更新？

2、这次的滑动窗口和边缘化策略和vins系列相比，有没有更精妙的提升？

3、既然视觉地图和激光点云地图“不相关”，并且我们使用了位姿图优化，那么我们怎么处理回环的相关问题？

带着问题学习从demo.launch开始，它有三个进程，lio_feat_extract（雷达特征点提取）、feature_tracker（视觉特征点提取）、r2live（状态估计）。这个工程的设计有点“拧巴”，切记！！！

1.整个包具有三个project，camera_model，feature_tracker和r2live，都是分别编译完成的，而camera_model提供了不止一种相机模型。

2.都在fast-lio包里的两个文件，lio_feat_extract进程由feature_extract.cpp文件编译而来，但它只负责提取特征点，真正的状态处理放在fast_lio.hpp里，而Fast_lio类又只在r2live进程里得到调用。

这也是“综述型”多传感器融合SLAM工程的烦点，作者在工程里残留了一些FAST-LIO与Vins的其它文件，阅读源码的时候需要看准include和cmakelist，有选择的观看。作者假定雷达与imu已标定好外参，而将在线标定相机与imu的外参（参照Vins-Mono的做法），当然，我们也可以把其它参数（lidar-imu外参，相机内参等等）都加进来做融合，提升整个系统的精度。

一、feature_tracker进程

视觉的特征采集与跟踪和以往类似，但也具有了一些改变。首先是一些即将用到的变量，对这部分作者没有注释有点奇怪。

    cv::Mat mask;
    cv::Mat fisheye_mask;//对不同相机模型分别指定特征点的检测区域
    cv::Mat prev_img, cur_img, forw_img;
    vector<cv::Point2f> n_pts;
    vector<cv::Point2f> prev_pts, cur_pts, forw_pts;//特征点
    vector<cv::Point2f> prev_un_pts, cur_un_pts;
    vector<cv::Point2f> pts_velocity;//特征点在像素上的帧间运动速度
    vector<int> ids;
    vector<int> track_cnt;
    map<int, cv::Point2f> cur_un_pts_map;//使前后两帧特征点联系起来的数据结构
    map<int, cv::Point2f> prev_un_pts_map;
    camodocal::CameraPtr m_camera;//相机模型的实现，位于camera_model包内
    double cur_time;//时间戳
    double prev_time;

    static int n_id;//纯计数用的

//主要的处理图像数据的模块
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    if (EQUALIZE)//为了使特征点与光流更加稳定，默认采用直方图均衡
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

    //将本帧图像浅拷贝
    if (forw_img.empty())
    {
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        forw_img = img;
    }

    forw_pts.clear();

    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;

        //采用LK光流法做跟踪，源码在这里注释了一条，尝试了不同尺寸的搜索窗口
        // cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(11, 11), 3);

        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))
                status[i] = 0;

        //根据每个点跟踪的效果，将跟踪失败或已经出界的点去除
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);
        reduceVector(cur_un_pts, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }

    for (auto &n : track_cnt)
        n++;

    if (PUB_THIS_FRAME)
    {
        //在这里对切向畸变和径向畸变都做了消除。然后使用了ransac方法，对前后两帧的特征点计算基础矩阵并剔除掉错误跟踪的点
        rejectWithF();
        
        //对鱼眼相机和针孔相机,指定特征点检测区域，针孔相机就检测全部区域
        setMask();
        
        //最大的跟踪数目，被作者设置为400，当跟踪到的点不足数目时，将会对新的一帧再次采样，补齐特征点数目
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.05, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        
        //预备下一次跟踪的相应数据，补齐即将跟踪的特征点编号
        addPoints();
    }
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    cur_img = forw_img;
    cur_pts = forw_pts;

    //对当前帧去运动畸变，同时计算两帧对应特征点的像素运动速度留给estimator备用
    undistortedPoints();
    prev_time = cur_time;
}

在分析完处理图像的模块后，我们发现这个进程需要发送特征信息给下一个进程，以下是要发出去的rosmsg，它的points部分不必说，是特征点的坐标，而下面的一些channel也是有用的信息要一并发过去的。

sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;
sensor_msgs::ChannelFloat32 v_of_point;
sensor_msgs::ChannelFloat32 velocity_x_of_point;
sensor_msgs::ChannelFloat32 velocity_y_of_point;

二、estimator进程（r2live）

状态估计的部分我们依然结合论文观看，还是这两个加减符号，定义了状态向量在它的邻域与正切空间内的变化趋势。

 为了避免一篇太长，我们下一篇再来详细阅读状态估计部分。