From ed0711f96230243f107584f74824271bb8b723d7 Mon Sep 17 00:00:00 2001 From: huanggan52 <1367876650@qq.com> Date: Thu, 4 Apr 2024 15:30:31 +0800 Subject: [PATCH] 4.4 --- xrslam/src/xrslam/backend/global_map.cpp | 10 +++ xrslam/src/xrslam/backend/global_map.h | 1 + xrslam/src/xrslam/backend/local_mapper.cpp | 63 ++++++++++--------- xrslam/src/xrslam/backend/loop_closer.cpp | 22 +++---- xrslam/src/xrslam/backend/loop_closer.h | 2 +- xrslam/src/xrslam/backend/optimizer.cpp | 33 ++-------- xrslam/src/xrslam/backend/optimizer.h | 2 +- xrslam/src/xrslam/common.h | 9 +-- .../xrslam/core/sliding_window_tracker.cpp | 33 +++------- 9 files changed, 74 insertions(+), 101 deletions(-) diff --git a/xrslam/src/xrslam/backend/global_map.cpp b/xrslam/src/xrslam/backend/global_map.cpp index 1e6247c..aad4936 100644 --- a/xrslam/src/xrslam/backend/global_map.cpp +++ b/xrslam/src/xrslam/backend/global_map.cpp @@ -63,6 +63,16 @@ std::set GlobalMap::get_frontend_mappoints() { return frontend_mappoints; } +std::vector GlobalMap::get_keyframes_list() { + std::unique_lock lock(mutex_map); + std::vector sorted_keyframes(keyframes.begin(), keyframes.end()); + + std::sort(sorted_keyframes.begin(), sorted_keyframes.end(), [](const KeyFrame *a, const KeyFrame *b) { + return a->frame_id < b->frame_id; + }); + return sorted_keyframes; +} + MapPoint *GlobalMap::get_frontend_mappoint(size_t track_id) { std::unique_lock lock(mutex_map); if(frontend_mappoint_map.count(track_id)) diff --git a/xrslam/src/xrslam/backend/global_map.h b/xrslam/src/xrslam/backend/global_map.h index 9eddbb9..6c6964b 100644 --- a/xrslam/src/xrslam/backend/global_map.h +++ b/xrslam/src/xrslam/backend/global_map.h @@ -26,6 +26,7 @@ class GlobalMap { std::set get_keyframes(); std::set get_mappoints(); std::set get_frontend_mappoints(); + std::vector get_keyframes_list(); std::mutex mutex_map_update; size_t frontend_init_keyframe_id; diff --git a/xrslam/src/xrslam/backend/local_mapper.cpp b/xrslam/src/xrslam/backend/local_mapper.cpp index f2ac78c..3d0c248 100644 --- a/xrslam/src/xrslam/backend/local_mapper.cpp +++ b/xrslam/src/xrslam/backend/local_mapper.cpp @@ -45,9 +45,7 @@ void LocalMapper::work() { process_new_keyframe(); mappoint_culling(); create_new_mappoints(); - - if(!check_new_keyframe()) - search_in_neighbors(); + search_in_neighbors(); if(!check_new_keyframe() && !stop_request()) { local_bundle_adjustment(); @@ -307,8 +305,7 @@ void LocalMapper::initialize() { } } - if(!solver->solve()) - return; + solver->solve(); std::vector residuals = solver->evaluate(); @@ -319,7 +316,7 @@ void LocalMapper::initialize() { if(!mp) continue; float dis = (kf->get_camera_to_world_translation() - mp->get_landmark()).norm(); - if(residuals[i] > 3.0 || dis < 0.1 || dis > 20.0) { + if(residuals[i] > 3.0 || dis > 20.0) { mp->erase_observation(kf); size_t idx = mp->get_observations()[kf]; kf->get_features()->erase_mappoint(idx); @@ -331,6 +328,7 @@ void LocalMapper::initialize() { initialized = true; last_keyframe = curr_keyframe; + std::cout << "Initialization finished with " << matches << " matches and mappoints " << backend->global_map->get_mappoints().size() << std::endl; } } @@ -344,10 +342,9 @@ bool LocalMapper::need_new_keyframe() { int n_curr_matches = curr_keyframe->tracked_mappoints(2); bool c1 = curr_keyframe->frame_id - last_keyframe->frame_id >= 10; - bool c2 = n_curr_matches < n_last_matches * 0.9; - bool c3 = n_curr_matches < 30; + bool c2 = n_curr_matches < n_last_matches * 0.9 && n_curr_matches > 10; - return (c1 || c2 || c3); + return (c1 || c2); } void LocalMapper::set_accept_keyframes(bool flag) { @@ -479,7 +476,8 @@ void LocalMapper::search_local_points() { if(to_match>0) { ORBmatcher matcher; int matches = matcher.search_by_projection(curr_keyframe, local_mappoints, 1.0); - std::cout << " front "<< latest_tracked_id << " back " << curr_keyframe->frame_id << " to match " << to_match << " " << matches <timestamp - init_keyframe->timestamp; + std::cout << "time " << dura << " front "<< latest_tracked_id << " back " << curr_keyframe->frame_id << " to match " << to_match << " " << matches <frame_id <= frontend_init_frame_id) return; OptimizedData optimized_data; - optimized_data.t = curr_keyframe->timestamp; - optimized_data.frame_id = curr_keyframe->frame_id; - optimized_data.pose = curr_keyframe->get_pose(); - - auto features = curr_keyframe->get_features(); - for (size_t i = 0; i < features->feature_num(); i++) { - MapPoint *mappoint = features->get_mappoint(i); - if(mappoint && mappoint->tag(MPT_VALID)) { - optimized_data.track_ids.push_back(nil()); - optimized_data.observations.push_back(features->keypoints[i]); - optimized_data.landmarks.push_back(mappoint->get_landmark()); + + std::vector keyframes = backend->global_map->get_keyframes_list(); + for (size_t i = keyframes.size()-1; i > keyframes.size()-10 && i > 0; i--) { + KeyFrame *kf = keyframes[i]; + optimized_data.connected_frames.push_back(kf->frame_id); + + std::vector> observations; + std::vector> landmarks; + + auto features = kf->get_features(); + for (size_t j = 0; j < features->feature_num(); j++) { + MapPoint *mappoint = features->get_mappoint(j); + if(mappoint && mappoint->tag(MPT_VALID)) { + observations.push_back(features->keypoints[j]); + landmarks.push_back(mappoint->get_landmark()); + } } - } - auto frontend_features = curr_keyframe->get_frontend_features(); - for (size_t i = 0; i < frontend_features->feature_num(); i++) { - MapPoint *mappoint = frontend_features->get_mappoint(i); - if(mappoint && mappoint->tag(MPT_VALID)) { - optimized_data.track_ids.push_back(frontend_features->track_ids[i]); - optimized_data.observations.push_back(frontend_features->keypoints[i]); - optimized_data.landmarks.push_back(mappoint->get_landmark()); + auto frontend_features = kf->get_frontend_features(); + for (size_t j = 0; j < frontend_features->feature_num(); j++) { + MapPoint *mappoint = frontend_features->get_mappoint(j); + if(mappoint && mappoint->tag(MPT_VALID)) { + observations.push_back(frontend_features->keypoints[j]); + landmarks.push_back(mappoint->get_landmark()); + } } + optimized_data.observations.push_back(observations); + optimized_data.landmarks.push_back(landmarks); } + backend->detail->frontend->mirror_matched_points(optimized_data); } diff --git a/xrslam/src/xrslam/backend/loop_closer.cpp b/xrslam/src/xrslam/backend/loop_closer.cpp index 379b771..8eb11f1 100644 --- a/xrslam/src/xrslam/backend/loop_closer.cpp +++ b/xrslam/src/xrslam/backend/loop_closer.cpp @@ -155,7 +155,7 @@ bool LoopCloser::detect_loop() { bool LoopCloser::compute_se3() { bool match = false; - ORBmatcher matcher(0.8, false); + ORBmatcher matcher(0.9, false); const double &fx = curr_keyframe->intrinsics(0); const double &fy = curr_keyframe->intrinsics(1); @@ -205,15 +205,15 @@ bool LoopCloser::compute_se3() { int nfound = matcher.search_by_se3(curr_keyframe, keyframe, Tcl, matches, 10.0); std::cout << "matches: " << nmatches << " inliers: " << ninliers << " found: " << nfound << std::endl; - Pose pose_lc; - pose_lc.q = Tcl.block<3, 3>(0, 0).transpose(); - pose_lc.p = Tcl.block<3, 3>(0, 0).transpose() * (-Tcl.block<3, 1>(0, 3)); + if(ninliers + nfound > MIN_LOOP_NUM) { + Pose pose_lc; + pose_lc.q = Tcl.block<3, 3>(0, 0).transpose(); + pose_lc.p = Tcl.block<3, 3>(0, 0).transpose() * (-Tcl.block<3, 1>(0, 3)); - loop_keyframe = keyframe; + loop_keyframe = keyframe; - match = optimize_se3(pose_lc); + optimize_se3(pose_lc); - if(match) { std::string filename = std::to_string(curr_keyframe->frame_id) + "_" + std::to_string(keyframe->frame_id) + ".png"; curr_keyframe->draw_matches(keyframe, matches, filename); @@ -221,9 +221,9 @@ bool LoopCloser::compute_se3() { pose_wc.q = pose_wl.q * pose_lc.q; pose_wc.p = pose_wl.p + pose_wl.q * pose_lc.p; + match = true; break; } - } } @@ -441,11 +441,9 @@ void LoopCloser::insert_keyframe(KeyFrame *keyframe) { keyframe_buffer.push(keyframe); } -bool LoopCloser::optimize_se3(Pose &pose_lc) { +void LoopCloser::optimize_se3(Pose &pose_lc) { std::unique_lock lock(mutex_keyframe); - int ninliers = Optimizer::optimize_se3(curr_keyframe, loop_keyframe, matches, pose_lc); - std::cout << " optimized: " << ninliers << std::endl; - return ninliers > MIN_LOOP_NUM; + Optimizer::optimize_se3(curr_keyframe, loop_keyframe, matches, pose_lc); } void LoopCloser::global_bundle_adjustment(size_t curr_frame_id) { diff --git a/xrslam/src/xrslam/backend/loop_closer.h b/xrslam/src/xrslam/backend/loop_closer.h index 0678e65..1dcea29 100644 --- a/xrslam/src/xrslam/backend/loop_closer.h +++ b/xrslam/src/xrslam/backend/loop_closer.h @@ -29,7 +29,7 @@ class LoopCloser { void search_and_fuse(const std::map &corrected_poses_map); bool is_running_gba(); void insert_keyframe(KeyFrame *keyframe); - bool optimize_se3(Pose &pose_lc); + void optimize_se3(Pose &pose_lc); void global_bundle_adjustment(size_t curr_frame_id); diff --git a/xrslam/src/xrslam/backend/optimizer.cpp b/xrslam/src/xrslam/backend/optimizer.cpp index 26b9098..f35ae0c 100644 --- a/xrslam/src/xrslam/backend/optimizer.cpp +++ b/xrslam/src/xrslam/backend/optimizer.cpp @@ -9,7 +9,7 @@ namespace xrslam { -int Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector &matches, Pose &pose_lc) { +void Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector &matches, Pose &pose_lc) { int ninliers = 0; auto solver = Solver::create(); @@ -33,50 +33,29 @@ int Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, st if(!mappoint_i || !mappoint_j) continue; - mappoints.insert(mappoint_i); vector<3> Pw2 = mappoint_i->get_landmark(); vector<3> Pc_i = curr_keyframe->world_to_camera(Pw2); vector<3> Pm_i = pose_lc.q * Pc_i + pose_lc.p; mappoint_i->pnp_landmark = Pm_i; - mappoint_i->tag(MPT_FIX_LANDMARK) = false; + solver->add_mappoint_states(mappoint_i, OptimTarget::PNP); solver->put_factor(Solver::create_mvsreprojection_error_factor(loop_keyframe, matches[i], mappoint_i), OptimTarget::PNP); - factor_id_index_map.emplace(factor_id, i); - factor_id++; + solver->put_factor(Solver::create_mvsreprojection_error_factor(loop_keyframe, matches[i], mappoint_j), OptimTarget::PNP); - mappoints.insert(mappoint_j); vector<3> Pw1 = mappoint_j->get_landmark(); vector<3> Pm_j = loop_keyframe->world_to_camera(Pw1); mappoint_j->pnp_landmark = Pm_j; - mappoint_j->tag(MPT_FIX_LANDMARK) = true; solver->add_mappoint_states(mappoint_j, OptimTarget::PNP); + solver->put_factor(Solver::create_mvsreprojection_error_factor(curr_keyframe, i, mappoint_i), OptimTarget::PNP); solver->put_factor(Solver::create_mvsreprojection_error_factor(curr_keyframe, i, mappoint_j), OptimTarget::PNP); - factor_id++; } } solver->solve(); - std::vector residuals = solver->evaluate(); - - for (size_t i = 0; i < residuals.size(); i+=2) { - if(residuals[i] > 15.0 || residuals[i+1] > 15.0) { - matches[factor_id_index_map[i]] = -1; - } - else { - ninliers++; - } - } - loop_keyframe->tag(KFT_FIX_POSE) = false; - for (const auto &mappoint : mappoints) { - mappoint->tag(MPT_FIX_LANDMARK) = false; - } - pose_lc = curr_keyframe->pnp_pose; - - return ninliers; } void Optimizer::local_bundle_adjustment(KeyFrame *curr_keyframe) { @@ -177,7 +156,7 @@ void Optimizer::local_bundle_adjustment(KeyFrame *curr_keyframe) { if(!mp) continue; float dis = (kf->get_camera_to_world_translation() - mp->lba_landmark).norm(); - if(residuals[i] > 3.0 || dis < 0.1 || dis > 20.0) { + if(residuals[i] > 3.0 || dis > 20.0) { mp->erase_observation(kf); size_t idx = mp->get_observations()[kf]; if(mp->tag(MPT_FRONTEND)) @@ -413,7 +392,7 @@ void Optimizer::global_bundle_adjustment(size_t curr_frame_id, std::shared_ptrget_camera_to_world_translation() - mp->gba_landmark).norm(); - if(residuals[i] > 3.0 || dis < 0.1 || dis > 20.0) { + if(residuals[i] > 3.0 || dis > 20.0) { mp->erase_observation(kf); size_t idx = mp->get_observations()[kf]; if(mp->tag(MPT_FRONTEND)) diff --git a/xrslam/src/xrslam/backend/optimizer.h b/xrslam/src/xrslam/backend/optimizer.h index 176d213..71dc136 100644 --- a/xrslam/src/xrslam/backend/optimizer.h +++ b/xrslam/src/xrslam/backend/optimizer.h @@ -10,7 +10,7 @@ class MapPoint; class Optimizer { public: - int static optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector &matches, Pose &pose_lc); + void static optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector &matches, Pose &pose_lc); void static local_bundle_adjustment(KeyFrame *curr_keyframe); diff --git a/xrslam/src/xrslam/common.h b/xrslam/src/xrslam/common.h index eaf7237..dc2eb7a 100644 --- a/xrslam/src/xrslam/common.h +++ b/xrslam/src/xrslam/common.h @@ -65,12 +65,9 @@ template struct compare { typedef DBoW2::TemplatedVocabulary ORBVocabulary; struct OptimizedData { - double t; - size_t frame_id; - Pose pose; - std::vector track_ids; - std::vector> observations; - std::vector> landmarks; + std::vector connected_frames; + std::vector>> observations; + std::vector>> landmarks; }; struct ImuData { diff --git a/xrslam/src/xrslam/core/sliding_window_tracker.cpp b/xrslam/src/xrslam/core/sliding_window_tracker.cpp index a9b2031..dfd0195 100644 --- a/xrslam/src/xrslam/core/sliding_window_tracker.cpp +++ b/xrslam/src/xrslam/core/sliding_window_tracker.cpp @@ -336,32 +336,15 @@ void SlidingWindowTracker::refine_window() { } } - size_t frame_id = map->frame_index_by_id(optimized_data.frame_id); - - if (frame_id != nil()) { - Frame *frame_i = map->get_frame(frame_id); - for (size_t i = 0; i < optimized_data.track_ids.size(); ++i) { - size_t track_id = optimized_data.track_ids[i]; - vector<2> observation = optimized_data.observations[i]; - vector<3> landmark = optimized_data.landmarks[i]; - - if(track_id != nil()) { - Track *track = map->get_track_by_id(track_id); - for (size_t j = 0; j < map->frame_num(); ++j) { - Frame *frame_j = map->get_frame(j); - if(!track || frame_i == frame_j) - continue; - - size_t index = track->get_keypoint_index(frame_j); - if(index != nil()) { - observation = apply_k(frame_j->get_keypoint(index), frame_j->K); - solver->put_factor(Solver::create_reprojection_consis_factor(frame_j, observation, landmark)); - } - } - } - else { + for (size_t i = 0; i < optimized_data.connected_frames.size(); i++) { + size_t frame_id = optimized_data.connected_frames[i]; + size_t frame_index = map->frame_index_by_id(frame_id); + if (frame_index != nil()) { + std::vector> observations = optimized_data.observations[i]; + std::vector> landmarks = optimized_data.landmarks[i]; + for (size_t j = 0; j < observations.size(); j++) { solver->put_factor(Solver::create_reprojection_consis_factor( - frame_i, observation, landmark)); + map->get_frame(frame_index), observations[j], landmarks[j])); } } }