change the fast detector

GPU_version/vio/include/vio/feature_detection.h

@@ -20,6 +20,9 @@
 #include <vio/global.h>
 #include <vio/frame.h>
 #include <vio/cl_class.h>
+#include <opencv2/xfeatures2d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/core.hpp>
 
 namespace vio {
 
@@ -57,13 +60,15 @@ public:
       Frame* frame,
       const ImgPyr& img_pyr,
       const double detection_threshold,
-      Features& fts) = 0;
+      Features& fts,
+      cv::Mat* descriptors= nullptr) = 0;
 
   /// Flag the grid cell as occupied
   void setGridOccpuancy(const Vector2d& px);
 
   /// Set grid cells of existing features as occupied
   void setExistingFeatures(const Features& fts);
+  void resetGrid();
 
 protected:
 
@@ -74,8 +79,6 @@ protected:
   const int grid_n_rows_;
   vector<bool> grid_occupancy_;
 
-  void resetGrid();
-
   inline int getCellIndex(int x, int y, int level)
   {
     const int scale = (1<<level);
@@ -84,7 +87,7 @@ protected:
 };
 typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
 
-/// FAST detector by Edward Rosten.
+/// FAST detector by Majid Geravand.
 class FastDetector : public AbstractDetector
 {
 public:
@@ -101,7 +104,8 @@ public:
       Frame* frame,
       const ImgPyr& img_pyr,
       const double detection_threshold,
-      Features& fts);
+      Features& fts,
+      cv::Mat* descriptors= nullptr);
   opencl* gpu_fast_;
 };
 

GPU_version/vio/include/vio/reprojector.h

@@ -72,7 +72,6 @@ public:
       std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
       opencl* gpu_fast_);
 
-  void fast_detector(cv::Mat img, opencl * gpu_fast_, std::vector<cv::KeyPoint>& keypoints_cur, double detection_threshold);
 
 private:
 

GPU_version/vio/src/feature_detection.cpp

@@ -69,9 +69,10 @@ void FastDetector::detect(
     Frame* frame,
     const ImgPyr& img_pyr,
     const double detection_threshold,
-    Features& fts)
+    Features& fts,
-{
+    cv::Mat* descriptors){
   Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
+  std::vector<cv::KeyPoint> keypoints;
   for(int L=0; L<n_pyr_levels_; ++L)
   {
     const int scale = (1<<L);
@@ -100,14 +101,20 @@ void FastDetector::detect(
       if(score > corners.at(k).score){
           corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
           // Create feature for every corner that has high enough corner score
-          if(corners.at(k).score > detection_threshold)
+          if(corners.at(k).score > detection_threshold){
               fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
+              if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x, fast_corners[i].y, 1));
+          }
       }
     }
 
   }
+  if(descriptors){
+      cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
+      extractor->compute(frame->img(), keypoints, *descriptors);
+  }
   resetGrid();
-}
+};
 
 } // namespace feature_detection
 } // namespace vio

GPU_version/vio/src/frame_handler_mono.cpp

@@ -176,51 +176,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->Cov_ = init_f.first;
   // sparse image align
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
-/*#if VIO_DEBUG
-    fprintf(log_,"[%s] Before alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
-            new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
-            new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
-            fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
-    int nullcount = 0;
-    int nullpointcount = 0;
-    for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
-    {
-        if(*f == nullptr) {
-            nullcount++;
-            continue;
-        }
-        if((*f)->point == nullptr) {
-            nullpointcount++;
-            continue;
-        }
-        fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
-                (*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
-    }
-    fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
-#endif*/
   if(!img_align.run(last_frame_, new_frame_, log_))new_frame_->T_f_w_=init_f.second;
-/*#if VIO_DEBUG
-    fprintf(log_,"[%s] After alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
-            new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
-            new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
-            fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
-    nullcount = 0;
-    nullpointcount = 0;
-    for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
-    {
-        if(*f == nullptr) {
-            nullcount++;
-            continue;
-        }
-        if((*f)->point == nullptr) {
-            nullpointcount++;
-            continue;
-        }
-        fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
-                (*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
-    }
-    fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
-#endif*/
   reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
   int n_point=0;
   for(auto i:overlap_kfs_)n_point+=i.second;

GPU_version/vio/src/initialization.cpp

@@ -81,15 +81,6 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
       return NO_KEYFRAME;
   }
     frame_cur->T_f_w_ = T_cur_from_ref_;
-  // Rescale the map such that the mean scene depth is equal to the specified scale
-/*  vector<double> depth_vec;
-  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
-    depth_vec.push_back((xyz_in_cur_[i]).z());
-  double scene_depth_median = vk::getMedian(depth_vec);
-  double scale = Config::mapScale()/scene_depth_median;
-  frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
-  frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
-                                -frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));*/
   // For each inlier create 3D point and add feature in both frames
     for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
     {

GPU_version/vio/src/reprojector.cpp

@@ -28,6 +28,7 @@
 #include <vio/math_utils.h>
 #include <vio/timer.h>
 #include <fstream>
+#include <vio/feature_detection.h>
 
 #include <opencv2/xfeatures2d.hpp>
 #include <opencv2/features2d.hpp>
@@ -118,31 +119,6 @@ namespace vio {
         }
     }
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// TODO - fast feature detector
-    void Reprojector::fast_detector(cv::Mat img, opencl * gpu_fast_, std::vector<cv::KeyPoint>& keypoints_cur, double detection_threshold = Config::triangMinCornerScore()) {
-        const int width = img.cols;
-        const int height = img.rows;
-        gpu_fast_->reload_buf(0,0,img);
-        int icorner[1]={0};
-        gpu_fast_->reload_buf(0,2, icorner);
-        gpu_fast_->run(0, img.cols, img.rows);
-        int count[1]={0};
-        gpu_fast_->read(0,2,1, count);
-        cl_int2 fast_corners[350000];
-        gpu_fast_->read(0,1,count[0], fast_corners);
-        for(uint i = 0; i < count[0]; ++i)
-        {
-            if(fast_corners[i].x < 0 || fast_corners[i].x > width || fast_corners[i].y < 0 || fast_corners[i].y > height)
-                continue;
-
-            const float score = vk::shiTomasiScore(img, fast_corners[i].x, fast_corners[i].y);
-            if(score > detection_threshold) {
-                cv::KeyPoint kp(fast_corners[i].x, fast_corners[i].y, 7);
-                keypoints_cur.push_back(kp);
-            }
-        }
-    }
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // TODO - second reimplementation
     void Reprojector::reprojectMap2(
             FramePtr frame,
@@ -155,12 +131,7 @@ namespace vio {
         std::vector<cv::KeyPoint> keypoints_cur;
         cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
         cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
-        //cv::FAST(frame->img(), keypoints_cur, 50);
-        //for (int i = 0; i < keypoints_cur.size(); i++)
-        //    std::cerr<<keypoints_cur[i].pt.x<<"\t"<<keypoints_cur[i].pt.y<<"\t"<<keypoints_cur[i].size<<"\t"<<keypoints_cur[i].class_id<<"\t"<<keypoints_cur[i].octave<<"\t"<<keypoints_cur[i].angle<<std::endl;
         fast_detector(frame->img(), gpu_fast_, keypoints_cur);
-        //for (int i = 0; i < keypoints_cur.size(); i++)
-        //    std::cerr<<keypoints_cur[i].pt.x<<"\t"<<keypoints_cur[i].pt.y<<"\t"<<keypoints_cur[i].size<<"\t"<<keypoints_cur[i].class_id<<"\t"<<keypoints_cur[i].octave<<"\t"<<keypoints_cur[i].angle<<std::endl;
         extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
         list<pair<FramePtr, double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);