From f9e61a49efb1ff8743bec43cec6ec7797d5017cf Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Thu, 2 Sep 2021 02:13:37 +0200
Subject: [PATCH] remove key point segmentation fault

---
 GPU_version/vio/src/frame.cpp | 23 +++++++++++++++++------
 GPU_version/vio/src/map.cpp   |  9 +++++----
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp
index 0a0b7ea..c6822c2 100644
--- a/GPU_version/vio/src/frame.cpp
+++ b/GPU_version/vio/src/frame.cpp
@@ -125,13 +125,24 @@ void Frame::checkKeyPoints(Feature* ftr)
 
 void Frame::removeKeyPoint(Feature* ftr)
 {
-  bool found = false;
-  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){
-    if(i == ftr) {
-      i = NULL;
-      found = true;
+    std::cerr<<key_pts_.size()<<'\t';
+    if(key_pts_.size()!=5){
+        key_pts_.clear();
+        key_pts_.reserve(5);
+        return;
     }
-  });
+  bool found = false;
+  for(auto&& key_pt:key_pts_){
+      std::cerr<<"131\t";
+      if(key_pt==NULL)continue;
+      std::cerr<<"131\t";
+      if(key_pt==ftr){
+          std::cerr<<"131\t";
+          key_pt=NULL;
+          found=true;
+      }
+  }
+    std::cerr<<"dd\n";
   if(found)
     setKeyPoints();
 }
diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp
index cdb6ce5..295fb75 100644
--- a/GPU_version/vio/src/map.cpp
+++ b/GPU_version/vio/src/map.cpp
@@ -83,10 +83,11 @@ void Map::removePtFrameRef(Frame* frame, Feature* ftr)
 void Map::safeDeletePoint(Point* pt)
 {
   // Delete references to mappoints in all keyframes
-  std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){
-    ftr->point=NULL;
-    ftr->frame->removeKeyPoint(ftr);
-  });
+  for(auto&& ftr:pt->obs_){
+      assert(ftr);
+      if(ftr->frame!= nullptr)ftr->frame->removeKeyPoint(ftr);
+      ftr->point=NULL;
+  }
   pt->obs_.clear();
 
   // Delete mappoint