solve the crash issue for feature delete

GPU_version/vio/include/gpu_svo/map.h

@@ -66,7 +66,7 @@ public:
 
   void emptyTrash();
 };
-
+static bool syn_=false;
 /// Map object which saves all keyframes which are in a map.
 class Map : boost::noncopyable
 {
@@ -75,6 +75,7 @@ public:
   list< Point* > trash_points_;         //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
   MapPointCandidates point_candidates_;
 
+
   Map();
   ~Map();
 

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/feature_alignment.cpp

@@ -36,9 +36,9 @@ bool align1D(
     Vector2d& cur_px_estimate,
     double& h_inv)
 {
-  const int halfpatch_size_ = 4;
+  const int halfpatch_size_ = 1;
-  const int patch_size = 8;
+  const int patch_size = 2;
-  const int patch_area = 64;
+  const int patch_area = 4;
   bool converged=false;
 
   // compute derivative of template and prepare inverse compositional
@@ -69,7 +69,7 @@ bool align1D(
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.5*0.5;
+  const float min_update_squared = 0.05;
   const int cur_step = cur_img.step.p[0];
   float chi2 = 0;
   Vector2f update; update.setZero();
@@ -163,9 +163,9 @@ bool align2D(
     return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
 #endif
 
-  const int halfpatch_size_ = 4;
+  const int halfpatch_size_ = 1;
-  const int patch_size_ = 8;
+  const int patch_size_ = 2;
-  const int patch_area_ = 64;
+  const int patch_area_ = 4;
   bool converged=false;
 
   // compute derivative of template and prepare inverse compositional
@@ -202,7 +202,7 @@ bool align2D(
   bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 0.5;//0.001
+  const float min_update_squared = 0.05;//0.001
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();

GPU_version/vio/src/frame_handler_mono.cpp

@@ -148,7 +148,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   pose_optimizer::optimizeGaussNewton(
             Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
-  if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){
+  if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 ||
+     fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI ||
+     sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
       new_frame_=last_frame_;
       return RESULT_NO_KEYFRAME;
   }
@@ -160,9 +162,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
   <<"\tReprojected points after opmization: "<<sfba_n_edges_final<<"\t distance between two frames: "<<
   (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
-  if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
-      return RESULT_NO_KEYFRAME;
-  }
 
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   // select keyframe

GPU_version/vio/src/map.cpp

@@ -40,6 +40,7 @@ void Map::reset()
 
 bool Map::safeDeleteFrame(FramePtr frame)
 {
+  if(syn_==true)return false;
   bool found = false;
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
   {
@@ -107,6 +108,7 @@ void Map::getCloseKeyframes(
     const FramePtr& frame,
     std::list< std::pair<FramePtr,double> >& close_kfs) const
 {
+    syn_=true;
   for(auto kf : keyframes_)
   {
     // check if kf has overlaping field of view with frame, use therefore KeyPoints
@@ -123,6 +125,7 @@ void Map::getCloseKeyframes(
       }
     }
   }
+  syn_=false;
 }
 
 FramePtr Map::getClosestKeyframe(const FramePtr& frame) const
@@ -149,6 +152,7 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
 {
   FramePtr furthest_kf;
   double maxdist = 0.0;
+  syn_=true;
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
   {
     double dist = ((*it)->pos()-pos).norm();
@@ -157,18 +161,21 @@ FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
       furthest_kf = *it;
     }
   }
+  syn_=false;
   return furthest_kf;
 }
 
 bool Map::getKeyframeById(const int id, FramePtr& frame) const
 {
   bool found = false;
+  syn_=true;
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
     if((*it)->id_ == id) {
       found = true;
       frame = *it;
       break;
     }
+  syn_=false;
   return found;
 }
 
@@ -178,6 +185,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
     R3<<R(0,0),0.0,R(0,1),
         0.0,1.0,0.0,
         R(1,0),0.0,R(1,1);
+    syn_=true;
   for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
   {
       SE2_5 tem(SE2(R*(*it)->T_f_w_.inverse().rotation_matrix(),s*R*(*it)->pos() + t));
@@ -192,6 +200,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
       (*ftr)->point->pos_ = s*R3*(*ftr)->point->pos_ + Vector3d(t(0),0.0,t(1));
     }
   }
+  syn_=false;
 }
 
 void Map::emptyTrash()

GPU_version/vio/src/matcher.cpp

@@ -161,20 +161,20 @@ bool Matcher::findMatchDirect(
   Vector2d px_scaled(px_cur/(1<<search_level_));
 
   bool success = false;
- // if(ref_ftr_->type == Feature::EDGELET)
+  if(ref_ftr_->type == Feature::EDGELET)
-  //{
+  {
-  //  Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
+    Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
-  //  dir_cur.normalize();
+    dir_cur.normalize();
-  //  success = feature_alignment::align1D(
+    success = feature_alignment::align1D(
-  //        cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
+          cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
-  //        patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
-  //}
+  }
-  //else
+  else
-  //{
+  {
     success = feature_alignment::align2D(
       cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
       options_.align_max_iter, px_scaled);
- // }
+  }
   px_cur = px_scaled * (1<<search_level_);
   return success;
 }