change delete feature

GPU_version/vio/include/vio/frame.h

@@ -119,6 +119,7 @@ namespace vio {
 
         /// Return the pose of the frame in the (w)orld coordinate frame.
         inline Vector2d pos() const {
+            assert(!T_f_w_.empty());
             SE2 tem=T_f_w_.se2();
             return tem.translation();
         }

GPU_version/vio/include/vio/point.h

@@ -19,7 +19,10 @@
 
 #include <boost/noncopyable.hpp>
 #include <vio/global.h>
+#include <boost/thread.hpp>
+#include <boost/function.hpp>
 
+static boost::mutex point_mut_;
 namespace g2o {
   class VertexSBAPointXYZ;
 }
@@ -36,7 +39,7 @@ class Point : boost::noncopyable
 {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
+
   enum PointType {
     TYPE_DELETED,
     TYPE_CANDIDATE,
@@ -59,7 +62,6 @@ public:
   int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
   int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
-  bool                        syn_on=false;
 
   Point(const Vector3d& pos);
   Point(const Vector3d& pos, Feature* ftr);

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
+  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

GPU_version/vio/src/frame.cpp

@@ -126,7 +126,7 @@ void Frame::checkKeyPoints(Feature* ftr)
 void Frame::removeKeyPoint(Feature* ftr)
 {
     bool found = false;
-    if(key_pts_.size()!=5){
+    if(key_pts_.size()!=5 ){
         key_pts_.clear();
         key_pts_.reserve(5);
         found=true;

GPU_version/vio/src/map.cpp

@@ -69,7 +69,7 @@ void Map::removePtFrameRef(Frame* frame, Feature* ftr)
   if(ftr->point == NULL)
     return; // mappoint may have been deleted in a previous ref. removal
   Point* pt = ftr->point;
-  ftr->point = NULL;
+  delete ftr->point;
   if(pt->obs_.size() <= 2)
   {
     // If the references list of mappoint has only size=2, delete mappoint
@@ -86,10 +86,10 @@ void Map::safeDeletePoint(Point* pt)
   if(pt->obs_.size()>1){
       for(auto&& ftr:pt->obs_){
           ftr->frame->removeKeyPoint(ftr);
-          ftr->point=NULL;
+          delete ftr->point;
       }
   }else{
-      pt->obs_.back()->point=NULL;
+      delete pt->obs_.back()->point;
   }
   pt->obs_.clear();
 
@@ -212,7 +212,6 @@ void Map::emptyTrash()
 {
   std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){
     delete pt;
-    pt=NULL;
   });
   trash_points_.clear();
   point_candidates_.emptyTrash();
@@ -297,7 +296,7 @@ void MapPointCandidates::deleteCandidate(PointCandidate& c)
 {
   // camera-rig: another frame might still be pointing to the candidate point
   // therefore, we can't delete it right now.
-  delete c.second; c.second=NULL;
+  delete c.second;
   c.first->type_ = Point::TYPE_DELETED;
   trash_points_.push_back(c.first);
 }
@@ -305,7 +304,7 @@ void MapPointCandidates::deleteCandidate(PointCandidate& c)
 void MapPointCandidates::emptyTrash()
 {
   std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){
-    delete p; p=NULL;
+    delete p;
   });
   trash_points_.clear();
 }

GPU_version/vio/src/point.cpp

@@ -66,6 +66,7 @@ void Point::addFrameRef(Feature* ftr)
 
 Feature* Point::findFrameRef(Frame* frame)
 {
+    boost::unique_lock<boost::mutex> lock(point_mut_);
   for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
     if((*it)->frame == frame)
       return *it;
@@ -74,8 +75,13 @@ Feature* Point::findFrameRef(Frame* frame)
 
 bool Point::deleteFrameRef(Frame* frame)
 {
+    boost::unique_lock<boost::mutex> lock(point_mut_);
   for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
   {
+    if(!(*it)->frame){
+        obs_.erase(it);
+        continue;
+    }
     if((*it)->frame->id_ == frame->id_)
     {
       obs_.erase(it);
@@ -98,26 +104,34 @@ void Point::initNormal()
 
 bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) const
 {
-  // TODO: get frame with same point of view AND same pyramid level!
+    boost::unique_lock<boost::mutex> lock(point_mut_);
+    // TODO: get frame with same point of view AND same pyramid level!
   ftr= nullptr;
   if(id_<1)return false;
+  if(obs_.size()<1)return false;
   Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize();
   double max_cos_angle = 1.0;
-  for(auto&& ob:obs_){
-      if(!ob->frame)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());
-      if(cos_angle < max_cos_angle)
-      {
-          max_cos_angle = cos_angle;
-          ftr = ob;
+  try{
+      for(auto&& ob:obs_){
+          if(!ob->frame)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());
+          if(cos_angle < max_cos_angle)
+          {
+              max_cos_angle = cos_angle;
+              ftr = ob;
+          }
       }
+  } catch (std::exception& e) {
+      std::cerr << "Exception caught : " << e.what() << std::endl;
+      exit(0);
   }
+ 
   if(ftr== nullptr)return false;
   return true;
 }

GPU_version/vio/src/reprojector.cpp

@@ -105,6 +105,7 @@ namespace vio {
                     if(keypoints_cur.size() < match.trainIdx)continue;
                     list<Feature*>::iterator point=keypoints_cur.begin();
                     std::advance(point,match.trainIdx);
+                    if(!(*point))continue;
                     if (f.item->point == NULL){
                         const int k = static_cast<int>((*point)->px.y() / grid_.cell_size) *
                                       grid_.grid_n_cols
@@ -114,7 +115,7 @@ namespace vio {
                                     (int) (*point)->px.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));
+                                px,it_frame.item.first->cam_->cam2world(f.item->px),(*point)->level));
                         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++;
@@ -128,7 +129,6 @@ namespace vio {
                         assert(grid_.cells.at(k) != nullptr);
                         Vector2d px((int) (*point)->px.x(),
                                     (int) (*point)->px.y());
-                        //f.item->point->addFrameRef(f.item);
                         grid_.cells.at(k)->push_back(Candidate(f.item->point, px));
                         overlap_kfs.back().second++;
                         break;
@@ -171,6 +171,7 @@ namespace vio {
             }
 
             syn_=true;
+
             if(!matcher_.findMatchDirect(*it->pt, *frame, it->px))
             {
                 it->pt->n_failed_reproj_++;