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); } }
其他
1 與landmark相關