diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h
index 98e7b35..3c91654 100644
--- a/GPU_version/vio/include/vio/frame.h
+++ b/GPU_version/vio/include/vio/frame.h
@@ -119,7 +119,8 @@ namespace vio {
 
         /// Return the pose of the frame in the (w)orld coordinate frame.
         inline Vector2d pos() const {
-            return T_f_w_.se2().translation();
+            Vector2d tem=T_f_w_.se2().translation();
+            return tem;
         }
 
         inline SE3 se3() const{
diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h
index 3aab729..28eed6c 100644
--- a/GPU_version/vio/include/vio/global.h
+++ b/GPU_version/vio/include/vio/global.h
@@ -99,7 +99,9 @@ namespace vio
             return SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
         }
         ~SE2_5(){
-           // delete T2_;
+        }
+        bool empty(){
+            return T2_ == nullptr ? true : false;
         }
 
     private:
diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h
index 00dc47f..347f272 100644
--- a/GPU_version/vio/include/vio/matcher.h
+++ b/GPU_version/vio/include/vio/matcher.h
@@ -99,6 +99,7 @@ public:
   double h_inv_;                //!< hessian of 1d image alignment along epipolar line
   int search_level_;
   bool reject_;
+  Feature* ref_ftr_;
   Vector2d px_cur_;
 
   Matcher() = default;
diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp
index fbef1e8..843be4c 100644
--- a/GPU_version/vio/src/matcher.cpp
+++ b/GPU_version/vio/src/matcher.cpp
@@ -140,7 +140,6 @@ bool Matcher::findMatchDirect(
     const Frame& cur_frame,
     Vector2d& px_cur)
 {
-  Feature* ref_ftr_= nullptr;
   if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
   //if(ref_ftr_->frame->cam_== nullptr)return false;
   assert(ref_ftr_->frame!= nullptr);
diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp
index ccbab25..d88e4ab 100644
--- a/GPU_version/vio/src/point.cpp
+++ b/GPU_version/vio/src/point.cpp
@@ -102,7 +102,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
   double max_cos_angle = 1.0;
   if(obs_.empty())return false;
   for(auto&& ob:obs_){
-      assert(ob->frame!= nullptr);
+      if(ob->frame->T_f_w_.empty())continue;
       Vector2d t=ob->frame->pos();
       Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_);
       double cos_angle = obs_dir.dot(dir.normalized());
diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp
index fe4e67c..0bccbf4 100644
--- a/GPU_version/vio/src/reprojector.cpp
+++ b/GPU_version/vio/src/reprojector.cpp
@@ -189,41 +189,50 @@ namespace vio {
 
     bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
         cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
-        for(auto&& it : cell) {
+        Cell::iterator it=cell.begin();
+        while(it!=cell.end())
+        {
             ++n_trials_;
-            if (it.pt->type_ == Point::TYPE_DELETED) {
-               // cell.erase(it.index);
-                continue;
-            }
-            if (!matcher_.findMatchDirect(*it.pt, *frame, it.px)) {
-                it.pt->n_failed_reproj_++;
-                if (it.pt->type_ == Point::TYPE_UNKNOWN && it.pt->n_failed_reproj_ > 15)
-                    map_.safeDeletePoint(it.pt);
-                if (it.pt->type_ == Point::TYPE_CANDIDATE && it.pt->n_failed_reproj_ > 30)
-                    map_.point_candidates_.deleteCandidatePoint(it.pt);
-                //it = cell.erase(it);
-                continue;
-            }
-            it.pt->n_succeeded_reproj_++;
-            if (it.pt->type_ == Point::TYPE_UNKNOWN && it.pt->n_succeeded_reproj_ > 10)
-                it.pt->type_ = Point::TYPE_GOOD;
 
-            Feature *new_feature = new Feature(frame.get(), it.px, matcher_.search_level_);
+            if(it->pt->type_ == Point::TYPE_DELETED)
+            {
+                it = cell.erase(it);
+                continue;
+            }
+            bool found_match = true;
+            if(options_.find_match_direct)
+                found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
+            if(!found_match)
+            {
+                it->pt->n_failed_reproj_++;
+                if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
+                    map_.safeDeletePoint(it->pt);
+                if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
+                    map_.point_candidates_.deleteCandidatePoint(it->pt);
+                it = cell.erase(it);
+                continue;
+            }
+            it->pt->n_succeeded_reproj_++;
+            if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
+                it->pt->type_ = Point::TYPE_GOOD;
+
+            Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
             frame->addFeature(new_feature);
 
             // Here we add a reference in the feature to the 3D point, the other way
             // round is only done if this frame is selected as keyframe.
-            new_feature->point = it.pt;
-/*
-            if (matcher_.ref_ftr_->type == Feature::EDGELET) {
+            new_feature->point = it->pt;
+
+            if(matcher_.ref_ftr_->type == Feature::EDGELET)
+            {
                 new_feature->type = Feature::EDGELET;
-                new_feature->grad = matcher_.A_cur_ref_ * matcher_.ref_ftr_->grad;
+                new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
                 new_feature->grad.normalize();
             }
-*/
+
             // If the keyframe is selected and we reproject the rest, we don't have to
             // check this point anymore.
-            //it = cell.erase(it);
+            it = cell.erase(it);
 
             // Maximum one point per cell.
             return true;