Merge pull request #35 from tum-vision/fix_19

Ensure poseOpti always has the latest best pose estimate
tum-vision · Sep 24, 2019 · 1c1e3c9 · 1c1e3c9
2 parents 7e8b988 + 2944a67
commit 1c1e3c9
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 1 deletion.
diff --git a/examples/run_dso_euroc.cc b/examples/run_dso_euroc.cc
@@ -400,6 +400,7 @@ int main(int argc, char **argv) {
 
         // for evaluation, we print the result before loop closing and after loop closing
         fullSystem->printResult(output_file, true);
+        fullSystem->printResult(output_file + ".noloop", false);
 
         int numFramesProcessed = abs(idsToPlay[0] - idsToPlay.back());
         double numSecondsProcessed = fabs(reader->getTimestamp(idsToPlay[0]) - reader->getTimestamp(idsToPlay.back()));

diff --git a/examples/run_dso_tum_mono.cc b/examples/run_dso_tum_mono.cc
@@ -430,6 +430,7 @@ int main(int argc, char **argv) {
         gettimeofday(&tv_end, NULL);
 
         fullSystem->printResult(output_file, true);
+        fullSystem->printResult(output_file + ".noloop", false);
 
         int numFramesProcessed = abs(idsToPlay[0] - idsToPlay.back());
         double numSecondsProcessed = fabs(reader->getTimestamp(idsToPlay[0]) - reader->getTimestamp(idsToPlay.back()));

diff --git a/include/Map.h b/include/Map.h
@@ -43,6 +43,9 @@ namespace ldso {
          */
         bool OptimizeALLKFs();
 
+        // optimize pose graph on all kfs after odometry loop is done
+        void lastOptimizeAllKFs();
+
         /// update the cached 3d position of all points.
         void UpdateAllWorldPoints();
 
@@ -62,6 +65,8 @@ namespace ldso {
 
         set<shared_ptr<Frame>, CmpFrameID> GetAllKFs() { return frames; }
 
+        unsigned long getLatestOptimizedKfId() const { return latestOptimizedKfId; }
+
     private:
         // the pose graph optimization thread
         void runPoseGraphOptimization();
@@ -71,6 +76,9 @@ namespace ldso {
         set<shared_ptr<Frame>, CmpFrameID> framesOpti;  // KFs to be optimized
         shared_ptr<Frame> currentKF = nullptr;
 
+        // keyframe id of newest optimized keyframe frame
+        unsigned long latestOptimizedKfId = 0;
+
         bool poseGraphRunning = false;  // is pose graph running?
         mutex mutexPoseGraph;
 

diff --git a/src/Map.cc b/src/Map.cc
@@ -26,6 +26,22 @@ namespace ldso {
         }
     }
 
+    void Map::lastOptimizeAllKFs() {
+        LOG(INFO) << "Final pose graph optimization after odometry is finished.";
+
+        {
+            unique_lock<mutex> lock(mutexPoseGraph);
+            if (poseGraphRunning) {
+                LOG(FATAL) << "Should not be called while pose graph optimization is running";
+            }
+        }
+
+        // no locking of mapMutex since we assume that odometry has finished
+        framesOpti = frames;
+        currentKF = *frames.rbegin();
+        runPoseGraphOptimization();
+    }
+
     bool Map::OptimizeALLKFs() {
         {
             unique_lock<mutex> lock(mutexPoseGraph);
@@ -141,6 +157,10 @@ namespace ldso {
 
         poseGraphRunning = false;
 
+        if (currentKF) {
+            latestOptimizedKfId = currentKF->kfId;
+        }
+
         if (fullsystem) fullsystem->RefreshGUI();
     }
 

diff --git a/src/frontend/FullSystem.cc b/src/frontend/FullSystem.cc
@@ -394,8 +394,12 @@ namespace ldso {
 
         mappingThread.join();
 
-        if (setting_enableLoopClosing)
+        if (setting_enableLoopClosing) {
             loopClosing->SetFinish(true);
+            if (globalMap->NumFrames() > 4) {
+                globalMap->lastOptimizeAllKFs();
+            }
+        }
 
         // Update world points in case optimization hasn't run (with all keyframes)
         // It would maybe be better if the 3d points would always be updated as soon
@@ -849,6 +853,9 @@ namespace ldso {
             unique_lock<mutex> crlock(shellPoseMutex);
             for (auto fr: frames) {
                 fr->setPose(fr->frameHessian->PRE_camToWorld.inverse());
+                if (fr->kfId >= globalMap->getLatestOptimizedKfId()) {
+                    fr->setPoseOpti(Sim3(fr->getPose().matrix()));
+                }
                 fr->aff_g2l = fr->frameHessian->aff_g2l();
             }
         }