From b44d04faba9a2dd7688faf311efcac03df826d8f Mon Sep 17 00:00:00 2001
From: VahdPourAmin <v.pouramin@gmail.com>
Date: Tue, 31 Aug 2021 13:34:05 +0200
Subject: [PATCH] working version 5

---
 GPU_version/vio/src/frame_handler_mono.cpp |  4 +--
 GPU_version/vio/src/reprojector.cpp        | 37 ++++++++++++++--------
 2 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp
index a67f222..563b27b 100644
--- a/GPU_version/vio/src/frame_handler_mono.cpp
+++ b/GPU_version/vio/src/frame_handler_mono.cpp
@@ -170,8 +170,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   //assert(new_frame_.get()== nullptr);
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
-  //reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-    //reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
+//  reprojector_.reprojectMap(new_frame_, overlap_kfs_);
+//    reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
     reprojector_.reprojectMap2(new_frame_, overlap_kfs_);
   //assert (false);
 
diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp
index 49ccd17..74adc9b 100644
--- a/GPU_version/vio/src/reprojector.cpp
+++ b/GPU_version/vio/src/reprojector.cpp
@@ -162,17 +162,16 @@ void Reprojector::reprojectMap(
         matcher->match(descriptors_ref,descriptors_cur,matches);
         //std::cerr<<"matches size : "<<matches.size()<<std::endl;
 
-        for(int i=0; i < matches.size(); i++)
-        {
-            auto fts = framel->fts_.begin();
-            auto fts2 = fts;
-            for(int f = 0; f < matches[i].queryIdx; f++)
-                fts2++;
-            if(reprojectPoint1(frame, (*fts2)->point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)){
-                overlap_kfs.back().second++;
+        int i = 0;
+        for(auto&& f: _for(framel->fts_)){
+            if (f.index == matches[i].queryIdx) {
+                if (reprojectPoint1(frame, f.item->point, keypoints_cur[matches[i].trainIdx].pt.x,
+                                    keypoints_cur[matches[i].trainIdx].pt.y)) {
+                    overlap_kfs.back().second++;
+                }
+                keypoints_cur[i].class_id = 0;
+                i++;
             }
-            keypoints_cur[i].class_id = 0;
-
         }
         for(int i=0; i < keypoints_cur.size(); i++)
         {
@@ -251,12 +250,25 @@ void Reprojector::reprojectMap(
             }
             extractor->compute(it_frame->first->img(), keypoints_kfs, descriptors_ref);
             matcher->match(descriptors_ref, descriptors_cur, matches);
+            int i = 0;
             for(auto&& f: _for(ref_frame->fts_)){
-                if (reprojectPoint1(frame, f.item->point, keypoints_cur[matches[f.index].trainIdx].pt.x, keypoints_cur[matches[f.index].trainIdx].pt.y)) {
-                    overlap_kfs.back().second++;
+                if (f.index == matches[i].queryIdx) {
+                    if (reprojectPoint1(frame, f.item->point, keypoints_cur[matches[i].trainIdx].pt.x,
+                                        keypoints_cur[matches[i].trainIdx].pt.y)) {
+                        overlap_kfs.back().second++;
+//                        keypoints_cur[matches[i].trainIdx].class_id = 0;
+                    }
+                    i++;
                 }
             }
         }
+//        for(int i=0; i < keypoints_cur.size(); i++)
+//        {
+//            if(keypoints_cur[i].class_id == 0)
+//                continue;
+//            if(reprojectPoint1(frame, nullptr, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y))
+//                overlap_kfs.back().second++;
+//        }
         boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
         auto it=map_.point_candidates_.candidates_.begin();
         while(it!=map_.point_candidates_.candidates_.end())
@@ -296,7 +308,6 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
   while(it!=cell.end())
   {
     ++n_trials_;
-    //std::cerr<<"n_trials_ : "<<n_trials_<<"\t"<<it->pt->type_<<"\t"<<it->pt->n_failed_reproj_<<"\t"<<it->pt->n_succeeded_reproj_<<std::endl;
     if(it->pt->type_ == Point::TYPE_DELETED)
     {
       it = cell.erase(it);