remove key point segmentation fault

GPU_version/vio/src/frame.cpp

@@ -125,13 +125,24 @@ void Frame::checkKeyPoints(Feature* ftr)
 
 void Frame::removeKeyPoint(Feature* ftr)
 {
+    std::cerr<<key_pts_.size()<<'\t';
+    if(key_pts_.size()!=5){
+        key_pts_.clear();
+        key_pts_.reserve(5);
+        return;
+    }
   bool found = false;
-  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){
-    if(i == ftr) {
-      i = NULL;
+  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();
 }

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){
+  for(auto&& ftr:pt->obs_){
+      assert(ftr);
+      if(ftr->frame!= nullptr)ftr->frame->removeKeyPoint(ftr);
       ftr->point=NULL;
-    ftr->frame->removeKeyPoint(ftr);
-  });
+  }
   pt->obs_.clear();
 
   // Delete mappoint