Skip to content
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.

Commit ed0711f

Browse files
huanggan52huanggan52
committedApr 4, 2024·
4.4
1 parent 25d6a54 commit ed0711f

9 files changed

+74
-101
lines changed
 

‎xrslam/src/xrslam/backend/global_map.cpp

+10
Original file line numberDiff line numberDiff line change
@@ -63,6 +63,16 @@ std::set<MapPoint *> GlobalMap::get_frontend_mappoints() {
6363
return frontend_mappoints;
6464
}
6565

66+
std::vector<KeyFrame *> GlobalMap::get_keyframes_list() {
67+
std::unique_lock<std::mutex> lock(mutex_map);
68+
std::vector<KeyFrame *> sorted_keyframes(keyframes.begin(), keyframes.end());
69+
70+
std::sort(sorted_keyframes.begin(), sorted_keyframes.end(), [](const KeyFrame *a, const KeyFrame *b) {
71+
return a->frame_id < b->frame_id;
72+
});
73+
return sorted_keyframes;
74+
}
75+
6676
MapPoint *GlobalMap::get_frontend_mappoint(size_t track_id) {
6777
std::unique_lock<std::mutex> lock(mutex_map);
6878
if(frontend_mappoint_map.count(track_id))

‎xrslam/src/xrslam/backend/global_map.h

+1
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ class GlobalMap {
2626
std::set<KeyFrame *> get_keyframes();
2727
std::set<MapPoint *> get_mappoints();
2828
std::set<MapPoint *> get_frontend_mappoints();
29+
std::vector<KeyFrame *> get_keyframes_list();
2930

3031
std::mutex mutex_map_update;
3132
size_t frontend_init_keyframe_id;

‎xrslam/src/xrslam/backend/local_mapper.cpp

+34-29
Original file line numberDiff line numberDiff line change
@@ -45,9 +45,7 @@ void LocalMapper::work() {
4545
process_new_keyframe();
4646
mappoint_culling();
4747
create_new_mappoints();
48-
49-
if(!check_new_keyframe())
50-
search_in_neighbors();
48+
search_in_neighbors();
5149

5250
if(!check_new_keyframe() && !stop_request()) {
5351
local_bundle_adjustment();
@@ -307,8 +305,7 @@ void LocalMapper::initialize() {
307305
}
308306
}
309307

310-
if(!solver->solve())
311-
return;
308+
solver->solve();
312309

313310
std::vector<double> residuals = solver->evaluate();
314311

@@ -319,7 +316,7 @@ void LocalMapper::initialize() {
319316
if(!mp) continue;
320317
float dis = (kf->get_camera_to_world_translation() - mp->get_landmark()).norm();
321318

322-
if(residuals[i] > 3.0 || dis < 0.1 || dis > 20.0) {
319+
if(residuals[i] > 3.0 || dis > 20.0) {
323320
mp->erase_observation(kf);
324321
size_t idx = mp->get_observations()[kf];
325322
kf->get_features()->erase_mappoint(idx);
@@ -331,6 +328,7 @@ void LocalMapper::initialize() {
331328

332329
initialized = true;
333330
last_keyframe = curr_keyframe;
331+
std::cout << "Initialization finished with " << matches << " matches and mappoints " << backend->global_map->get_mappoints().size() << std::endl;
334332
}
335333
}
336334

@@ -344,10 +342,9 @@ bool LocalMapper::need_new_keyframe() {
344342
int n_curr_matches = curr_keyframe->tracked_mappoints(2);
345343

346344
bool c1 = curr_keyframe->frame_id - last_keyframe->frame_id >= 10;
347-
bool c2 = n_curr_matches < n_last_matches * 0.9;
348-
bool c3 = n_curr_matches < 30;
345+
bool c2 = n_curr_matches < n_last_matches * 0.9 && n_curr_matches > 10;
349346

350-
return (c1 || c2 || c3);
347+
return (c1 || c2);
351348
}
352349

353350
void LocalMapper::set_accept_keyframes(bool flag) {
@@ -479,7 +476,8 @@ void LocalMapper::search_local_points() {
479476
if(to_match>0) {
480477
ORBmatcher matcher;
481478
int matches = matcher.search_by_projection(curr_keyframe, local_mappoints, 1.0);
482-
std::cout << " front "<< latest_tracked_id << " back " << curr_keyframe->frame_id << " to match " << to_match << " " << matches <<std::endl;
479+
double dura = curr_keyframe->timestamp - init_keyframe->timestamp;
480+
std::cout << "time " << dura << " front "<< latest_tracked_id << " back " << curr_keyframe->frame_id << " to match " << to_match << " " << matches <<std::endl;
483481
}
484482

485483
}
@@ -600,29 +598,36 @@ void LocalMapper::mirror_matched_points() {
600598
if(curr_keyframe->frame_id <= frontend_init_frame_id)
601599
return;
602600
OptimizedData optimized_data;
603-
optimized_data.t = curr_keyframe->timestamp;
604-
optimized_data.frame_id = curr_keyframe->frame_id;
605-
optimized_data.pose = curr_keyframe->get_pose();
606-
607-
auto features = curr_keyframe->get_features();
608-
for (size_t i = 0; i < features->feature_num(); i++) {
609-
MapPoint *mappoint = features->get_mappoint(i);
610-
if(mappoint && mappoint->tag(MPT_VALID)) {
611-
optimized_data.track_ids.push_back(nil());
612-
optimized_data.observations.push_back(features->keypoints[i]);
613-
optimized_data.landmarks.push_back(mappoint->get_landmark());
601+
602+
std::vector<KeyFrame *> keyframes = backend->global_map->get_keyframes_list();
603+
for (size_t i = keyframes.size()-1; i > keyframes.size()-10 && i > 0; i--) {
604+
KeyFrame *kf = keyframes[i];
605+
optimized_data.connected_frames.push_back(kf->frame_id);
606+
607+
std::vector<vector<2>> observations;
608+
std::vector<vector<3>> landmarks;
609+
610+
auto features = kf->get_features();
611+
for (size_t j = 0; j < features->feature_num(); j++) {
612+
MapPoint *mappoint = features->get_mappoint(j);
613+
if(mappoint && mappoint->tag(MPT_VALID)) {
614+
observations.push_back(features->keypoints[j]);
615+
landmarks.push_back(mappoint->get_landmark());
616+
}
614617
}
615-
}
616618

617-
auto frontend_features = curr_keyframe->get_frontend_features();
618-
for (size_t i = 0; i < frontend_features->feature_num(); i++) {
619-
MapPoint *mappoint = frontend_features->get_mappoint(i);
620-
if(mappoint && mappoint->tag(MPT_VALID)) {
621-
optimized_data.track_ids.push_back(frontend_features->track_ids[i]);
622-
optimized_data.observations.push_back(frontend_features->keypoints[i]);
623-
optimized_data.landmarks.push_back(mappoint->get_landmark());
619+
auto frontend_features = kf->get_frontend_features();
620+
for (size_t j = 0; j < frontend_features->feature_num(); j++) {
621+
MapPoint *mappoint = frontend_features->get_mappoint(j);
622+
if(mappoint && mappoint->tag(MPT_VALID)) {
623+
observations.push_back(frontend_features->keypoints[j]);
624+
landmarks.push_back(mappoint->get_landmark());
625+
}
624626
}
627+
optimized_data.observations.push_back(observations);
628+
optimized_data.landmarks.push_back(landmarks);
625629
}
630+
626631
backend->detail->frontend->mirror_matched_points(optimized_data);
627632
}
628633

‎xrslam/src/xrslam/backend/loop_closer.cpp

+10-12
Original file line numberDiff line numberDiff line change
@@ -155,7 +155,7 @@ bool LoopCloser::detect_loop() {
155155
bool LoopCloser::compute_se3() {
156156

157157
bool match = false;
158-
ORBmatcher matcher(0.8, false);
158+
ORBmatcher matcher(0.9, false);
159159

160160
const double &fx = curr_keyframe->intrinsics(0);
161161
const double &fy = curr_keyframe->intrinsics(1);
@@ -205,25 +205,25 @@ bool LoopCloser::compute_se3() {
205205
int nfound = matcher.search_by_se3(curr_keyframe, keyframe, Tcl, matches, 10.0);
206206
std::cout << "matches: " << nmatches << " inliers: " << ninliers << " found: " << nfound << std::endl;
207207

208-
Pose pose_lc;
209-
pose_lc.q = Tcl.block<3, 3>(0, 0).transpose();
210-
pose_lc.p = Tcl.block<3, 3>(0, 0).transpose() * (-Tcl.block<3, 1>(0, 3));
208+
if(ninliers + nfound > MIN_LOOP_NUM) {
209+
Pose pose_lc;
210+
pose_lc.q = Tcl.block<3, 3>(0, 0).transpose();
211+
pose_lc.p = Tcl.block<3, 3>(0, 0).transpose() * (-Tcl.block<3, 1>(0, 3));
211212

212-
loop_keyframe = keyframe;
213+
loop_keyframe = keyframe;
213214

214-
match = optimize_se3(pose_lc);
215+
optimize_se3(pose_lc);
215216

216-
if(match) {
217217
std::string filename = std::to_string(curr_keyframe->frame_id) + "_" + std::to_string(keyframe->frame_id) + ".png";
218218
curr_keyframe->draw_matches(keyframe, matches, filename);
219219

220220
Pose pose_wl = keyframe->get_pose();
221221
pose_wc.q = pose_wl.q * pose_lc.q;
222222
pose_wc.p = pose_wl.p + pose_wl.q * pose_lc.p;
223223

224+
match = true;
224225
break;
225226
}
226-
227227
}
228228
}
229229

@@ -441,11 +441,9 @@ void LoopCloser::insert_keyframe(KeyFrame *keyframe) {
441441
keyframe_buffer.push(keyframe);
442442
}
443443

444-
bool LoopCloser::optimize_se3(Pose &pose_lc) {
444+
void LoopCloser::optimize_se3(Pose &pose_lc) {
445445
std::unique_lock<std::mutex> lock(mutex_keyframe);
446-
int ninliers = Optimizer::optimize_se3(curr_keyframe, loop_keyframe, matches, pose_lc);
447-
std::cout << " optimized: " << ninliers << std::endl;
448-
return ninliers > MIN_LOOP_NUM;
446+
Optimizer::optimize_se3(curr_keyframe, loop_keyframe, matches, pose_lc);
449447
}
450448

451449
void LoopCloser::global_bundle_adjustment(size_t curr_frame_id) {

‎xrslam/src/xrslam/backend/loop_closer.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ class LoopCloser {
2929
void search_and_fuse(const std::map<KeyFrame *, Pose> &corrected_poses_map);
3030
bool is_running_gba();
3131
void insert_keyframe(KeyFrame *keyframe);
32-
bool optimize_se3(Pose &pose_lc);
32+
void optimize_se3(Pose &pose_lc);
3333
void global_bundle_adjustment(size_t curr_frame_id);
3434

3535

‎xrslam/src/xrslam/backend/optimizer.cpp

+6-27
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99

1010
namespace xrslam {
1111

12-
int Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc) {
12+
void Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc) {
1313
int ninliers = 0;
1414

1515
auto solver = Solver::create();
@@ -33,50 +33,29 @@ int Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, st
3333
if(!mappoint_i || !mappoint_j)
3434
continue;
3535

36-
mappoints.insert(mappoint_i);
3736
vector<3> Pw2 = mappoint_i->get_landmark();
3837
vector<3> Pc_i = curr_keyframe->world_to_camera(Pw2);
3938
vector<3> Pm_i = pose_lc.q * Pc_i + pose_lc.p;
4039
mappoint_i->pnp_landmark = Pm_i;
41-
mappoint_i->tag(MPT_FIX_LANDMARK) = false;
40+
4241
solver->add_mappoint_states(mappoint_i, OptimTarget::PNP);
4342
solver->put_factor(Solver::create_mvsreprojection_error_factor(loop_keyframe, matches[i], mappoint_i), OptimTarget::PNP);
44-
factor_id_index_map.emplace(factor_id, i);
45-
factor_id++;
43+
solver->put_factor(Solver::create_mvsreprojection_error_factor(loop_keyframe, matches[i], mappoint_j), OptimTarget::PNP);
4644

47-
mappoints.insert(mappoint_j);
4845
vector<3> Pw1 = mappoint_j->get_landmark();
4946
vector<3> Pm_j = loop_keyframe->world_to_camera(Pw1);
5047
mappoint_j->pnp_landmark = Pm_j;
51-
mappoint_j->tag(MPT_FIX_LANDMARK) = true;
5248
solver->add_mappoint_states(mappoint_j, OptimTarget::PNP);
49+
solver->put_factor(Solver::create_mvsreprojection_error_factor(curr_keyframe, i, mappoint_i), OptimTarget::PNP);
5350
solver->put_factor(Solver::create_mvsreprojection_error_factor(curr_keyframe, i, mappoint_j), OptimTarget::PNP);
54-
factor_id++;
5551
}
5652
}
5753

5854
solver->solve();
5955

60-
std::vector<double> residuals = solver->evaluate();
61-
62-
for (size_t i = 0; i < residuals.size(); i+=2) {
63-
if(residuals[i] > 15.0 || residuals[i+1] > 15.0) {
64-
matches[factor_id_index_map[i]] = -1;
65-
}
66-
else {
67-
ninliers++;
68-
}
69-
}
70-
7156
loop_keyframe->tag(KFT_FIX_POSE) = false;
7257

73-
for (const auto &mappoint : mappoints) {
74-
mappoint->tag(MPT_FIX_LANDMARK) = false;
75-
}
76-
7758
pose_lc = curr_keyframe->pnp_pose;
78-
79-
return ninliers;
8059
}
8160

8261
void Optimizer::local_bundle_adjustment(KeyFrame *curr_keyframe) {
@@ -177,7 +156,7 @@ void Optimizer::local_bundle_adjustment(KeyFrame *curr_keyframe) {
177156
if(!mp) continue;
178157
float dis = (kf->get_camera_to_world_translation() - mp->lba_landmark).norm();
179158

180-
if(residuals[i] > 3.0 || dis < 0.1 || dis > 20.0) {
159+
if(residuals[i] > 3.0 || dis > 20.0) {
181160
mp->erase_observation(kf);
182161
size_t idx = mp->get_observations()[kf];
183162
if(mp->tag(MPT_FRONTEND))
@@ -413,7 +392,7 @@ void Optimizer::global_bundle_adjustment(size_t curr_frame_id, std::shared_ptr<G
413392

414393
float dis = (kf->get_camera_to_world_translation() - mp->gba_landmark).norm();
415394

416-
if(residuals[i] > 3.0 || dis < 0.1 || dis > 20.0) {
395+
if(residuals[i] > 3.0 || dis > 20.0) {
417396
mp->erase_observation(kf);
418397
size_t idx = mp->get_observations()[kf];
419398
if(mp->tag(MPT_FRONTEND))

‎xrslam/src/xrslam/backend/optimizer.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ class MapPoint;
1010

1111
class Optimizer {
1212
public:
13-
int static optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc);
13+
void static optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc);
1414

1515
void static local_bundle_adjustment(KeyFrame *curr_keyframe);
1616

‎xrslam/src/xrslam/common.h

+3-6
Original file line numberDiff line numberDiff line change
@@ -65,12 +65,9 @@ template <typename T> struct compare<T *> {
6565
typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB> ORBVocabulary;
6666

6767
struct OptimizedData {
68-
double t;
69-
size_t frame_id;
70-
Pose pose;
71-
std::vector<size_t> track_ids;
72-
std::vector<vector<2>> observations;
73-
std::vector<vector<3>> landmarks;
68+
std::vector<size_t> connected_frames;
69+
std::vector<std::vector<vector<2>>> observations;
70+
std::vector<std::vector<vector<3>>> landmarks;
7471
};
7572

7673
struct ImuData {

‎xrslam/src/xrslam/core/sliding_window_tracker.cpp

+8-25
Original file line numberDiff line numberDiff line change
@@ -336,32 +336,15 @@ void SlidingWindowTracker::refine_window() {
336336
}
337337
}
338338

339-
size_t frame_id = map->frame_index_by_id(optimized_data.frame_id);
340-
341-
if (frame_id != nil()) {
342-
Frame *frame_i = map->get_frame(frame_id);
343-
for (size_t i = 0; i < optimized_data.track_ids.size(); ++i) {
344-
size_t track_id = optimized_data.track_ids[i];
345-
vector<2> observation = optimized_data.observations[i];
346-
vector<3> landmark = optimized_data.landmarks[i];
347-
348-
if(track_id != nil()) {
349-
Track *track = map->get_track_by_id(track_id);
350-
for (size_t j = 0; j < map->frame_num(); ++j) {
351-
Frame *frame_j = map->get_frame(j);
352-
if(!track || frame_i == frame_j)
353-
continue;
354-
355-
size_t index = track->get_keypoint_index(frame_j);
356-
if(index != nil()) {
357-
observation = apply_k(frame_j->get_keypoint(index), frame_j->K);
358-
solver->put_factor(Solver::create_reprojection_consis_factor(frame_j, observation, landmark));
359-
}
360-
}
361-
}
362-
else {
339+
for (size_t i = 0; i < optimized_data.connected_frames.size(); i++) {
340+
size_t frame_id = optimized_data.connected_frames[i];
341+
size_t frame_index = map->frame_index_by_id(frame_id);
342+
if (frame_index != nil()) {
343+
std::vector<vector<2>> observations = optimized_data.observations[i];
344+
std::vector<vector<3>> landmarks = optimized_data.landmarks[i];
345+
for (size_t j = 0; j < observations.size(); j++) {
363346
solver->put_factor(Solver::create_reprojection_consis_factor(
364-
frame_i, observation, landmark));
347+
map->get_frame(frame_index), observations[j], landmarks[j]));
365348
}
366349
}
367350
}

0 commit comments

Comments
 (0)
Please sign in to comment.