public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

latest working version

Browse Source
This commit is contained in:
gfuse
2021-08-31 15:34:56 +02:00
parent 2109823b4d
commit 6cafe07db2
3 changed files with 16 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch> <launch>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/ --> <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen"/>
<!-- Camera topic to subscribe to --> <!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" /> <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/frame_handler_mono.cpp
+3 -3
View File
@@ -172,11 +172,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
//reprojector_.reprojectMap(new_frame_, overlap_kfs_); //reprojector_.reprojectMap(new_frame_, overlap_kfs_);
// reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); // reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
std::cerr<<"new frame\n";
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
//assert (false); //assert (false);
int n_point=0;
std::cerr<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second for(auto i:overlap_kfs_)n_point+=i.second;
std::cerr<<"Reprojection Map nPoint: "<<n_point
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_ <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
<<"\t distance between two frames: "<< <<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
GPU_version/vio/src/reprojector.cpp
+12 -9
View File
@@ -217,7 +217,7 @@ void Reprojector::reprojectMap(
resetGrid(); resetGrid();
cv::Mat descriptors_cur; cv::Mat descriptors_cur;
std::vector<cv::KeyPoint> keypoints_cur; std::vector<cv::KeyPoint> keypoints_cur;
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4); 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::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
cv::FAST(frame->img(),keypoints_cur,50); cv::FAST(frame->img(),keypoints_cur,50);
extractor->compute(frame->img(), keypoints_cur, descriptors_cur); extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
@@ -228,8 +228,7 @@ void Reprojector::reprojectMap(
boost::bind(&std::pair<FramePtr, double>::second, _2)); boost::bind(&std::pair<FramePtr, double>::second, _2));
overlap_kfs.reserve(options_.max_n_kfs); overlap_kfs.reserve(options_.max_n_kfs);
for(auto&& it_frame:_for(close_kfs)) { for(auto&& it_frame:_for(close_kfs)) {
//if(it_frame.index>options_.max_n_kfs)continue; if(it_frame.index>options_.max_n_kfs)continue;
std::cerr<<it_frame.index<<","<<frame->id_<<'\n';
std::vector<cv::KeyPoint> keypoints_kfs; std::vector<cv::KeyPoint> keypoints_kfs;
std::vector<cv::DMatch> matches; std::vector<cv::DMatch> matches;
cv::Mat descriptors_ref; cv::Mat descriptors_ref;
@@ -245,11 +244,15 @@ void Reprojector::reprojectMap(
// f.item->point->last_projected_kf_id_ = it_frame.item.first->id_; // f.item->point->last_projected_kf_id_ = it_frame.item.first->id_;
for(auto&& match:matches){ for(auto&& match:matches){
if(match.queryIdx!=f.index)continue; if(match.queryIdx!=f.index)continue;
//if(match.distance>100.0)continue;
assert(keypoints_cur.size()>match.trainIdx); assert(keypoints_cur.size()>match.trainIdx);
if(reprojectPoint1(frame, f.item->point, &keypoints_cur.at(match.trainIdx))==true) { const int k = static_cast<int>(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols
overlap_kfs.back().second++; + static_cast<int>(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size);
} assert(grid_.cells.at(k)!= nullptr);
if(f.item->point==NULL)continue;
Vector2d px((int)keypoints_cur.at(match.trainIdx).pt.x,(int)keypoints_cur.at(match.trainIdx).pt.y);
grid_.cells.at(k)->push_back(Candidate(f.item->point,px));
overlap_kfs.back().second++;
break;
} }
} }
} }
@@ -296,7 +299,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
{ {
it = cell.erase(it); it = cell.erase(it);
continue; continue;
} }/*
bool found_match = true; bool found_match = true;
if(options_.find_match_direct) if(options_.find_match_direct)
found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
@@ -309,7 +312,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
map_.point_candidates_.deleteCandidatePoint(it->pt); map_.point_candidates_.deleteCandidatePoint(it->pt);
it = cell.erase(it); it = cell.erase(it);
continue; continue;
} }*/
it->pt->n_succeeded_reproj_++; it->pt->n_succeeded_reproj_++;
if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
it->pt->type_ = Point::TYPE_GOOD; it->pt->type_ = Point::TYPE_GOOD;