opencv中LM迭代算法 opencv slam

转载

laokugonggao 2024-04-05 13:13:30

文章标签 opencv中LM迭代算法 slam 特征点 sed 关键帧 文章分类 计算机视觉人工智能

上一篇讲到跟踪状态，其中跟踪当前帧用了三种匹配方式，分别是根据motion的匹配，根据BOW的匹配，暴力匹配，下面是对每一种匹配关系的梳理，调用函数如下：

bool tracking_module::track_current_frame() {
    bool succeeded = false;
    if (tracking_state_ == tracker_state_t::Tracking) {
        // Tracking mode
        if (velocity_is_valid_ && last_reloc_frm_id_ + 2 < curr_frm_.id_) {
            // if the motion model is valid
            succeeded = frame_tracker_.motion_based_track(curr_frm_, last_frm_, velocity_);
        }
        if (!succeeded) {
            succeeded = frame_tracker_.bow_match_based_track(curr_frm_, last_frm_, ref_keyfrm_);
        }
        if (!succeeded) {
            succeeded = frame_tracker_.robust_match_based_track(curr_frm_, last_frm_, ref_keyfrm_);
        }
    }
    else {
        // Lost mode
        // try to relocalize
        succeeded = relocalizer_.relocalize(curr_frm_);
        if (succeeded) {
            last_reloc_frm_id_ = curr_frm_.id_;
        }
    }
    return succeeded;
}

1 根据motion的匹配

输入：当前帧信息（特征点，网格划分），上一帧的信息（特征点，位姿，可观测的三维点），motion对应的相对位姿

输出：当前帧的位姿以及当前帧可观测的三维点

适合条件：motion对应的相对位姿已知，做匀速直线运动

1.1流程

当前帧位姿初始化为motion*上一帧的全局位姿（velocity * last_frm.cam_pose_cw_）
上一帧对应的可观测的三维点重投影（为P）到当前帧（这也是为什么要更新上一帧的信息）
在当前帧可视范围内的三维点对应的P点，根据当前帧网格划分，在P点附近的点作为候选点
所有候选点计算与当前三维点对应的描述子之间的距离得到最佳匹配点
若需要检查特征点对应的角度信息，则通过统计所有的匹配点对(当前帧与上一帧对应的特征点)之间的角度差来排除外点
优化当前帧的位姿（固定三维点和其他帧的位姿）
根据优化后的位姿去除外点

1.2 代码

bool frame_tracker::motion_based_track(data::frame& curr_frm, const data::frame& last_frm, const Mat44_t& velocity) const {
    match::projection projection_matcher(0.9, true);

    // motion modelを使って姿勢の初期値を設定
    curr_frm.set_cam_pose(velocity * last_frm.cam_pose_cw_);

    // 2D-3D対応を初期化
    std::fill(curr_frm.landmarks_.begin(), curr_frm.landmarks_.end(), nullptr);

    // last frameで見えている3次元点を再投影して2D-3D対応を見つける
    const float margin = (camera_->setup_type_ != camera::setup_type_t::Stereo) ? 20 : 10;
    auto num_matches = projection_matcher.match_current_and_last_frames(curr_frm, last_frm, margin);

    if (num_matches < num_matches_thr_) {
        // marginを広げて再探索
        std::fill(curr_frm.landmarks_.begin(), curr_frm.landmarks_.end(), nullptr);
        num_matches = projection_matcher.match_current_and_last_frames(curr_frm, last_frm, 2 * margin);
    }

    if (num_matches < num_matches_thr_) {
        spdlog::debug("motion based tracking failed: {} matches < {}", num_matches, num_matches_thr_);
        return false;
    }

    // pose optimization
    pose_optimizer_.optimize(curr_frm);

    // outlierを除く
    const auto num_valid_matches = discard_outliers(curr_frm);

    if (num_valid_matches < num_matches_thr_) {
        spdlog::debug("motion based tracking failed: {} inlier matches < {}", num_valid_matches, num_matches_thr_);
        return false;
    }
    else {
        return true;
    }
}

1.3 涉及到的数据结构

curr_frm_当前帧信息（特征点与网格划分）, last_frm_（特征点，位姿，可观测三维点）, velocity_（当前motion的状态）
当前状态是为velocity_is_valid_，以及上一次重定位是否过去至少两帧（last_reloc_frm_id_ + 2 < curr_frm_.id_）
匹配自身的参数：最少匹配点对数num_matches_thr_= 20，次佳距离比值ratio=0.9,是否验证角度=true

2 根据BOW的匹配

输入：当前帧信息（特征点），上一帧位姿，参考关键帧特征点及三维点信息

输出：当前帧位姿与匹配信息（三维点信息）

适合条件：motion不成立或者失败的情况下

2.1 流程

计算当前帧特征点对应的词根（first:词根，second: 词根对应的特征点（可能是多个））
计算当前帧与参考关键帧之间的匹配关系
首先根据词根（词根是有序的）从前往后找（相当于将特征点进一步划分为不同的子集（同一个子集的词根相同）
相同词根的参考关键帧与当前帧对应的特征点进行一对一的匹配（计算描述子之间的距离），找到每个参考关键帧对应的当前帧的最佳候选点与次佳候选点（距离第二小的特征点）
若是次佳候选点的距离*ratio（取0.7）> 最佳候选点的距离，说明最佳候选点是靠谱的，建立匹配关系
若是需要检查角度信息同1中的过程一样检查
以上一帧的位姿作为当前帧的位姿初始值
优化当前帧的位姿
根据位姿去除外点

2.2 代码

bool frame_tracker::bow_match_based_track(data::frame& curr_frm, const data::frame& last_frm, data::keyframe* ref_keyfrm) const {
    match::bow_tree bow_matcher(0.7, true);

    // BoW matchを行うのでBoWを計算しておく
    curr_frm.compute_bow();

    // 在keyframe和frame中寻找2d对应，得到frame的特征点和keyframe中观测的三维点的对应。
    std::vector<data::landmark*> matched_lms_in_curr;
    auto num_matches = bow_matcher.match_frame_and_keyframe(ref_keyfrm, curr_frm, matched_lms_in_curr);

    if (num_matches < num_matches_thr_) {
        spdlog::debug("bow match based tracking failed: {} matches < {}", num_matches, num_matches_thr_);
        return false;
    }

    // 2D-3D対応情報を更新
    curr_frm.landmarks_ = matched_lms_in_curr;

    // pose optimization
    // 初期値は前のフレームの姿勢
    curr_frm.set_cam_pose(last_frm.cam_pose_cw_);
    pose_optimizer_.optimize(curr_frm);

    // outlierを除く
    const auto num_valid_matches = discard_outliers(curr_frm);

    if (num_valid_matches < num_matches_thr_) {
        spdlog::debug("bow match based tracking failed: {} inlier matches < {}", num_valid_matches, num_matches_thr_);
        return false;
    }
    else {
        return true;
    }
}

2.3 涉及到的数据结构

当前帧的特征点
上一帧的位姿
参考关键帧的可观测三维点以及特征点信息
bow_tree自带的参数：最小跟踪点数num_matches_thr_=20，是否检查角度=true,次佳距离比值ratio=0.7

3 暴力匹配

输入：当前帧（特征），参考关键帧（特征，位姿，三维点）

输出：当前帧位姿，可观测三维点（匹配信息）

3.1 流程

暴力匹配当前帧与参考关键帧之间的匹配关系
首先对参考关键帧中带有可观测三维点的特征点找到当前帧中的最佳匹配候选点与次佳匹配候选点
然后通过次佳距离*ratio>最佳距离，确定匹配关系
八点法计算本质矩阵，通过ransac得到可靠的匹配关系
上一帧位姿初始化当前帧位姿
优化当前帧位姿
根据优化后的位姿去除外点

3.2 代码

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

bool frame_tracker::robust_match_based_track(data::frame& curr_frm, const data::frame& last_frm, data::keyframe* ref_keyfrm) const {
    match::robust robust_matcher(0.8, false);

    // 在keyframe和frame中寻找2d对应，得到frame的特征点和keyframe中观测的三维点的对应
    std::vector<data::landmark*> matched_lms_in_curr;
    auto num_matches = robust_matcher.match_frame_and_keyframe(curr_frm, ref_keyfrm, matched_lms_in_curr);

    if (num_matches < num_matches_thr_) {
        spdlog::debug("robust match based tracking failed: {} matches < {}", num_matches, num_matches_thr_);
        return false;
    }

    // 更新2d - 3d支持信息
    curr_frm.landmarks_ = matched_lms_in_curr;

    // pose optimization
    // 初始值是前一帧的姿势
    curr_frm.set_cam_pose(last_frm.cam_pose_cw_);
    pose_optimizer_.optimize(curr_frm);

    // outlierを除く
    const auto num_valid_matches = discard_outliers(curr_frm);

    if (num_valid_matches < num_matches_thr_) {
        spdlog::debug("robust match based tracking failed: {} inlier matches < {}", num_valid_matches, num_matches_thr_);
        return false;
    }
    else {
        return true;
    }
}