Open_Vins学习（一）前端tracking

简介

open_vins的前端主要有三种方式：(1)光流KLT，描述子和Aruco跟踪。在MSCKF中默认使用的是光流跟踪。open_vins的前端可以参考ov_core中的test_tracking文件。

1.Feature类

    class Feature {
    public:
        /// 特征点的ID
        size_t featid;
        /// 是否从数据库中删除的标志位
        bool to_delete;
        
        /// 特征点在每一次被观测到时的像素坐标
        std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs;
        /// 特征点在每一次被观测到时的去畸变像素坐标
        std::unordered_map<size_t, std::vector<Eigen::VectorXf>> uvs_norm;
        /// 特征点在每一次被观测到时的时间戳信息，后面MSCKF过程中计算特征点的生命周期只需要计算vector的size就可以。
        std::unordered_map<size_t, std::vector<double>> timestamps;

        /// 特征点坐标相对于哪一个局部坐标系，默认是第一帧
        int anchor_cam_id = -1;
        /// Timestamp of anchor clone
        double anchor_clone_timestamp;

        /// 在局部坐标系anchor frame 的三维坐标，三角化之后的
        Eigen::Vector3d p_FinA;
        /// 在全局坐标的三角化Pose
        Eigen::Vector3d p_FinG;
    };

2.光流KLT跟踪

/**
// 此函数为前端主体函数
// KLT跟踪分为以下步骤：
1.对图像直方图均衡化
2.构建图像金字塔
3.对当前图像补充新的特征点保障KLT有足够多的点
4.KLT光流追踪
5.更新database中的一些变量，对于一直跟踪的点，记录被观测到的局部坐标和当前帧时间戳，用于统计特征点的生命周期；新提取到的点则加入到数据库中
***/
void TrackKLT::feed_monocular(double timestamp, cv::Mat &img, size_t cam_id)
{
    // 1. 直方图均衡化用来增强图像质量
    cv::equalizeHist(img, img);
    // 2. 构建3层图像金字塔
    std::vector<cv::Mat> imgpyr;
    cv::buildOpticalFlowPyramid(img, imgpyr, win_size, pyr_levels);
    
    // 3. pts_last存储的是上一帧提取到的特征点，首先进行第一帧初始化提取特征点和对应的id
    if(pts_last[cam_id].empty()) {
        // 通过输入的图像金字塔来检测fast特征点和特征点的id信息
        perform_detection_monocular(imgpyr, pts_last[cam_id], ids_last[cam_id]);
        // Save the current image and pyramid
        img_last[cam_id] = img.clone();
        img_pyramid_last[cam_id] = imgpyr;
        return;
    }
    // 为了确保上一帧的图像有足够多的特征点用来KLT
    perform_detection_monocular(img_pyramid_last[cam_id], pts_last[cam_id], ids_last[cam_id]);
    // 4. 光流跟踪，mask_ll返回特征点是否成功track的标志位
    std::vector<uchar> mask_ll;
    std::vector<cv::KeyPoint> pts_left_new = pts_last[cam_id];

    // Lets track temporally
    perform_matching(img_pyramid_last[cam_id],imgpyr,pts_last[cam_id],pts_left_new,cam_id,cam_id,mask_ll);
    // 标志位容器为空那么跟踪失败，则把当前图像金字塔赋值给img_pyramid_last用于下一帧跟踪
    if(mask_ll.empty()) {
        img_last[cam_id] = img.clone();
        img_pyramid_last[cam_id] = imgpyr;
        pts_last[cam_id].clear();
        ids_last[cam_id].clear();
        ROS_ERROR("[KLT-EXTRACTOR]: Failed to get enough points to do RANSAC, resetting.....");
        return;
    }
    
    // 5.把跟踪成功的点提取出来用于后面的更新database中的特征
    std::vector<cv::KeyPoint> good_left;
    std::vector<size_t> good_ids_left;

    // Loop through all left points
    for(size_t i=0; i<pts_left_new.size(); i++) {
        // Ensure we do not have any bad KLT tracks (i.e., points are negative)
        if(pts_left_new[i].pt.x < 0 || pts_left_new[i].pt.y < 0)
            continue;
        // If it is a good track, and also tracked from left to right
        if(mask_ll[i]) {
            good_left.push_back(pts_left_new[i]);
            good_ids_left.push_back(ids_last[cam_id][i]);
        }
    }
    // 更新数据库中的特征
    //update_feature()：如果特征点数据库中没有该特征，则新创建一个feature对象。若有则把更新
    for(size_t i=0; i<good_left.size(); i++) {
        cv::Point2f npt_l = undistort_point(good_left.at(i).pt, cam_id);
        database->update_feature(good_ids_left.at(i), timestamp, cam_id,
                                 good_left.at(i).pt.x, good_left.at(i).pt.y,
                                 npt_l.x, npt_l.y);
    }
    // 前后帧信息clone
    img_last[cam_id] = img.clone();
    img_pyramid_last[cam_id] = imgpyr;
    pts_last[cam_id] = good_left;
    ids_last[cam_id] = good_ids_left;
}

3.Fast特征点提取策略

open_vins提取fast特征点，值得学习的是把图像分成尺寸相同的网格，然后对这些网格并行提取特征点。采用占据网格的方法均匀化提取特征点，在每个网格中只能有一个特征点，在EuRoc数据集上设置的默认值是10*10像素的网格。

// perform_detection_monocular在每一帧图像到来时提取特征点，虽然KLT已经追踪到一些特征点，但是要保证每一帧都有足够的特征点进行KLT，所以要新加一些特征点。
void TrackKLT::perform_detection_monocular(const std::vector<cv::Mat> &img0pyr, std::vector<cv::KeyPoint> &pts0, std::vector<size_t> &ids0) {
    // 这里保证一个网格只能有一个点
    Eigen::MatrixXi grid_2d_current = Eigen::MatrixXi::Zero((int)(img0pyr.at(0).cols / min_px_dist) + 10, (int)			(img0pyr.at(0).rows / min_px_dist) + 10);
    auto it0 = pts0.begin();
    auto it2 = ids0.begin();
    while(it0 != pts0.end()) {
        // Get current left keypoint
        cv::KeyPoint kpt = *it0;
        // Check if this keypoint is near another point
        // 如果这个格子中有特征点，则剔除
        if(grid_2d_current((int)(kpt.pt.x/min_px_dist),(int)(kpt.pt.y/min_px_dist)) == 1) {
            it0 = pts0.erase(it0);
            it2 = ids0.erase(it2);
            continue;
        }
        // 设置grid以被占用
        grid_2d_current((int)(kpt.pt.x/min_px_dist),(int)(kpt.pt.y/min_px_dist)) = 1;
        it0++;
        it2++;
    }

    // 计算需要提取的特征数量，EuRoc设置为1000
    int num_featsneeded = num_features - (int)pts0.size();

    // If we don't need any features, just return
    if(num_featsneeded < 1)
        return;

    // Extract our features (use fast with griding)
    // 使用网格均匀化提取并行特征特征点
    // 将图像划分为grid_x*grid_y个网格，再这些网格中并行的提取特征点保存到pts0_ext
    std::vector<cv::KeyPoint> pts0_ext;
    Grider_FAST::perform_griding(img0pyr.at(0), pts0_ext, num_featsneeded, grid_x, grid_y, threshold, true);

    // Create a 2D occupancy grid for this current image
    // Note that we scale this down, so that each grid point is equal to a set of pixels
    // This means that we will reject points that less then grid_px_size points away then existing features
    // 创建一个2D网格，重新计算占据网格的情况
    Eigen::MatrixXi grid_2d = Eigen::MatrixXi::Zero((int)(img0pyr.at(0).cols / min_px_dist) + 10, (int)(img0pyr.at(0).rows / min_px_dist) + 10);
    for(auto& kpt : pts0) {
        grid_2d((int)(kpt.pt.x/min_px_dist),(int)(kpt.pt.y/min_px_dist)) = 1;
    }

    // 将新提取到的特征点分配到网格中保证每个网格最多有一个点
    std::vector<cv::KeyPoint> kpts0_new;
    std::vector<cv::Point2f> pts0_new;
    for(auto& kpt : pts0_ext) {
        // See if there is a point at this location
        if(grid_2d((int)(kpt.pt.x/min_px_dist),(int)(kpt.pt.y/min_px_dist)) == 1)
            continue;
        // Else lets add it!
        kpts0_new.push_back(kpt);
        pts0_new.push_back(kpt.pt);
        grid_2d((int)(kpt.pt.x/min_px_dist),(int)(kpt.pt.y/min_px_dist)) = 1;
    }

    // Loop through and record only ones that are valid
    // 最后把新提取到的点分配到pts0的末尾
    for(size_t i=0; i<pts0_new.size(); i++) {
        // update the uv coordinates
        kpts0_new.at(i).pt = pts0_new.at(i);
        // append the new uv coordinate
        pts0.push_back(kpts0_new.at(i));
        // move id foward and append this new point
        size_t temp = ++currid;
        ids0.push_back(temp);
    }

}

参考

1.https://www.zhihu.com/people/mao-shu-yuan
2.https://www.zhihu.com/people/anson2004110
3.https://github.com/rpng/open_vins
4.OpenVins官方文档