VINS 估计器之检查视差

为什么检查视差

VINS里为了控制优化计算量，在实时情况下，只对当前帧之前某一部分帧进行优化，而不是全部历史帧。局部优化帧的数量就是窗口大小。为了维持窗口大小，需要去除旧的帧添加新的帧，也就是边缘化 Marginalization。到底是删去最旧的帧（MARGIN_OLD）还是删去刚刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)，就需要对当前帧与之前帧进行视差比较，如果是当前帧变化很小，就会删去倒数第二帧，如果变化很大，就删去最旧的帧。

VINS 里把特征点管理和检查视差放在了同一个函数里，先添加特征点，再进行视差检查。所以今天主要看的是这个函数：

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)；

在哪里需要检查视差

视差检查函数使用位置是在processImage内，也就是当estimator得到一帧图片的信息后，就会立马进行处理，首先第一步就是添加图片内的特征点以及检查视差。

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)

{

    ROS_DEBUG("new image coming ------------------------------------------");

    ROS_DEBUG("Adding feature points %lu", image.size());

    if (f_manager.addFeatureCheckParallax(frame_count, image, td))

        marginalization_flag = MARGIN_OLD;

    else

        marginalization_flag = MARGIN_SECOND_NEW;

...

如何检查视差

分析函数内代码

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

{

    ROS_DEBUG("input feature: %d", (int)image.size());

    ROS_DEBUG("num of feature: %d", getFeatureCount());

    //所有特征点视差总和

    double parallax_sum = 0;

    // 满足某些条件的特征点个数

    int parallax_num = 0;

    //被跟踪点的个数

    last_track_num = 0;

    for (auto &id_pts : image)

    {

        //特征点管理器，存储特征点格式：首先按照特征点ID，一个一个存储，每个ID会包含其在不同帧上的位置

        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

        int feature_id = id_pts.first;

       // find_if 函数，找到一个interator使第三个仿函数参数为真

        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)

                          {

            return it.feature_id == feature_id;

                          } );

        if (it == feature.end())

        {

            //如果没有找到此ID，就在管理器中增加此特征点

            feature.push_back(FeaturePerId(feature_id, frame_count));

            feature.back().feature_per_frame.push_back(f_per_fra);

        }

        else if (it->feature_id == feature_id)

        {

            //如果找到了相同ID特征点，就在其FeaturePerFrame内增加此特征点在此帧的位置以及其他信息，然后增加last_track_num，说明此帧有多少个相同特征点被跟踪到

            it->feature_per_frame.push_back(f_per_fra);

            last_track_num++;

        }

    }

    if (frame_count < 2 || last_track_num < 20)

        return true;

    for (auto &it_per_id : feature)

    {

        //计算能被当前帧和其前两帧共同看到的特征点视差

        if (it_per_id.start_frame <= frame_count - 2 &&

            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)

        {

            parallax_sum += compensatedParallax2(it_per_id, frame_count);

            parallax_num++;

        }

    }

    if (parallax_num == 0)

    {

        return true;

    }

    else

    {

        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);

        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);

        return parallax_sum / parallax_num >= MIN_PARALLAX;

    }

}

每个特征点视差计算如下：

double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count)

{

    //check the second last frame is keyframe or not

    //parallax betwwen second last frame and third last frame

    const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];

    const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 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;

    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;

}

巴特西

VINS 估计器之检查视差

为什么检查视差

在哪里需要检查视差

如何检查视差

最新文章

热门文章