- 和前一帧的平均视差。如果跟踪特征的平均视差超过某个阈值,我们会将此图像视为关键帧。
- 跟踪质量。如果跟踪特征的数量低于一个阈值,我们把这一帧看做一个新的关键帧。
具体代码如下:
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)
{
// 用特征点信息构造一个对象
FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
int feature_id = id_pts.first;
// 在已有的id中寻找是否是有相同的特征点
auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
{
return it.feature_id == feature_id;
});
// 这是一个新的特征点
if (it == feature.end())
{
// 在特征点管理器中,新创建一个特征点id,这里的frame_count就是该特征点在滑窗中的当前位置,作为这个特征点的起始位置
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)
{
it->feature_per_frame.push_back(f_per_fra);
last_track_num++; // 追踪到上一帧的特征点数目
}
}
// 前两帧都设置为KF,追踪次数小于20也认为是KF (跟踪质量)
if (frame_count < 2 || last_track_num < 20)
return true;
// 计算每个特征在次新帧和次次新帧中的视差
for (auto &it_per_id : feature)
{
// 计算的实际上是frame_count-1,也就是前一帧是否为关键帧
// 因此起始帧至少得是frame_count - 2,同时至少覆盖到frame_count - 1帧
// 观测该特征点的: 起始帧小于倒数第三帧,终止帧大于倒数第二帧,保证至少有两帧能观测到
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)
{
// 总视差:该特征点在两帧的归一化平面上的坐标点的距离ans (平均视差)
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;
}
}