From 6c3b52564b2c7820d78ef401e21e520d4589e717 Mon Sep 17 00:00:00 2001
From: laobai <email>
Date: Mon, 27 Feb 2023 19:53:37 +0800
Subject: [PATCH] fix: save map break

---
 include/MapPoint.h |  4 ++--
 src/Map.cc         |  6 ++++--
 src/MapPoint.cc    | 15 +++++++++------
 3 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/include/MapPoint.h b/include/MapPoint.h
index 29cb420..51762d4 100644
--- a/include/MapPoint.h
+++ b/include/MapPoint.h
@@ -123,12 +123,12 @@ public:
     int Observations();
 
     void AddObservation(KeyFrame* pKF,int idx);
-    void EraseObservation(KeyFrame* pKF);
+    void EraseObservation(KeyFrame* pKF, bool erase = true);
 
     std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
     bool IsInKeyFrame(KeyFrame* pKF);
 
-    void SetBadFlag();
+    void SetBadFlag(bool erase = true);
     bool isBad();
 
     void Replace(MapPoint* pMP);    
diff --git a/src/Map.cc b/src/Map.cc
index 2010585..34f160a 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -311,6 +311,8 @@ void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool b
     {
         // 更新每一个mp在世界坐标系下的坐标
         MapPoint *pMP = *sit;
+        if (!pMP || pMP->isBad())
+            continue;
         pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);
         pMP->UpdateNormalAndDepth();
     }
@@ -412,9 +414,9 @@ void Map::PreSave(std::set<GeometricCamera *> &spCams)
         map<KeyFrame *, std::tuple<int, int>> mpObs = pMPi->GetObservations();
         for (map<KeyFrame *, std::tuple<int, int>>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it)
         {
-            if (it->first->GetMap() != this || it->first->isBad())
+            if (!it->first || it->first->GetMap() != this || it->first->isBad())
             {
-                pMPi->EraseObservation(it->first);
+                pMPi->EraseObservation(it->first, false);
             }
         }
     }
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 9c1d87e..85a4156 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -201,7 +201,7 @@ void MapPoint::AddObservation(KeyFrame* pKF, int idx)
 }
 
 // 删除某个关键帧对当前地图点的观测
-void MapPoint::EraseObservation(KeyFrame* pKF)
+void MapPoint::EraseObservation(KeyFrame* pKF, bool erase)
 {
     bool bBad=false;
     {
@@ -236,7 +236,7 @@ void MapPoint::EraseObservation(KeyFrame* pKF)
     }
     // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
     if(bBad)
-        SetBadFlag();
+        SetBadFlag(erase);
 }
 
 // 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引
@@ -260,7 +260,7 @@ int MapPoint::Observations()
  * @brief 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
  * 
  */
-void MapPoint::SetBadFlag()
+void MapPoint::SetBadFlag(bool erase)
 {
     map<KeyFrame*, tuple<int,int>> obs;
     {
@@ -285,7 +285,8 @@ void MapPoint::SetBadFlag()
     }
 
     // 擦除该MapPoint申请的内存
-    mpMap->EraseMapPoint(this);
+    if (erase)
+        mpMap->EraseMapPoint(this);
 }
 
 /**
@@ -769,6 +770,7 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
     mBackupObservationsId1.clear();
     mBackupObservationsId2.clear();
     // Save the id and position in each KF who view it
+    std::vector<KeyFrame*> erase_kfs;
     for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(), 
         end = mObservations.end(); it != end; ++it)
     {
@@ -780,10 +782,11 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
         }
         else
         {
-            EraseObservation(pKFi);
+            erase_kfs.push_back(pKFi); 
         }
     }
-
+    for (auto pKFi : erase_kfs)
+        EraseObservation(pKFi, false);
     // Save the id of the reference KF
     // 3. 备份参考关键帧ID
     if(spKF.find(mpRefKF) != spKF.end())