diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 563b27b..a67f222 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 74adc9b..49ccd17 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -162,16 +162,17 @@ void Reprojector::reprojectMap( matcher->match(descriptors_ref,descriptors_cur,matches); //std::cerr<<"matches size : "<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++; + 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++; } + keypoints_cur[i].class_id = 0; + } for(int i=0; i < keypoints_cur.size(); i++) { @@ -250,25 +251,12 @@ 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 (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++; + 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++; } } } -// 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 lock(map_.point_candidates_.mut_); auto it=map_.point_candidates_.candidates_.begin(); while(it!=map_.point_candidates_.candidates_.end()) @@ -308,6 +296,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) while(it!=cell.end()) { ++n_trials_; + //std::cerr<<"n_trials_ : "<pt->type_<<"\t"<pt->n_failed_reproj_<<"\t"<pt->n_succeeded_reproj_<pt->type_ == Point::TYPE_DELETED) { it = cell.erase(it);