add backend features to frontend for optimization

Author: huanggan52

xrslam/src/xrslam/backend/keyframe.h

@@ -91,6 +91,7 @@ class KeyFrame: public Tagged<KeyFrameTag> {
 
     std::mutex mutex_pose;
     Pose pose;
+    Pose pnp_pose;
     Pose lba_pose;
     Pose pgo_pose;
     Pose gba_pose;

xrslam/src/xrslam/backend/local_mapper.cpp

@@ -1,5 +1,7 @@
 #include <xrslam/map/map.h>
 #include <xrslam/map/frame.h>
+#include <xrslam/core/detail.h>
+#include <xrslam/core/frontend_worker.h>
 #include <xrslam/estimation/solver.h>
 #include <xrslam/estimation/ceres/reprojection_factor.h>
 #include <xrslam/backend/backend_worker.h>
@@ -52,6 +54,7 @@ void LocalMapper::work() {
                     keyframe_culling();
                 }
 
+                mirror_matched_points();
                 last_keyframe = curr_keyframe;
                 backend->loop_closer->insert_keyframe(curr_keyframe);
             }
@@ -593,6 +596,36 @@ void LocalMapper::keyframe_culling() {
     }
 }
 
+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());
+        }
+    }
+
+    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());
+        }
+    }
+    backend->detail->frontend->mirror_matched_points(optimized_data);
+}
+
 void LocalMapper::create_new_mappoints() {
     const std::vector<KeyFrame *> neighbors = curr_keyframe->get_best_covisibility_keyframes(20);
 

xrslam/src/xrslam/backend/local_mapper.h

@@ -38,6 +38,7 @@ class LocalMapper {
     void process_new_keyframe();
     void mappoint_culling();
     void keyframe_culling();
+    void mirror_matched_points();
     void create_new_mappoints();
     void search_in_neighbors();
     void set_accept_keyframes(bool flag);

xrslam/src/xrslam/backend/loop_closer.cpp

@@ -202,22 +202,28 @@ bool LoopCloser::compute_se3() {
                     matches[indices[i]] = -1;
             }
 
-            if(ninliers > 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));
+            int nfound = matcher.search_by_se3(curr_keyframe, keyframe, Tcl, matches, 10.0);
+            std::cout << "matches: " << nmatches << " inliers: " << ninliers << " found: " << nfound << std::endl;
 
-                std::cout << "matches: " << nmatches << " inliers: " << ninliers << 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));
+
+            loop_keyframe = keyframe;
+
+            match = 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;
 
-                loop_keyframe = keyframe;
-                match = true;
+                break;
             }
+
         }
     }
 
@@ -435,6 +441,13 @@ void LoopCloser::insert_keyframe(KeyFrame *keyframe) {
     keyframe_buffer.push(keyframe);
 }
 
+bool 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;
+}
+
 void LoopCloser::global_bundle_adjustment(size_t curr_frame_id) {
     std::cout << "Global Bundle Adjustment started" << std::endl;
 

xrslam/src/xrslam/backend/loop_closer.h

@@ -29,6 +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 global_bundle_adjustment(size_t curr_frame_id);
 
 

xrslam/src/xrslam/backend/map_point.h

@@ -59,6 +59,7 @@ class MapPoint: public Tagged<MapPointTag> {
 
     std::mutex mutex_landmark;
     vector<3> landmark;
+    vector<3> pnp_landmark;
     vector<3> lba_landmark;
     vector<3> gba_landmark;
 

xrslam/src/xrslam/backend/optimizer.cpp

@@ -9,6 +9,76 @@
 
 namespace xrslam {
 
+int Optimizer::optimize_se3(KeyFrame *curr_keyframe, KeyFrame *loop_keyframe, std::vector<int> &matches, Pose &pose_lc) {
+    int ninliers = 0;
+
+    auto solver = Solver::create();
+
+    curr_keyframe->tag(KFT_FIX_POSE) = false;
+    curr_keyframe->pnp_pose = pose_lc;
+    solver->add_keyframe_states(curr_keyframe, OptimTarget::PNP);
+
+    loop_keyframe->tag(KFT_FIX_POSE) = true;
+    loop_keyframe->pnp_pose = Pose();
+    solver->add_keyframe_states(loop_keyframe, OptimTarget::PNP);
+
+    size_t factor_id = 0;
+    std::map<size_t, size_t> factor_id_index_map;
+    std::set<MapPoint *> mappoints;
+
+    for (size_t i = 0; i < matches.size(); i++) {
+        if (matches[i] != -1) {
+            MapPoint *mappoint_i = curr_keyframe->get_features()->get_mappoint(i);
+            MapPoint *mappoint_j = loop_keyframe->get_features()->get_mappoint(matches[i]);
+            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++;
+
+            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_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) {
     auto map = curr_keyframe->backend->global_map;
 

xrslam/src/xrslam/backend/optimizer.h

@@ -10,6 +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 local_bundle_adjustment(KeyFrame *curr_keyframe);