segmentation fault close obs solved

GPU_version/vio/include/vio/point.h

@@ -78,7 +78,7 @@ public:
   Feature* findFrameRef(Frame* frame);
 
   /// Get Frame with similar viewpoint.
-  bool getCloseViewObs(const Vector2d& pos, Feature*& obs) const;
+  bool getCloseViewObs(const Vector2d& pos, Feature*& obs, int id=0) const;
 
   /// Get number of observations.
   inline size_t nRefs() const { return obs_.size(); }

GPU_version/vio/src/depth_filter.cpp

@@ -299,7 +299,7 @@ namespace vio {
         lock_t lock(seeds_mut_);
         for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
         {
-            if (it->ftr->frame == frame.get())
+            if (it->ftr->frame->id_ == frame->id_)
                 seeds.push_back(*it);
         }
     }

GPU_version/vio/src/map.cpp

@@ -44,7 +44,7 @@ bool Map::safeDeleteFrame(FramePtr frame)
   bool found = false;
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
   {
-    if(*it == frame)
+    if(it->get()->id_ == frame->id_)
     {
       std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){
         removePtFrameRef(it->get(), ftr);
@@ -268,7 +268,7 @@ void MapPointCandidates::removeFrameCandidates(FramePtr frame)
   auto it=candidates_.begin();
   while(it!=candidates_.end())
   {
-    if(it->second->frame == frame.get())
+    if(it->second->frame->id_ == frame->id_)
     {
       deleteCandidate(*it);
       it = candidates_.erase(it);

GPU_version/vio/src/matcher.cpp

@@ -140,7 +140,9 @@ bool Matcher::findMatchDirect(
     const Frame& cur_frame,
     Vector2d& px_cur)
 {
-  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
+  if(pt.last_frame_overlap_id_ !=cur_frame.id_)return true;
+  if(pt.obs_.size()<2)return false;
+  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_,cur_frame.id_))return false;
   if(ref_ftr_== nullptr)return false;
   if(ref_ftr_->frame== nullptr)return false;
   if(ref_ftr_->frame->cam_== nullptr)return false;

GPU_version/vio/src/point.cpp

@@ -59,6 +59,7 @@ Point::~Point()
 
 void Point::addFrameRef(Feature* ftr)
 {
+
   obs_.push_front(ftr);
   ++n_obs_;
 }
@@ -95,7 +96,7 @@ void Point::initNormal()
   normal_set_ = true;
 }
 
-bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
+bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) const
 {
   // TODO: get frame with same point of view AND same pyramid level!
   ftr= nullptr;
@@ -103,7 +104,10 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
   double max_cos_angle = 1.0;
   if(obs_.empty())return false;
   for(auto&& ob:obs_){
-      if(ob == nullptr)continue;
+      if(ob->frame->id_==id){
+          ftr = ob;
+          return true;
+      }
       Vector2d t=ob->frame->pos();
       Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_);
       double cos_angle = obs_dir.dot(dir.normalized());

GPU_version/vio/src/reprojector.cpp

@@ -155,23 +155,33 @@ namespace vio {
                 for (auto &&match:matches) {
                     if (match.queryIdx != f.index)continue;
                     if (f.item->point == NULL){
-                        f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()));
+                        const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
-                       // assert(!f.item->point->obs_.empty());
+                                      grid_.grid_n_cols
+                                      + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+                        Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
+                                    (int) keypoints_cur.at(match.trainIdx).pt.y);
+                        frame->fts_.push_back(new Feature(it_frame.item.first.get(),
+                                                          new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
+                                px,it_frame.item.first->cam_->cam2world(f.item->px),0));
+                        frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
+                        grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
+                        overlap_kfs.back().second++;
+                        break;
+                    }else{
+                        if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
+                        f.item->point->last_frame_overlap_id_ = frame->id_;
+                        assert(keypoints_cur.size() > match.trainIdx);
+                        const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
+                                      grid_.grid_n_cols
+                                      + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
+                        assert(grid_.cells.at(k) != nullptr);
+                        Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
+                                    (int) keypoints_cur.at(match.trainIdx).pt.y);
+                        //f.item->point->addFrameRef(f.item);
+                        grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
+                        overlap_kfs.back().second++;
+                        break;
                     }
-                    if (f.item->point->last_frame_overlap_id_ == frame->id_)continue;
-                    f.item->point->last_frame_overlap_id_ = frame->id_;
-                    assert(keypoints_cur.size() > match.trainIdx);
-                    const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) *
-                                  grid_.grid_n_cols
-                                  + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
-                    assert(grid_.cells.at(k) != nullptr);
-                    Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x,
-                                (int) keypoints_cur.at(match.trainIdx).pt.y);
-                    f.item->point->addFrameRef(new Feature(it_frame.item.first.get(),f.item->px,0));
-                    //f.item->point->addFrameRef(f.item);
-                    grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
-                    overlap_kfs.back().second++;
-                    break;
                 }
             }
         }
@@ -193,12 +203,13 @@ namespace vio {
     }
 
     bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
+        if(cell.size()<1)return false;
         cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
         Cell::iterator it=cell.begin();
         while(it!=cell.end())
         {
             ++n_trials_;
-            if(it->pt->type_ == Point::TYPE_DELETED)
+            if(it->pt->type_ == Point::TYPE_DELETED || it->pt ==NULL)
             {
                 it = cell.erase(it);
                 continue;