Tracking_Module()是该SLAM的前端，主要任务是得到新获取图像的视觉特征与局部地图中的路标点之间的匹配关系，进一步通过优化法PnP计算得到当前时刻相机位姿，并利用一定的规则选择关键帧。流程如下：
图像处理
此步骤将灰度图像(如果输入为彩色图像，将转换成灰度图像)信息转换成包含FAST特征点和描述子的帧(frame)数据，首先通过下采样得到图像的金字塔，其中原图为金字塔0层，然后在各层级金字塔上提取ORB特征点并计算对应描述子。提取特征点过程中利用树结构将特征点按照坐标分配至各层树中，并利用非极大值抑制方法以使得最终的特征点分配均匀。(猜测:越低层级金字塔的特征点，距离相机越远）
初始化
初始化目的是利用初始时刻的观测信息创建用于后续跟踪的初始地图。对于RGBD或者双目而已，可以直接得到特征点的深度，进而得到对应的路标点，而对于单目而言，需要在两帧之间利用H矩阵或者F矩阵进行初始化。最后将初始化产生的路标点和涉及的图像帧转换成关键帧，存储到地图库中。
跟踪当前帧
此步骤利用三种方式跟踪上一帧或者关键帧，建立局部地图3D路标点与当前帧2D特征点匹配(match)，并利用优化PnP获得当前帧初始位姿(optimize)，优化法PnP也就是将路标点固定的BA。
| motion_based_track() |
unsigned int projection::match_current_and_last_frames(data::frame& curr_frm, const data::frame& last_frm,
const float margin) const
{
unsigned int num_matches = 0;
angle_checker<int> angle_checker;
const Mat33_t rot_cw = curr_frm.cam_pose_cw_.block<3, 3>(0, 0);
const Vec3_t trans_cw = curr_frm.cam_pose_cw_.block<3, 1>(0, 3);
const Vec3_t trans_wc = -rot_cw.transpose() * trans_cw;
const Mat33_t rot_lw = last_frm.cam_pose_cw_.block<3, 3>(0, 0);
const Vec3_t trans_lw = last_frm.cam_pose_cw_.block<3, 1>(0, 3);
const Vec3_t trans_lc = rot_lw * trans_wc + trans_lw;
//判断当前时刻相对上一时刻是在前进还是后退。其中monocular没有真实的平移尺度，无法判断当前相对上一时刻是前进还是后退
const bool assume_forward = (curr_frm.camera_->setup_type_ == camera::setup_type_t::Monocular ||
curr_frm.camera_->setup_type_ == camera::setup_type_t::Monocular_Inertial)
? false
: trans_lc(2) > curr_frm.camera_->true_baseline_;
const bool assume_backward = (curr_frm.camera_->setup_type_ == camera::setup_type_t::Monocular ||
curr_frm.camera_->setup_type_ == camera::setup_type_t::Monocular_Inertial)
? false
: -trans_lc(2) > curr_frm.camera_->true_baseline_;
// 将上一帧的特征点投影到当前帧，寻找匹配
for (unsigned int idx_last = 0; idx_last < last_frm.num_keypts_; ++idx_last) {
auto* lm = last_frm.landmarks_.at(idx_last);
if (!lm) {
continue;
}
if (last_frm.outlier_flags_.at(idx_last)) {
continue;
}
const Vec3_t pos_w = lm->get_pos_in_world();
// 再投影して可視性を求める
Vec2_t reproj;
float x_right;
const bool in_image = curr_frm.camera_->reproject_to_image(rot_cw, trans_cw, pos_w, reproj, x_right);
if (!in_image) {
continue;
}
// 上一帧上检测到该特征点对应的金字塔层级
const auto last_scale_level = last_frm.keypts_.at(idx_last).octave;
//针对不同的运动假设（前进或者后退），在当前帧不同层级金字塔上寻找匹配的特征点
//curr_frm.get_keypoints_in_cell(投影位置x, 投影位置y,搜索范围,搜索金字塔最小层, 搜索金字塔最大层)
std::vector<unsigned int> indices;
if (assume_forward) {
//当前帧相对上一帧在前进，路标点位置距离相机更近了，则在更高金字塔层级搜索
indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1),
margin * curr_frm.scale_factors_.at(last_scale_level),
last_scale_level, last_frm.num_scale_levels_ - 1);
} else if (assume_backward) {
//当前帧相对上一帧在后退，路标点位置距离相机更远了，则在更低层级搜索
indices = curr_frm.get_keypoints_in_cell(
reproj(0), reproj(1), margin * curr_frm.scale_factors_.at(last_scale_level), 0, last_scale_level);
} else {
//运动变化不大，则在当前金字塔层级搜索
indices = curr_frm.get_keypoints_in_cell(reproj(0), reproj(1),
margin * curr_frm.scale_factors_.at(last_scale_level),
last_scale_level - 1, last_scale_level + 1);
}
if (indices.empty()) {
continue;
}
const auto lm_desc = lm->get_descriptor();
unsigned int best_hamm_dist = MAX_HAMMING_DIST;
int best_idx = -1;
for (const auto curr_idx : indices) {
if (curr_frm.landmarks_.at(curr_idx) && curr_frm.landmarks_[curr_idx]->has_observation()) {
continue;
}
if (curr_frm.stereo_x_right_.at(curr_idx) > 0) {
const float reproj_error = std::fabs(x_right - curr_frm.stereo_x_right_.at(curr_idx));
if (margin * curr_frm.scale_factors_.at(last_scale_level) < reproj_error) {
continue;
}
}
const auto& desc = curr_frm.descriptors_.row(curr_idx);
const auto hamm_dist = compute_descriptor_distance_32(lm_desc, desc);
if (hamm_dist < best_hamm_dist) {
best_hamm_dist = hamm_dist;
best_idx = curr_idx;
}
}
if (HAMMING_DIST_THR_HIGH < best_hamm_dist) {
continue;
}
curr_frm.landmarks_.at(best_idx) = lm;
++num_matches;
if (check_orientation_) {
const auto delta_angle =
last_frm.undist_keypts_.at(idx_last).angle - curr_frm.undist_keypts_.at(best_idx).angle;
angle_checker.append_delta_angle(delta_angle, best_idx);
}
}
if (check_orientation_) {
const auto invalid_matches = angle_checker.get_invalid_matches();
for (const auto invalid_idx : invalid_matches) {
curr_frm.landmarks_.at(invalid_idx) = nullptr;
--num_matches;
}
}
return num_matches;
}
| bow_based_track() |
unsigned int bow_tree::match_frame_and_keyframe(data::keyframe* keyfrm, data::frame& frm,
std::vector<data::landmark*>& matched_lms_in_frm) const
{
unsigned int num_matches = 0;
angle_checker<int> angle_checker;
matched_lms_in_frm = std::vector<data::landmark*>(frm.num_keypts_, nullptr);
const auto keyfrm_lms = keyfrm->get_landmarks();
spdlog::debug("reference keyframe landmark size is {}", keyfrm_lms.size());
#ifdef USE_DBOW2
DBoW2::FeatureVector::const_iterator keyfrm_itr = keyfrm->bow_feat_vec_.begin();
DBoW2::FeatureVector::const_iterator frm_itr = frm.bow_feat_vec_.begin();
const DBoW2::FeatureVector::const_iterator kryfrm_end = keyfrm->bow_feat_vec_.end();
const DBoW2::FeatureVector::const_iterator frm_end = frm.bow_feat_vec_.end();
#else
fbow::BoWFeatVector::const_iterator keyfrm_itr = keyfrm->bow_feat_vec_.begin();
fbow::BoWFeatVector::const_iterator frm_itr = frm.bow_feat_vec_.begin();
const fbow::BoWFeatVector::const_iterator kryfrm_end = keyfrm->bow_feat_vec_.end();
const fbow::BoWFeatVector::const_iterator frm_end = frm.bow_feat_vec_.end();
#endif
int count = 0;
while (keyfrm_itr != kryfrm_end && frm_itr != frm_end) {
// BoW treeのノード番号(first)が一致しているか確認する
if (keyfrm_itr->first == frm_itr->first) {
// BoW treeのノード番号(first)が一致していれば，
// 実際に特徴点index(second)を持ってきて対応しているか確認する
const auto& keyfrm_indices = keyfrm_itr->second;
const auto& frm_indices = frm_itr->second;
for (const auto keyfrm_idx : keyfrm_indices) {
// keyfrm_idxの特徴点と3次元点が対応していない場合はスルーする
auto* lm = keyfrm_lms.at(keyfrm_idx);
if (!lm) {
// spdlog::debug("landmark does not exist!");
continue;
}
if (lm->will_be_erased()) {
// spdlog::debug("landmark will be erased!");
continue;
}
// spdlog::debug("{} landmarks are valid", count);
count++;
const auto& keyfrm_desc = keyfrm->descriptors_.row(keyfrm_idx);
unsigned int best_hamm_dist = MAX_HAMMING_DIST;
int best_frm_idx = -1;
unsigned int second_best_hamm_dist = MAX_HAMMING_DIST;
for (const auto frm_idx : frm_indices) {
if (matched_lms_in_frm.at(frm_idx)) {
continue;
}
const auto& frm_desc = frm.descriptors_.row(frm_idx);
const auto hamm_dist = compute_descriptor_distance_32(keyfrm_desc, frm_desc);
if (hamm_dist < best_hamm_dist) {
second_best_hamm_dist = best_hamm_dist;
best_hamm_dist = hamm_dist;
best_frm_idx = frm_idx;
} else if (hamm_dist < second_best_hamm_dist) {
second_best_hamm_dist = hamm_dist;
}
}
if (HAMMING_DIST_THR_LOW < best_hamm_dist) {
continue;
}
// ratio test
if (lowe_ratio_ * second_best_hamm_dist < static_cast<float>(best_hamm_dist)) {
continue;
}
matched_lms_in_frm.at(best_frm_idx) = lm;
if (check_orientation_) {
const auto delta_angle = keyfrm->keypts_.at(keyfrm_idx).angle - frm.keypts_.at(best_frm_idx).angle;
angle_checker.append_delta_angle(delta_angle, best_frm_idx);
}
++num_matches;
// spdlog::debug("num_matches is {}", num_matches);
}
++keyfrm_itr;
++frm_itr;
} else if (keyfrm_itr->first < frm_itr->first) {
// keyfrm_itrのノード番号のほうが小さいので，ノード番号が合うところまでイテレータkeyfrm_itrをすすめる
keyfrm_itr = keyfrm->bow_feat_vec_.lower_bound(frm_itr->first);
} else {
// frm_itrのノード番号のほうが小さいので，ノード番号が合うところまでイテレータfrm_itrをすすめる
frm_itr = frm.bow_feat_vec_.lower_bound(keyfrm_itr->first);
}
}
spdlog::debug("final num_matches is {}", num_matches);
if (check_orientation_) {
const auto invalid_matches = angle_checker.get_invalid_matches();
for (const auto invalid_idx : invalid_matches) {
matched_lms_in_frm.at(invalid_idx) = nullptr;
--num_matches;
}
}
return num_matches;
}
基于恒速模型的跟踪失败后将执行基于词袋的跟踪。该步骤被跟踪对象是最新关键帧，上一帧的作用仅是提供优化的初始位姿。该步骤首先根据当前帧特征点及其描述子，利用K-means无监督聚类方法建立词袋树(bow tree)，然后在不同树的不同层次上寻找当前帧与关键帧之间的匹配特征点。该跟踪方法在词袋数层次范围内搜索匹配点，而基于恒速模型则在空间范围内搜索匹配点。
| robust_based_track() |
unsigned int robust::match_frame_and_keyframe(data::frame& frm, data::keyframe* keyfrm,
std::vector<data::landmark*>& matched_lms_in_frm)
{
// 初期化
const auto num_frm_keypts = frm.num_keypts_;
const auto keyfrm_lms = keyfrm->get_landmarks();
unsigned int num_inlier_matches = 0;
matched_lms_in_frm = std::vector<data::landmark*>(num_frm_keypts, nullptr);
// brute-force matchを計算
std::vector<std::pair<int, int>> matches;
brute_force_match(frm, keyfrm, matches);
// eight-point RANSACでインライアのみを抽出
solve::essential_solver solver(frm.bearings_, keyfrm->bearings_, matches);
solver.find_via_ransac(50, false);
if (!solver.solution_is_valid()) {
return 0;
}
const auto is_inlier_matches = solver.get_inlier_matches();
// 情報を格納する
for (unsigned int i = 0; i < matches.size(); ++i) {
if (!is_inlier_matches.at(i)) {
continue;
}
const auto frm_idx = matches.at(i).first;
const auto keyfrm_idx = matches.at(i).second;
matched_lms_in_frm.at(frm_idx) = keyfrm_lms.at(keyfrm_idx);
++num_inlier_matches;
}
return num_inlier_matches;
}
如果上述两种跟踪方法均失效，则执行鲁棒跟踪方法，其实就是暴力匹配法。
跟踪局部地图
该步骤封装在Optimize_current_frame_with_local_map()函数中，与基于恒速模型跟踪图像帧很近似，将局部地图中的所有地图点投影到当前帧，然后在投影点附近寻找匹配点，最后进行优化PnP。通过这一步可以找到更多的3D-2D匹配，在局部地图与当前帧之间建立更多的视觉联系。
关键帧选择
bool keyframe_inserter::new_keyframe_is_needed(const data::frame& curr_frm, const unsigned int num_tracked_lms,
const data::keyframe& ref_keyfrm) const
{
assert(mapper_);
// mapping module被暂停了，不产生关键帧
if (mapper_->is_paused() || mapper_->pause_is_requested()) {
return false;
}
const auto num_keyfrms = map_db_->get_num_keyframes();
// reference keyframeで観測している3次元点のうち，3視点以上から観測されている3次元点の数を数える
const unsigned int min_obs_thr = (3 <= num_keyfrms) ? 3 : 2;
//可靠路标点：参考关键帧中有一定观测数量的路标点数量
const auto num_reliable_lms = ref_keyfrm.get_num_tracked_landmarks(
min_obs_thr); // Here landmarks from server won't be counted, for it only has no observation at now
// 局部建图模块可否接收新的关键帧
const bool mapper_is_idle = mapper_->get_keyframe_acceptability();
constexpr unsigned int num_tracked_lms_thr = 15;
const float lms_ratio_thr = 0.9;
// 条件A1: 当前帧与上一关键帧之间隔足够多的帧数量，需要产生关键帧
const bool cond_a1 = frm_id_of_last_keyfrm_ + max_num_frms_ <= curr_frm.id_;
// 条件A2: 当前帧与上一关键帧之间隔有最低数量的帧且局部建图模块可以接收新的关键帧
const bool cond_a2 = (frm_id_of_last_keyfrm_ + min_num_frms_ <= curr_frm.id_) && mapper_is_idle;
// 条件A3: 跟踪上的路标点数量低于可靠路标点数量的四分之一，此时当前帧跟踪的数量太少，需要产生关键帧
const bool cond_a3 = num_tracked_lms < num_reliable_lms * 0.25;
// 条件B:跟踪的路标点数量大于设定阈值且小于可靠路标点数量的90%
bool cond_b = (num_tracked_lms_thr <= num_tracked_lms);
cond_b = cond_b && (num_tracked_lms < num_reliable_lms * lms_ratio_thr);
// 条件B不满足，意味着当前帧与参考关键帧的路标点很接近，极端情况为相机禁止不动， 此时不产生关键帧：
//另一种情况是运动过快，导致当前帧与参考关键帧之间的共视路标点低于阈值num_tracked_lms_thr ，也不产生关键帧
if (!cond_b) {
return false;
}
// A条件都不满足，意味着当前帧与参考帧之间的图像帧数量未超过最大阈值、低于最小阈值或者局部建图模块不接受新关键帧，且跟踪的路标点数量太少，低于可靠路标点四分之一，此时不产生关键帧
if (!cond_a1 && !cond_a2 && !cond_a3) {
return false;
}
//满足上述条件，且局部建图模块可以接收新的关键帧，才产生新的关键帧
if (mapper_is_idle) {
return true;
}
// 对于单目，会在一定情况下使得局部建图模块停下来，并插入关键帧
if (setup_type_ != camera::setup_type_t::Monocular && mapper_->get_num_queued_keyframes() <= 2) {
mapper_->abort_local_BA();
return true;
} else {
spdlog::debug("mapper_->get_num_queued_keyframes: {}", mapper_->get_num_queued_keyframes());
}
return false;
}
关键帧选择的条件：
局部建图模块优化任务完成；
- 与参考关键帧有一定的不同，避免在静止状态下产生冗余关键帧；
- 当前帧与参考关键帧之间要有一定数量的共视路标点，相机快速运动情况下可能不会产生关键帧；
- 当前帧与参考关键帧隔有足够数量的图像帧、或者跟踪的路标点少于参考帧可靠路标点的四分之一、或者当前帧与上一关键帧之间隔有最低数量的帧且局部建图模块可以接收新的关键帧。
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
