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;
}