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相关
