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