diff --git a/include/Map.h b/include/Map.h
index ddebb4a..6053837 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -42,7 +42,10 @@ namespace ldso {
          * @return true if pose graph thread is started
          */
         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,7 +65,8 @@ namespace ldso {
 
         set<shared_ptr<Frame>, CmpFrameID> GetAllKFs() { return frames; }
 
-            unsigned long lastkfId = 0;  // keyframe id of this frame
+        unsigned long getLatestOptimizedKfId() const { return latestOptimizedKfId; }
+
     private:
         // the pose graph optimization thread
         void runPoseGraphOptimization();
@@ -72,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
index fda8b7a..93b04ed 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -27,10 +27,21 @@ 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);
@@ -145,11 +156,12 @@ namespace ldso {
         }
 
         poseGraphRunning = false;
+
         if (currentKF) {
-            lastkfId = currentKF->kfId;
+            latestOptimizedKfId = currentKF->kfId;
         }
 
-            if (fullsystem) fullsystem->RefreshGUI();
+        if (fullsystem) fullsystem->RefreshGUI();
     }
 
 }
diff --git a/src/frontend/FullSystem.cc b/src/frontend/FullSystem.cc
index e222749..27e6e13 100644
--- a/src/frontend/FullSystem.cc
+++ b/src/frontend/FullSystem.cc
@@ -400,6 +400,7 @@ namespace ldso {
                 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
         // as the poses or depths are updated (no matter if in PGO or in sliding window BA)
@@ -852,7 +853,7 @@ namespace ldso {
             unique_lock<mutex> crlock(shellPoseMutex);
             for (auto fr: frames) {
                 fr->setPose(fr->frameHessian->PRE_camToWorld.inverse());
-                if (fr->kfId >= globalMap->lastkfId) {
+                if (fr->kfId >= globalMap->getLatestOptimizedKfId()) {
                     fr->setPoseOpti(Sim3(fr->getPose().matrix()));
                 }
                 fr->aff_g2l = fr->frameHessian->aff_g2l();