Motion modelの導入

[eryeden]

定速度モーションモデルでもいいので導入できればp3pでうまく行かなくてもなんとかる？

OpenVSLAMの実装では、下のような感じ。単純に前回の位置と今回の位置の差分(SE3)をVelocityとしていている。Frameのキャプチャ間隔が変わる場合、時間をかける必要がある。

void tracking_module::update_motion_model() {
    if (last_frm_.cam_pose_cw_is_valid_) {
        Mat44_t last_frm_cam_pose_wc = Mat44_t::Identity();
        last_frm_cam_pose_wc.block<3, 3>(0, 0) = last_frm_.get_rotation_inv();
        last_frm_cam_pose_wc.block<3, 1>(0, 3) = last_frm_.get_cam_center();
        velocity_is_valid_ = true;
        velocity_ = curr_frm_.cam_pose_cw_ * last_frm_cam_pose_wc;
    }
    else {
        velocity_is_valid_ = false;
        velocity_ = Mat44_t::Identity();
    }
}