VINS-FUSION 基于视差Parallax关键帧

云端舞步

于 2024-07-22 16:55:43 发布

阅读量311

点赞数 12

分类专栏： VINS-FUSION 文章标签： VINS-Fusion 视差

本文链接：https://blog.csdn.net/u010196944/article/details/136204780

版权

VINS-FUSION 专栏收录该内容

25 篇文章 20 订阅

订阅专栏

VINS-FUSION中基于视差Parallax关键帧，主要是在函数addFeatureCheckParallax中完成，其结果作为滑窗去除帧的依据。

在滑动窗口中，总是保持窗口总体大小不变，当新进来一帧，通过函数addFeatureCheckParallax判断，如果属于关键帧，则去除窗口中最老帧，置标志MARGIN_OLD；如果不属于关键帧，则去除窗口中次新帧，置标志MARGIN_SECOND_NEW。通过该种策略完成滑窗过程。

函数addFeatureCheckParallax

bool FeatureManager::addFeatureCheckParallax(int _frameCount, 
    const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &_image, double td) {

输入参数：

_frameCount：滑窗中帧数；

_image：特征点结构，{FeatureID, vector<{CameraID, (x, y, z, u, v, vx, vy)}>}；

td: camera和IMU数据时间偏移。

1. 函数首先遍历特征点

a.如果是双目则将左右目特征信息放入类型为FeaturePerFrame的变量f_per_fra中; 在文章“VINS-Fusion features特征点管理”中，知道FeaturePerFrame类型变量表征当前特征点在一帧图像的表达。

b.在滑窗特征点容器features_中找当前帧id_pts.first

(1)如果未找到，表明该特征点id_pts.first为新的特征点，将该特征点加入容器features_，并将新特征点计数new_feature_num_加1。

features_.push_back(FeaturePerId(feature_id, _frameCount)); features_.back().feature_per_frame.push_back(f_per_fra);

在文章“VINS-Fusion features特征点管理”中，知道类型FeaturePerId铆钉特征点，保存了特征点在所有帧中的观测信息。传入参数为该特征点序号及保存该特征点的起始帧序号start_frame，再将该特征点在该帧图像的表达压入vector<FeaturePerFrame> feature_per_frame；

features_.back().feature_per_frame获取容器features_最后节点的引用，再加该特征点在图像的表达f_per_fra压入features_.back().feature_per_frame。

简单理解就是将数据，按照其类型填充到FeaturePerId中。

(2)如果找到了，说明该特征点为老特征点。

则只需将该特征点在图像的表达f_per_fra压入对应找到的it.feature_per_frame中，并标记为已追踪到的特征点计数last_track_num加1，若it-> feature_per_frame.size() >= 4，则标记为被多帧追踪到的特征点计数long_track_num加1。

从这里可以更清楚理解特征点FeaturePerId结构中vector<FeaturePerFrame> feature_per_frame，feature_per_frame保存了该特征点所有图像帧的观测信息。

    // id_pts: {FeatID, vector{FeatPointInLeftImage, FeatPointInRightImage}}
    for (auto &id_pts /*one feature*/ : _image) {
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td); /*左目*/
        assert(id_pts.second[0].first == 0);
        if(id_pts.second.size() == 2) /*双目时也保存右目*/ {
            f_per_fra.rightObservation(id_pts.second[1].second);
            assert(id_pts.second[1].first == 1);
        }

        int feature_id = id_pts.first;
        auto it = find_if(features_.begin(), features_.end(), 
            [feature_id](const FeaturePerId &it) {return it.feature_id == feature_id;});

        if (it == features_.end()) {
            features_.push_back(FeaturePerId(feature_id, _frameCount));
            features_.back().feature_per_frame.push_back(f_per_fra);
            new_feature_num_++;
        }
        else if (it->feature_id == feature_id) {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num_++;
            if( it-> feature_per_frame.size() >= 4)
                long_track_num_++;
        }
    }

c.根据特征点追踪次数判断关键帧

若滑窗少于两帧，或者last_track_num < 20, 或者long_track_num < 40, 或者new_feature_num_ > 0.5 * last_track_num_新特征点超过一半的老特征点数量，则认为该帧为关键帧。

if (_frameCount < 2 || last_track_num_ < 20 || long_track_num_ < 40 
	|| new_feature_num_ > 0.5 * last_track_num_) {
	return true;
}

d.计算视差

两帧之前的特征点，一直跟踪到了当前帧，计算当前特征点在前两帧中归一化平面上的距离parallax_sum，并记录当前帧满足该条件的特征数量parallax_num。

if (it_per_id.start_frame <= _frameCount - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= _frameCount - 1)

frame_count保持为windows大小10，滑窗始终保持10帧（0-9）。即计算视差的特征点至少要在当前帧的前两帧被观测到。如果满足条件，则利用观测到该特征点的前两帧计算视差。

double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int _frameCount) {
    //check the second last frame is keyframe or not
    //parallax betwwen seconde last frame and third last frame
    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[_frameCount - 2 - it_per_id.start_frame];
    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[_frameCount - 1 - it_per_id.start_frame];

    double ans = 0;
    Vector3d p_j = frame_j.point;

    double u_j = p_j(0);
    double v_j = p_j(1);

    Vector3d p_i = frame_i.point;
    Vector3d p_i_comp;

    //int r_i = _frameCount - 2;
    //int r_j = _frameCount - 1;
    //p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
    p_i_comp = p_i;
    double dep_i = p_i(2);
    double u_i = p_i(0) / dep_i;
    double v_i = p_i(1) / dep_i;
    double du = u_i - u_j, dv = v_i - v_j;

    double dep_i_comp = p_i_comp(2);
    double u_i_comp = p_i_comp(0) / dep_i_comp;
    double v_i_comp = p_i_comp(1) / dep_i_comp;
    double du_comp = u_i_comp - u_j, dv_comp = v_i_comp - v_j;

    ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));

    return ans;
}

假设该特征点在滑窗被观测到的起始帧为第1帧，则frame_i = 7, frame_j = 8; 起始帧为第7帧，则frame_i = 1, frame_j = 2;
则，无论该特征点在滑窗中被全部帧观测到，还是只有最后两帧观测到，都是用该特征点在滑窗内被观测到的最后两帧计算视差。

计算两帧的视差，其中将一个特征点坐标除以深度再以另一个特征点做差。从几何上理解，当该特征点横纵坐标同除一个数（深度），只是将该特征点在像素坐标系下等比缩放一个比例。

e.根据视差判断是否为关键帧

如果该特征点数量为0，证明该帧中所有特征点不满足计算视差的条件，特征为新加入的特征点，判断为关键帧。

其它，计算该帧中满足条件特征点的平均视差，若平均视差超过设定阈值，则判断为关键帧。

if (parallax_num == 0) {
        return true;
}
else {
        // printf("[ DBG ] parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        // printf("[ DBG ] current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        last_average_parallax_ = parallax_sum / parallax_num * FOCAL_LENGTH;
        return parallax_sum / parallax_num >= MIN_PARALLAX; /*如果平均视差超过阈值，认定为出现新关键帧*/
}

整体函数实现如下

bool FeatureManager::addFeatureCheckParallax(int _frameCount, 
    const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &_image, double td) {

    // printf("[ DBG ] input feature: %d", (int)_image.size());
    // printf("[ DBG ] num of feature: %d", getRobustFeatureCount());

    double parallax_sum = 0;
    int parallax_num = 0;
    last_track_num_ = 0;
    last_average_parallax_ = 0;
    new_feature_num_ = 0;
    long_track_num_ = 0;

    // id_pts: {FeatID, vector{FeatPointInLeftImage, FeatPointInRightImage}}
    for (auto &id_pts /*one feature*/ : _image) {
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td); /*左目*/
        assert(id_pts.second[0].first == 0);
        if(id_pts.second.size() == 2) /*双目时也保存右目*/ {
            f_per_fra.rightObservation(id_pts.second[1].second);
            assert(id_pts.second[1].first == 1);
        }

        int feature_id = id_pts.first;
        auto it = find_if(features_.begin(), features_.end(), 
            [feature_id](const FeaturePerId &it) {return it.feature_id == feature_id;});

        if (it == features_.end()) {
            features_.push_back(FeaturePerId(feature_id, _frameCount));
            features_.back().feature_per_frame.push_back(f_per_fra);
            new_feature_num_++;
        }
        else if (it->feature_id == feature_id) {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num_++;
            if( it-> feature_per_frame.size() >= 4)
                long_track_num_++;
        }
    }

    //if (_frameCount < 2 || last_track_num_ < 20)
    //if (_frameCount < 2 || last_track_num_ < 20 || new_feature_num_ > 0.5 * last_track_num_)

    // 如果满足4个条件之一，直接认为是关键帧
    if (_frameCount < 2 || last_track_num_ < 20 || long_track_num_ < 40 
        || new_feature_num_ > 0.5 * last_track_num_) {
        return true;
    }

    // 仅对已经被多次观测到的feature们，计算一个平均视差
    for (auto &it_per_id : features_) {
        if (it_per_id.start_frame <= _frameCount - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= _frameCount - 1) {
            parallax_sum += compensatedParallax2(it_per_id, _frameCount);
            parallax_num++;
        }
    }

    // 根据视差判断是否为关键帧
    if (parallax_num == 0) {
        return true;
    }
    else {
        // printf("[ DBG ] parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        // printf("[ DBG ] current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        last_average_parallax_ = parallax_sum / parallax_num * FOCAL_LENGTH;
        return parallax_sum / parallax_num >= MIN_PARALLAX; /*如果平均视差超过阈值，认定为出现新关键帧*/
    }
}

参考：

https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

云端舞步

关注

12
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
VINS-FUSION 基于视差Parallax关键帧

VINS-FUSION中基于视差Parallax关键帧，主要是在函数addFeatureCheckParallax中完成，其结果作为滑窗去除帧的依据。在滑动窗口中，总是保持窗口总体大小不变，当新进来一帧，通过函数addFeatureCheckParallax判断，如果属于关键帧，则去除窗口中最老帧，置标志MARGIN_OLD；如果不属于关键帧，则去除窗口中次新帧，置标志MARGIN_SECOND_NEW。通过该种策略完成滑窗过程。
复制链接

扫一扫