(1) 地图尺度和位姿修改

 

1 设置相机位姿

 

void keyframe::set_cam_pose(const Mat44_t& cam_pose_cw) {
    std::lock_guard<std::mutex> lock(mtx_pose_);
    cam_pose_cw_ = cam_pose_cw;
 
    const Mat33_t rot_cw = cam_pose_cw_.block<3, 3>(0, 0);
    const Vec3_t trans_cw = cam_pose_cw_.block<3, 1>(0, 3);
    const Mat33_t rot_wc = rot_cw.transpose();
    cam_center_ = -rot_wc * trans_cw;
 
    cam_pose_wc_ = Mat44_t::Identity();
    cam_pose_wc_.block<3, 3>(0, 0) = rot_wc;
    cam_pose_wc_.block<3, 1>(0, 3) = cam_center_;
}

　　

2 地图尺度缩放

 

前两帧完成初始化

2-1 获取缩放scale

获取

 const auto inv_median_depth = 1.0 / median_depth;

 

　
scale = inv_median_depth * scaling_factor_

 

深度计算

float keyframe::compute_median_depth(const bool abs) const {
    std::vector<landmark*> landmarks;
    Mat44_t cam_pose_cw;
    {
        std::lock_guard<std::mutex> lock1(mtx_observations_);
        std::lock_guard<std::mutex> lock2(mtx_pose_);
        landmarks = landmarks_;
        cam_pose_cw = cam_pose_cw_;
    }

    std::vector<float> depths;
    depths.reserve(num_keypts_);
    const Vec3_t rot_cw_z_row = cam_pose_cw.block<1, 3>(2, 0);
    const float trans_cw_z = cam_pose_cw(2, 3);

    for (const auto lm : landmarks) {
        if (!lm) {
            continue;
        }
        const Vec3_t pos_w = lm->get_pos_in_world();
        const auto pos_c_z = rot_cw_z_row.dot(pos_w) + trans_cw_z;
        depths.push_back(abs ? std::abs(pos_c_z) : pos_c_z);
    }

    std::sort(depths.begin(), depths.end());

    return depths.at((depths.size() - 1) / 2);
}

　　

　　

2-2 设置地图尺度

请注意这里是把 t = s*t

而在3D-3D sRt计算中   s*R*p1质心+t=p2 质心    s尺度是作用在R上的

void initializer::scale_map(data::keyframe* init_keyfrm, data::keyframe* curr_keyfrm, const double scale) {
    // scaling keyframes
    Mat44_t cam_pose_cw = curr_keyfrm->get_cam_pose();
    cam_pose_cw.block<3, 1>(0, 3) *= scale;
    curr_keyfrm->set_cam_pose(cam_pose_cw);

    // scaling landmarks
    const auto landmarks = init_keyfrm->get_landmarks();
    for (auto lm : landmarks) {
        if (!lm) {
            continue;
        }
        lm->set_pos_in_world(lm->get_pos_in_world() * scale);
    }
}

　　

3局部BA结束

local_bundle_adjuster.cc

   // 8. 情報を更新

    {
        std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

        if (!outlier_observations.empty()) {
            for (auto& outlier_obs : outlier_observations) {
                auto keyfrm = outlier_obs.first;
                auto lm = outlier_obs.second;
                keyfrm->erase_landmark(lm);
                lm->erase_observation(keyfrm);
            }
        }

        for (auto id_local_keyfrm_pair : local_keyfrms) {
            auto local_keyfrm = id_local_keyfrm_pair.second;

            auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(local_keyfrm);
            local_keyfrm->set_cam_pose(keyfrm_vtx->estimate());
        }

        for (auto id_local_lm_pair : local_lms) {
            auto local_lm = id_local_lm_pair.second;

            auto lm_vtx = lm_vtx_container.get_vertex(local_lm);
            local_lm->set_pos_in_world(lm_vtx->estimate());
            local_lm->update_normal_and_depth();
        }
    }

　　

 

 

3全局BA结束

global_bundle_adjuster.cc

    const auto keyfrms = map_db_->get_all_keyframes();
    const auto lms = map_db_->get_all_landmarks();

　　

    for (auto keyfrm : keyfrms) {
        if (keyfrm->will_be_erased()) {
            continue;
        }
        auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
        const auto cam_pose_cw = util::converter::to_eigen_mat(keyfrm_vtx->estimate());
        if (lead_keyfrm_id_in_global_BA == 0) {
            keyfrm->set_cam_pose(cam_pose_cw);
        }
        else {
            keyfrm->cam_pose_cw_after_loop_BA_ = cam_pose_cw;
            keyfrm->loop_BA_identifier_ = lead_keyfrm_id_in_global_BA;
        }
    }

    for (unsigned int i = 0; i < lms.size(); ++i) {
        if (!is_optimized_lm.at(i)) {
            continue;
        }

        auto lm = lms.at(i);
        if (!lm) {
            continue;
        }
        if (lm->will_be_erased()) {
            continue;
        }

        auto lm_vtx = lm_vtx_container.get_vertex(lm);
        const Vec3_t pos_w = lm_vtx->estimate();

        if (lead_keyfrm_id_in_global_BA == 0) {
            lm->set_pos_in_world(pos_w);
            lm->update_normal_and_depth();
        }
        else {
            lm->pos_w_after_global_BA_ = pos_w;
            lm->loop_BA_identifier_ = lead_keyfrm_id_in_global_BA;
        }

　　

4 全局loop SIM3 BA优化结束 更新位姿和地图点

 

loop_bundle_adjuster.cc

位姿更新

        // update the camera pose along the spanning tree from the origin
        std::list<data::keyframe*> keyfrms_to_check;
        keyfrms_to_check.push_back(map_db_->origin_keyfrm_);
        while (!keyfrms_to_check.empty()) {
            auto parent = keyfrms_to_check.front();
            const Mat44_t cam_pose_wp = parent->get_cam_pose_inv();

            const auto children = parent->graph_node_->get_spanning_children();
            for (auto child : children) {
                if (child->loop_BA_identifier_ != identifier) {
                    // if `child` is NOT optimized by the loop BA
                    // propagate the pose correction from the spanning parent

                    // parent->child
                    const Mat44_t cam_pose_cp = child->get_cam_pose() * cam_pose_wp;
                    // world->child AFTER correction = parent->child * world->parent AFTER correction
                    child->cam_pose_cw_after_loop_BA_ = cam_pose_cp * parent->cam_pose_cw_after_loop_BA_;
                    // check as `child` has been corrected
                    child->loop_BA_identifier_ = identifier;
                }

                // need updating
                keyfrms_to_check.push_back(child);
            }

            // temporally store the camera pose BEFORE correction (for correction of landmark positions)
            parent->cam_pose_cw_before_BA_ = parent->get_cam_pose();
            // update the camera pose
            parent->set_cam_pose(parent->cam_pose_cw_after_loop_BA_);
            // finish updating
            keyfrms_to_check.pop_front();
        }

　　

地图点

        // update the positions of the landmarks
        const auto landmarks = map_db_->get_all_landmarks();
        for (auto lm : landmarks) {
            if (lm->will_be_erased()) {
                continue;
            }

            if (lm->loop_BA_identifier_ == identifier) {
                // if `lm` is optimized by the loop BA

                // update with the optimized position
                lm->set_pos_in_world(lm->pos_w_after_global_BA_);
            }
            else {
                // if `lm` is NOT optimized by the loop BA

                // correct the position according to the move of the camera pose of the reference keyframe
                auto ref_keyfrm = lm->get_ref_keyframe();

                assert(ref_keyfrm->loop_BA_identifier_ == identifier);

                // convert the position to the camera-reference using the camera pose BEFORE the correction
                const Mat33_t rot_cw_before_BA = ref_keyfrm->cam_pose_cw_before_BA_.block<3, 3>(0, 0);
                const Vec3_t trans_cw_before_BA = ref_keyfrm->cam_pose_cw_before_BA_.block<3, 1>(0, 3);
                const Vec3_t pos_c = rot_cw_before_BA * lm->get_pos_in_world() + trans_cw_before_BA;

                // convert the position to the world-reference using the camera pose AFTER the correction
                const Mat44_t cam_pose_wc = ref_keyfrm->get_cam_pose_inv();
                const Mat33_t rot_wc = cam_pose_wc.block<3, 3>(0, 0);
                const Vec3_t trans_wc = cam_pose_wc.block<3, 1>(0, 3);
                lm->set_pos_in_world(rot_wc * pos_c + trans_wc);
            }
        }

　　

 

所有关键帧保存操作

 

 

void trajectory_io::save_keyframe_trajectory(const std::string& path, const std::string& format) const {
    std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

    // 1. acquire keyframes and sort them

    assert(map_db_);
    auto keyfrms = map_db_->get_all_keyframes();
    std::sort(keyfrms.begin(), keyfrms.end(), [&](data::keyframe* keyfrm_1, data::keyframe* keyfrm_2) {
        return *keyfrm_1 < *keyfrm_2;
    });

    // 2. save the keyframes

    if (keyfrms.empty()) {
        spdlog::warn("there are no valid keyframes, cannot dump keyframe trajectory");
        return;
    }

    std::ofstream ofs(path, std::ios::out);
    if (!ofs.is_open()) {
        spdlog::critical("cannot create a file at {}", path);
        throw std::runtime_error("cannot create a file at " + path);
    }

    spdlog::info("dump keyframe trajectory in \"{}\" format from keyframe {} to keyframe {} ({} keyframes)",
                 format, (*keyfrms.begin())->id_, (*keyfrms.rbegin())->id_, keyfrms.size());

    for (const auto keyfrm : keyfrms) {
        const Mat44_t cam_pose_cw = keyfrm->get_cam_pose();
        const Mat44_t cam_pose_wc = cam_pose_cw.inverse();
        const auto timestamp = keyfrm->timestamp_;

        if (format == "KITTI") {
            ofs << std::setprecision(9)
                << cam_pose_wc(0, 0) << " " << cam_pose_wc(0, 1) << " " << cam_pose_wc(0, 2) << " " << cam_pose_wc(0, 3) << " "
                << cam_pose_wc(1, 0) << " " << cam_pose_wc(1, 1) << " " << cam_pose_wc(1, 2) << " " << cam_pose_wc(1, 3) << " "
                << cam_pose_wc(2, 0) << " " << cam_pose_wc(2, 1) << " " << cam_pose_wc(2, 2) << " " << cam_pose_wc(2, 3) << std::endl;
        }
        else if (format == "TUM") {
            const Mat33_t& rot_wc = cam_pose_wc.block<3, 3>(0, 0);
            const Vec3_t& trans_wc = cam_pose_wc.block<3, 1>(0, 3);
            const Quat_t quat_wc = Quat_t(rot_wc);
            ofs << std::setprecision(15)
                << timestamp << " "
                << std::setprecision(9)
                << trans_wc(0) << " " << trans_wc(1) << " " << trans_wc(2) << " "
                << quat_wc.x() << " " << quat_wc.y() << " " << quat_wc.z() << " " << quat_wc.w() << std::endl;
        }
        else {
            throw std::runtime_error("Not implemented: trajectory format \"" + format + "\"");
        }
    }

    ofs.close();
}

} // namespace io
} // namespace openvslam

　　

 

其他

1 与landmark相关