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結束
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);
}
}