VINS frontend - change feature's parent

Introduction

We want to change the parent(start frame) of a feature point.

在對VINS改進的時候，我們需要改變它優化的過程。而VINS是基於逆深度優化的，逆深度的值又是由每個特徵點最初觀測到是幀的位置決定的（在代碼中由start frame表示，而我們下文中有時候會用parent來表示，因爲特徵點可以看作這一幀的子，而特徵的優化完全由這個第一個觀察的幀決定，所以把它稱作parent非常貼切）。所以想要改變VINS的優化核心是要正確的改變每一個點的parent。

下面我會先講VINS系統中本身對這個start_frame的管理內容，然後拓展這個函數到任意的調整。

VINS feature manager

VINS中當需要邊緣化某一幀的時候就需要修改這一幀的子特徵(以這一幀爲start frame的特徵點)。而邊緣化有兩種： 邊緣化最舊的一幀和邊緣化比較新的幀。

他們分別通過如下的函數實現：

• 移除舊的一幀（特別注意這是在solve flag ！= Nonlinear optimization時調用的，也就是在初始化過程中調用的。優化時調用的是removeBackShiftDepth函數，在這一章的最後介紹。）

// remove the oldest frame's children feature detection
void FeatureManager::removeBack()
{
    // loop through all the features
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;
        // if this feature's parent is not the oldest frame
        // simply minus one will be fine, as the oldest frame deleted
        // all the frame's index will minus one
        if (it->start_frame != 0)
            it->start_frame--;
        else
        {
            // the system is not in the nonlinear optimization state
            // simply delete the observation of the point
            // if the points not observed anymore, delete it. 
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

void FeatureManager::removeFront(int frame_count)
{
    for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next)
    {
        it_next++;

        if (it->start_frame == frame_count)
        {
            it->start_frame--;
        }
        else
        {
            int j = WINDOW_SIZE - 1 - it->start_frame;
            if (it->endFrame() < frame_count - 1)
                continue;
            it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
            if (it->feature_per_frame.size() == 0)
                feature.erase(it);
        }
    }
}

原諒我比較懶。。懶得把自己的英文再翻譯過來了。。。詳細的解釋參見下面的註釋內容。

void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P)
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;

        if (it->start_frame != 0)
            it->start_frame--;
        else
        {
            // However if the point's parent is this frame
            // The story become more complicate
            // we first delete the observation of the point
            // And as it is a old point, its observation count must larger than 2
            // otherwise we cannot optimize its pose. we will erase it.
            // (for the new point it is possible to see it in the future)
            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  
            it->feature_per_frame.erase(it->feature_per_frame.begin());
            if (it->feature_per_frame.size() < 2)
            {
                feature.erase(it);
                continue;
            }
            else
            {
                // if needs to optimize the point, we will shift its depth from the old parent
                // to the new parent (from marg_pose to new_pose)
                // 1st calcualte the world position of the point
                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
                Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
                // 2nd transform the point to the new pose frame
                Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
                // 3rd assign the inverse depth
                double dep_j = pts_j(2);
                if (dep_j > 0)
                    it->estimated_depth = dep_j;
                else
                    it->estimated_depth = INIT_DEPTH;
            }
        }
    }
}

Switch parent

這裏實現的是調整parent的過程。

void FeatureManager::switchParent(Eigen::Matrix3d marg_R, Eigen::Vector3d marg_P, Eigen::Matrix3d new_R, Eigen::Vector3d new_P, int old_idx, int new_idx)
{
    for (auto it = feature.begin(), it_next = feature.begin();
         it != feature.end(); it = it_next)
    {
        it_next++;

        if (it->start_frame != old_idx)
            continue;
        else
        {
            it->start_frame = new_idx;
            Eigen::Vector3d uv_i = it->feature_per_frame[0].point;  
            {
                // if needs to optimize the point, we will shift its depth from the old parent
                // to the new parent (from marg_pose to new_pose)
                // 1st calcualte the world position of the point
                Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
                Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
                // 2nd transform the point to the new pose frame
                Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
                // 3rd assign the inverse depth
                double dep_j = pts_j(2);
                if (dep_j > 0)
                    it->estimated_depth = dep_j;
                else
                    it->estimated_depth = INIT_DEPTH;
            }
        }
    }
}