add a feature detection on each key frame

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: 3.5
+  poseoptim_thresh: 1.0
   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

GPU_version/vio/src/feature_detection.cpp

@@ -73,7 +73,6 @@ void FastDetector::detect(
     list<shared_ptr<Feature>>& fts)
     {
   std::vector<cv::KeyPoint> keypoints;
-  fts.clear();
   for(int L=0; L<n_pyr_levels_; ++L)
   {
     if(L>img_pyr.size())return;

GPU_version/vio/src/frame_handler_base.cpp

@@ -98,6 +98,7 @@ void FrameHandlerBase::optimizeStructure(
     int max_iter)
 {
   for(auto&& it:frame->fts_){
+      if(it->point==NULL)continue;
       if(it->point->obs_.size()<2){
           it->point->last_frame_overlap_id_= frame->id_;
           continue;
@@ -111,7 +112,6 @@ void FrameHandlerBase::posEdit(
     {
         for(auto&& it:frame->fts_){
             if(it->point!=NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
-                std::cerr<<frame->w2f(it->point->pos_).z()<<'\n';
                 if(it->point->obs_.size()==2){
                     Eigen::Matrix<double,4,4> A,frame_a,frame_b;
                     frame_a=it->point->obs_.front()->frame->se3().matrix();

GPU_version/vio/src/frame_handler_mono.cpp

@@ -96,9 +96,9 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
   if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
       return RESULT_NO_KEYFRAME;
   }
-  new_frame_->setKeyframe();
-  map_.reset();
-  map_.addKeyframe(new_frame_);
+  //new_frame_->setKeyframe();
+  //map_.reset();
+  //map_.addKeyframe(new_frame_);
   stage_ = STAGE_SECOND_FRAME;
 #if VIO_DEBUG
     fprintf(log_,"[%s] Init: Selected first frame. \n",vio::time_in_HH_MM_SS_MMM().c_str());
@@ -218,9 +218,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-  map_.checkKeyFrames();
-  ba_glob_->new_key_frame();
-  return RESULT_IS_KEYFRAME;
+  if(map_.checkKeyFrames()){
+      ba_glob_->new_key_frame();
+      std::unique_ptr<feature_detection::FastDetector> detector=std::make_unique<feature_detection::FastDetector>(
+              new_frame_->img().cols, new_frame_->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
+      detector->detect(new_frame_, new_frame_->img_pyr_, Config::triangMinCornerScore(), new_frame_->fts_);
+      return RESULT_IS_KEYFRAME;
+  }
+  return RESULT_FAILURE;
 }
 
 

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -55,9 +55,9 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
   }
   if(!feature_counter_) // more than 10
   {
-#if VIO_DEBUG
+/*#if VIO_DEBUG
       fprintf(log,"[%s] residual zero points \n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
+#endif*/
       free(featue_px);
       free(features);
       return 0;
@@ -103,9 +103,9 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
   free(chi2_);
   free(featue_px);
   free(features);
-#if VIO_DEBUG
+/*#if VIO_DEBUG
     fprintf(log,"[%s] residual out:%f %f %f \n",vio::time_in_HH_MM_SS_MMM().c_str(),pos[0].x,pos[0].y,pos[0].z);
-#endif
+#endif*/
   if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z) || fabs(pos[0].z-cur_pos[0].z)>M_PI_2)return 1;
   cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
   return 1;