diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml
index 0e0de00..515c6a4 100644
--- a/GPU_version/vio/param/vo_fast.yaml
+++ b/GPU_version/vio/param/vo_fast.yaml
@@ -8,7 +8,7 @@ vio:
   init_min_disparity: 10
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
-  poseoptim_thresh: 2.0
+  poseoptim_thresh: 3.5
   ACC_ekf: 0.2          #acc white noise in continuous in EKF
   GYO_ekf: 0.1         #gyro white noise in continuous in EKF
   VO_ekf_translation: 1.0              #VO noise  in EKF
diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp
index 97b373c..fa3def3 100644
--- a/GPU_version/vio/src/frame.cpp
+++ b/GPU_version/vio/src/frame.cpp
@@ -171,6 +171,10 @@ bool Frame::getSceneDepth(vio::Map& map,double& depth_mean, double& depth_min)
   vector<double> depth_vec;
   for(auto it=fts_.begin(); it!=fts_.end();)
   {
+      if((*it)->point==NULL){
+          ++it;
+          continue;
+      }
       double z=w2f((*it)->point->pos_).z();
       if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0. || z<0.05 || z > 20.0){
           map.safeDeletePoint((*it)->point);
diff --git a/GPU_version/vio/src/global_optimizer.cpp b/GPU_version/vio/src/global_optimizer.cpp
index fe1bbcd..9b62651 100644
--- a/GPU_version/vio/src/global_optimizer.cpp
+++ b/GPU_version/vio/src/global_optimizer.cpp
@@ -130,7 +130,7 @@ namespace vio {
                 if (v->dimension() == 3 && v->edges().size()>2) points.push_back(v);
             }
             // Optimization
-            if(points.empty() || optimizer_->vertices().empty()){
+            if(points.empty()){
                 optimizer_->clear();
                 ba_mux_.unlock();
                 continue;
@@ -157,8 +157,8 @@ namespace vio {
             for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                 it_kf != map_.keyframes_.end();++it_kf)
             {
-/*                (*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
-                                        (*it_kf)->v_kf_->estimate().translation()));*/
+                (*it_kf)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
+                                        (*it_kf)->v_kf_->estimate().translation()));
                 for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                 {
                     if((*it_ftr)->point == NULL)