From e2be0be2c6a0c53ebe62f9cde457cc927f3a6aa8 Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 31 Aug 2021 14:09:22 +0200 Subject: [PATCH] latest version majid --- GPU_version/vio/include/vio/reprojector.h | 3 +- GPU_version/vio/launch/Px30.launch | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 7 +- GPU_version/vio/src/reprojector.cpp | 85 +++++++++++----------- 4 files changed, 48 insertions(+), 49 deletions(-) diff --git a/GPU_version/vio/include/vio/reprojector.h b/GPU_version/vio/include/vio/reprojector.h index 605265a..26cd13d 100644 --- a/GPU_version/vio/include/vio/reprojector.h +++ b/GPU_version/vio/include/vio/reprojector.h @@ -72,6 +72,7 @@ public: void reprojectMap2( FramePtr frame, + FramePtr last_frame, std::vector< std::pair >& overlap_kfs); private: @@ -106,7 +107,7 @@ private: void resetGrid(); bool reprojectCell(Cell& cell, FramePtr frame); bool reprojectPoint(FramePtr frame, Point* point); - bool reprojectPoint1(FramePtr frame, Point* point, float x, float y); + bool reprojectPoint1(FramePtr frame, Point* point, cv::KeyPoint* px_cv); }; } // namespace vio diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index cda2ecb..d1bf289 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -1,6 +1,6 @@ - + diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index a67f222..24d0f4e 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -171,11 +171,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() 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_.reprojectMap2(new_frame_, overlap_kfs_); + // reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); + std::cerr<<"new frame\n"; + reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); //assert (false); - std::cout<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 49ccd17..57b5ff2 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -168,7 +168,7 @@ void Reprojector::reprojectMap( 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)){ + if(reprojectPoint1(frame, (*fts2)->point, &keypoints_cur[i])){ overlap_kfs.back().second++; } keypoints_cur[i].class_id = 0; @@ -178,7 +178,7 @@ void Reprojector::reprojectMap( { if(keypoints_cur[i].class_id == 0) continue; - if(reprojectPoint1(frame, nullptr, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) + if(reprojectPoint1(frame, nullptr, &keypoints_cur[i])) overlap_kfs.back().second++; } { @@ -211,6 +211,7 @@ void Reprojector::reprojectMap( // TODO - second reimplementation void Reprojector::reprojectMap2( FramePtr frame, + FramePtr last_frame, std::vector< std::pair >& overlap_kfs) { resetGrid(); @@ -222,38 +223,33 @@ void Reprojector::reprojectMap( extractor->compute(frame->img(), keypoints_cur, descriptors_cur); list< pair > close_kfs; map_.getCloseKeyframes(frame, close_kfs); + if(!last_frame->fts_.empty())close_kfs.push_back(pair(last_frame, (frame->T_f_w_.se2().translation()-last_frame->T_f_w_.se2().translation()).norm())); close_kfs.sort(boost::bind(&std::pair::second, _1) < boost::bind(&std::pair::second, _2)); - size_t n = 0; overlap_kfs.reserve(options_.max_n_kfs); - for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); - it_frame!=ite_frame && noptions_.max_n_kfs)continue; + std::cerr<id_<<'\n'; std::vector keypoints_kfs; std::vector matches; cv::Mat descriptors_ref; - FramePtr ref_frame = it_frame->first; - overlap_kfs.push_back(pair(ref_frame,0)); - - for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); - it_ftr!=ite_ftr; ++it_ftr) + overlap_kfs.push_back(pair(it_frame.item.first,0)); + for(auto&& it_ftr:it_frame.item.first->fts_) { - if((*it_ftr)->point == NULL) - continue; - - if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) - continue; - (*it_ftr)->point->last_projected_kf_id_ = frame->id_; - cv::KeyPoint kp; - kp.pt.x = (*it_ftr)->px.x(); - kp.pt.y = (*it_ftr)->px.y(); - keypoints_kfs.push_back(kp); + keypoints_kfs.push_back(cv::KeyPoint(it_ftr->px.x(),it_ftr->px.y(),1)); } - extractor->compute(it_frame->first->img(), keypoints_kfs, descriptors_ref); + extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref); matcher->match(descriptors_ref, descriptors_cur, matches); - 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++; + for(auto&& f: _for(it_frame.item.first->fts_)){ + // if(f.item->point->last_projected_kf_id_ == it_frame.item.first->id_ || f.index > matches.size()-1)continue; + // f.item->point->last_projected_kf_id_ = it_frame.item.first->id_; + for(auto&& match:matches){ + if(match.queryIdx!=f.index)continue; + //if(match.distance>100.0)continue; + assert(keypoints_cur.size()>match.trainIdx); + if(reprojectPoint1(frame, f.item->point, &keypoints_cur.at(match.trainIdx))==true) { + overlap_kfs.back().second++; + } } } } @@ -296,7 +292,6 @@ 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); @@ -358,33 +353,35 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point) } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO first reimplementation - bool Reprojector::reprojectPoint1(FramePtr frame, Point* point, float x, float y) + bool Reprojector::reprojectPoint1(FramePtr frame, Point* point, cv::KeyPoint* px_cv) { - if(point) { + if(point != nullptr) { + Vector2d px((int)px_cv->pt.x, (int)px_cv->pt.y); + if (frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher + { + const int k = static_cast(px[1] / grid_.cell_size) * grid_.grid_n_cols + + static_cast(px[0] / grid_.cell_size); + if(kpush_back(Candidate(point, px)); + return true; + } + } + return false; + } + return false;/*else{ Vector2d px(x, y); if (frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher { const int k = static_cast(px[1] / grid_.cell_size) * grid_.grid_n_cols + static_cast(px[0] / grid_.cell_size); - grid_.cells.at(k)->push_back(Candidate(point, px)); + frame->fts_.push_back(Fea) + grid_.cells.at(k)->push_back(Candidate(vio::Point(Vector3d(frame->cam_->cam2world(x, y))), px)); return true; } return false; - } - else{ - Vector2d px(x, y); - if (frame->cam_->isInFrame(px.cast(), 8)) // 8px is the patch size in the matcher - { - const int k = static_cast(px[1] / grid_.cell_size) * grid_.grid_n_cols - + static_cast(px[0] / grid_.cell_size); - Point* pnt = new Point(Vector3d(frame->cam_->cam2world(x, y))); - pnt->type_ = Point::TYPE_UNKNOWN; - grid_.cells.at(k)->push_back(Candidate(pnt, px)); - delete pnt; - return true; - } - return false; - } + }*/ } } // namespace vio