We want to change the parent(start frame) of a feature point.
在对VINS改进的时候，我们需要改变它优化的过程。而VINS是基于逆深度优化的，逆深度的值又是由每个特征点最初观测到是帧的位置决定的（在代码中由start frame表示，而我们下文中有时候会用parent来表示，因为特征点可以看作这一帧的子，而特征的优化完全由这个第一个观察的帧决定，所以把它称作parent非常贴切）。所以想要改变VINS的优化核心是要正确的改变每一个点的parent。
下面我会先讲VINS系统中本身对这个start_frame的管理内容，然后拓展这个函数到任意的调整。
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; } } } }这里实现的是调整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; } } } }