fix: save map break

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);    

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);
             }
         }
     }

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,6 +285,7 @@ void MapPoint::SetBadFlag()
     }
 
     // 擦除该MapPoint申请的内存
+    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())