4.4

xrslam/src/xrslam/backend/global_map.cpp

@@ -63,6 +63,16 @@ std::set<MapPoint *> GlobalMap::get_frontend_mappoints() {
     return frontend_mappoints;
 }
 
+std::vector<KeyFrame *> GlobalMap::get_keyframes_list() {
+    std::unique_lock<std::mutex> lock(mutex_map);
+    std::vector<KeyFrame *> 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<std::mutex> lock(mutex_map);
     if(frontend_mappoint_map.count(track_id))

xrslam/src/xrslam/backend/global_map.h

@@ -26,6 +26,7 @@ class GlobalMap {
     std::set<KeyFrame *> get_keyframes();
     std::set<MapPoint *> get_mappoints();
     std::set<MapPoint *> get_frontend_mappoints();
+    std::vector<KeyFrame *> get_keyframes_list();
 
     std::mutex mutex_map_update;
     size_t frontend_init_keyframe_id;

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<double> 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 <<std::endl;
+        double dura = curr_keyframe->timestamp - init_keyframe->timestamp;
+        std::cout << "time " << dura << " front "<< latest_tracked_id << " back " << curr_keyframe->frame_id << " to match " << to_match << " " << matches <<std::endl;
     }
 
 }
@@ -600,29 +598,36 @@ void LocalMapper::mirror_matched_points() {
     if(curr_keyframe->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<KeyFrame *> 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<vector<2>> observations;
+        std::vector<vector<3>> 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);
 }
 

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,25 +205,25 @@ 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);
 
                 Pose pose_wl = keyframe->get_pose();
                 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<std::mutex> 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) {

xrslam/src/xrslam/backend/loop_closer.h

@@ -29,7 +29,7 @@ class LoopCloser {
     void search_and_fuse(const std::map<KeyFrame *, Pose> &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);
 
 

xrslam/src/xrslam/backend/optimizer.cpp

@@ -9,7 +9,7 @@
 
 namespace xrslam {
 
-int Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc) {
+void Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &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<double> 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_ptr<G
 
         float dis = (kf->get_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))

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<int> &matches, Pose &pose_lc);
+    void static optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc);
 
     void static local_bundle_adjustment(KeyFrame *curr_keyframe);
 

xrslam/src/xrslam/common.h

@@ -65,12 +65,9 @@ template <typename T> struct compare<T *> {
 typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB> ORBVocabulary;
 
 struct OptimizedData {
-    double t;
-    size_t frame_id;
-    Pose pose;
-    std::vector<size_t> track_ids;
-    std::vector<vector<2>> observations;
-    std::vector<vector<3>> landmarks;
+    std::vector<size_t> connected_frames;
+    std::vector<std::vector<vector<2>>> observations;
+    std::vector<std::vector<vector<3>>> landmarks;
 };
 
 struct ImuData {

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<vector<2>> observations = optimized_data.observations[i];
+                std::vector<vector<3>> 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]));
                 }
             }
         }