detect that maching freak is not correct

GPU_version/vio/depth_filter_log.txt

@@ -1,2 +0,0 @@
-[13:20:52.582] New key frame 
-[13:20:52.582] BA loop 

GPU_version/vio/include/vio/matcher.h

@@ -123,6 +123,19 @@ public:
       double& depth,FILE* log);
 
   void createPatchFromPatchWithBorder();
+  uint i=0;
+  void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res){
+      cv::Mat Ref,Cur;
+      cv::cvtColor(cur,Cur,cv::COLOR_GRAY2RGB);
+      cv::cvtColor(ref,Ref,cv::COLOR_GRAY2RGB);
+      cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2);
+      cv::circle(Cur, cv::Point(cur_px_in.x(),cur_px_in.y() ), 2, cv::Scalar(0, 0,255),2);
+      cv::circle(Cur, cv::Point(cur_px_out.x(),cur_px_out.y() ), 2, cv::Scalar(0, 255,0),2);
+      cv::putText(Cur,res? "Accept": "reject",cv::Point(10,10),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA);
+      cv::imwrite(std::string(PROJECT_DIR)+"/Ref"+std::to_string(i)+".png",Ref);
+      cv::imwrite(std::string(PROJECT_DIR)+"/Cur"+std::to_string(i)+".png",Cur);
+      ++i;
+  }
 };
 
 } // namespace vio

GPU_version/vio/src/feature_detection.cpp

@@ -120,7 +120,7 @@ void FastDetector::detect(
       assert(false);
   }
   if(descriptors){
-      cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
+      cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 500);
       extractor->compute(frame->img(), keypoints, *descriptors);
   }
   resetGrid();

GPU_version/vio/src/global_optimizer.cpp

@@ -231,6 +231,7 @@ namespace vio {
        e->setVertex(1, (g2o::OptimizableGraph::Vertex*)(v_frame).get());
        e->setMeasurement(f_up);
        e->information() = weight * Eigen::Matrix2d::Identity(2,2);
+
        e->setRobustKernel(new g2o::RobustKernelHuber());
        e->setParameterId(0, 0); //old: e->setId(v_point->id());
        return e;

GPU_version/vio/src/matcher.cpp

@@ -184,6 +184,7 @@ bool Matcher::findMatchDirect(
       cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
       options_.align_max_iter, px_scaled);
   }
+  //debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[search_level_],ref_ftr_->px,px_cur,px_scaled * (1<<search_level_),success);
   px_cur = px_scaled * (1<<search_level_);
   return success;
 }

GPU_version/vio/src/reprojector.cpp

@@ -75,8 +75,8 @@ namespace vio {
         resetGrid();
         cv::Mat descriptors_cur;
         Features keypoints_cur;
-        cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
+        cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 500);
-        cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
+        cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce");
         feature_detection::FastDetector detector(
                 frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
         detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
@@ -95,10 +95,19 @@ namespace vio {
             cv::Mat descriptors_ref;
             overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
             for (auto &&it_ftr:it_frame.item.first->fts_) {
-                keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 7));
+                keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(), it_ftr->px.y(), 1));
             }
-            extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref);
+            std::vector<cv::KeyPoint> keypoints_cu;
+            for (auto &&f:keypoints_cur) {
+                keypoints_cu.push_back(cv::KeyPoint(f->px.x(), f->px.y(), 1));
+            }
+            extractor->compute(it_frame.item.first->img_pyr_[0], keypoints_kfs, descriptors_ref);
             matcher->match(descriptors_ref, descriptors_cur, matches);
+            cv::Mat imgMatch;
+            cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cu,matches, imgMatch);
+            cv::imshow("img",imgMatch);
+            cv::waitKey();
+            exit(0);
             for (auto &&f: _for(it_frame.item.first->fts_)) {
                 for (auto &&match:matches) {
                     if (match.queryIdx != f.index)continue;
@@ -198,7 +207,7 @@ namespace vio {
             bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);
             Vector2d uva(it->px.x(), it->px.y());
 #if VIO_DEBUG
-            fprintf(log_,"%f  %f  %s %f\n", it->px.x(), it->px.y(), (res?" TRUE":" FALSE"), (uvb - uva).squaredNorm());
+            fprintf(log_,"%f  %f  %s %f\n", it->px.x(), it->px.y(), (res?" TRUE":" FALSE"), (uvb - uva).norm());
 #endif
             //std::cerr<<it->px.x()<<" "<<it->px.y()<<(res?" TRUE":" FALSE")<<std::endl;
             //cv::Point pointa(it->px.x(), it->px.y());