diff --git a/GPU_version/vio/include/vio/abstract_camera.h b/GPU_version/vio/include/vio/abstract_camera.h index ccd3c98..c3e5924 100644 --- a/GPU_version/vio/include/vio/abstract_camera.h +++ b/GPU_version/vio/include/vio/abstract_camera.h @@ -9,6 +9,7 @@ #define ABSTRACT_CAMERA_H_ #include +#include namespace vk { @@ -66,8 +67,7 @@ public: inline bool isInFrame(const Vector2i &obs, int boundary, int level) const { - if(obs[0] >= boundary && obs[0] < width()/(1<= boundary && obs[1] = boundary && obs.x() < width()/(1<= boundary && obs.y() >& overlap_kfs); - void reprojectMap1( - FramePtr frame, - FramePtr framel, - std::vector< std::pair >& overlap_kfs); - void reprojectMap2( FramePtr frame, FramePtr last_frame, @@ -107,7 +102,6 @@ private: void resetGrid(); bool reprojectCell(Cell& cell, FramePtr frame); bool reprojectPoint(FramePtr frame, Point* point); - 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..be0b22e 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 9b2063b..d40723b 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -168,8 +168,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() // sparse image align //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false); //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; + 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_, last_frame_,overlap_kfs_); diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 3816fd8..117d6af 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -140,23 +140,21 @@ bool Matcher::findMatchDirect( const Frame& cur_frame, Vector2d& px_cur) { - if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_)) - return false; - if(!ref_ftr_->frame->cam_->isInFrame( - ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) + ref_ftr_= nullptr; + if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false; + if(!ref_ftr_->frame->cam_->isInFrame(ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) return false; if(ref_ftr_->frame->img_pyr_.empty())return false; if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false; // warp affine warp::getWarpMatrixAffine( - *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, + *ref_ftr_->frame->cam_, *(cur_frame.cam_), ref_ftr_->px, ref_ftr_->f, (Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(), cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_); search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false; createPatchFromPatchWithBorder(); - // px_cur should be set Vector2d px_scaled(px_cur/(1<frame->pos()(0),0.0,(*it)->frame->pos()(1)) - pos_); dir.normalize(); - double cos_angle = obs_dir.dot(dir); - if(cos_angle < max_cos_angle) - { - max_cos_angle = cos_angle; - max_it = it; - } + Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize(); + double max_cos_angle = 1.0; + if(obs_.empty())return false; + for(auto&& ob:obs_){ + Vector3d dir(Vector3d(ob->frame->pos()(0),1e-9,ob->frame->pos()(1)) + pos_); + double cos_angle = obs_dir.dot(dir.normalized()); + if(cos_angle < max_cos_angle) + { + max_cos_angle = cos_angle; + ftr = ob; + } } - ftr = *max_it; return true; } diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index c5772b3..979ab94 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -35,45 +35,41 @@ namespace vio { -Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) : - map_(map) -{ - initializeGrid(cam); -} + Reprojector::Reprojector(vk::AbstractCamera *cam, Map &map) : + map_(map) { + initializeGrid(cam); + } -Reprojector::~Reprojector() -{ - std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; }); -} + Reprojector::~Reprojector() { + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *c) { delete c; }); + } -void Reprojector::initializeGrid(vk::AbstractCamera* cam) -{ - grid_.cell_size = Config::gridSize(); - grid_.grid_n_cols = ceil(static_cast(cam->width())/grid_.cell_size); - grid_.grid_n_rows = ceil(static_cast(cam->height())/grid_.cell_size); - grid_.cells.resize(grid_.grid_n_cols*grid_.grid_n_rows); - std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell*& c){ c = new Cell; }); - grid_.cell_order.resize(grid_.cells.size()); - for(size_t i=0; i(cam->width()) / grid_.cell_size); + grid_.grid_n_rows = ceil(static_cast(cam->height()) / grid_.cell_size); + grid_.cells.resize(grid_.grid_n_cols * grid_.grid_n_rows); + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *&c) { c = new Cell; }); + grid_.cell_order.resize(grid_.cells.size()); + for (size_t i = 0; i < grid_.cells.size(); ++i) + grid_.cell_order[i] = i; + random_shuffle(grid_.cell_order.begin(), grid_.cell_order.end()); // maybe we should do it at every iteration! + } + + void Reprojector::resetGrid() { + n_matches_ = 0; + n_trials_ = 0; + std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell *c) { c->clear(); }); + } -void Reprojector::resetGrid() -{ - n_matches_ = 0; - n_trials_ = 0; - std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ c->clear(); }); -} //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO - Original implementation -void Reprojector::reprojectMap( - FramePtr frame, - std::vector< std::pair >& overlap_kfs) -{ - resetGrid(); + void Reprojector::reprojectMap( + FramePtr frame, + std::vector > &overlap_kfs) { + resetGrid(); - list< pair > close_kfs; + list > close_kfs; map_.getCloseKeyframes(frame, close_kfs); close_kfs.sort(boost::bind(&std::pair::second, _1) < @@ -81,21 +77,19 @@ void Reprojector::reprojectMap( 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 && nfirst; - overlap_kfs.push_back(pair(ref_frame,0)); + 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) - { - if((*it_ftr)->point == NULL) + for (auto it_ftr = ref_frame->fts_.begin(), ite_ftr = ref_frame->fts_.end(); + it_ftr != ite_ftr; ++it_ftr) { + if ((*it_ftr)->point == NULL) continue; - if((*it_ftr)->point->last_frame_overlap_id_ == frame->id_) + if ((*it_ftr)->point->last_frame_overlap_id_ == frame->id_) continue; - (*it_ftr)->point->last_frame_overlap_id_= frame->id_; - if(reprojectPoint(frame, (*it_ftr)->point)){ + (*it_ftr)->point->last_frame_overlap_id_ = frame->id_; + if (reprojectPoint(frame, (*it_ftr)->point)) { overlap_kfs.back().second++; } @@ -103,14 +97,11 @@ void Reprojector::reprojectMap( } { boost::unique_lock lock(map_.point_candidates_.mut_); - auto it=map_.point_candidates_.candidates_.begin(); - while(it!=map_.point_candidates_.candidates_.end()) - { - if(!reprojectPoint(frame, it->first)) - { + auto it = map_.point_candidates_.candidates_.begin(); + while (it != map_.point_candidates_.candidates_.end()) { + if (!reprojectPoint(frame, it->first)) { it->first->n_failed_reproj_ += 3; - if(it->first->n_failed_reproj_ > 30) - { + if (it->first->n_failed_reproj_ > 30) { map_.point_candidates_.deleteCandidate(*it); it = map_.point_candidates_.candidates_.erase(it); continue; @@ -119,257 +110,140 @@ void Reprojector::reprojectMap( ++it; } } - for(size_t i=0; i (size_t) Config::maxFts()) - break; - } -} - -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// TODO - First reimplementation - void Reprojector::reprojectMap1( - FramePtr frame, - FramePtr framel, - std::vector< std::pair >& overlap_kfs) - { - resetGrid(); - - std::vector keypoints_ref; - cv::KeyPoint * kp = new cv::KeyPoint; - for(auto it_ftr=framel->fts_.begin(), ite_ftr=framel->fts_.end(); - it_ftr!=ite_ftr; ++it_ftr) - { - kp->pt.x = (*it_ftr)->px.x(); - kp->pt.y = (*it_ftr)->px.y(); - keypoints_ref.push_back(*kp); - } - - overlap_kfs.reserve(1); - - overlap_kfs.push_back(pair(frame,0)); - - cv::Mat descriptors_ref, descriptors_cur; - std::vector keypoints_cur; - cv::FAST(frame->img(),keypoints_cur,50); - std::vector matches; - cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4); - extractor->compute(framel->img(), keypoints_ref, descriptors_ref); - extractor->compute(frame->img(), keypoints_cur, descriptors_cur); - cv::Ptr matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true); - matcher->match(descriptors_ref,descriptors_cur,matches); - //std::cerr<<"matches size : "<fts_.begin(); - auto fts2 = fts; - for(int f = 0; f < matches[i].queryIdx; f++) - fts2++; - if(reprojectPoint1(frame, (*fts2)->point, &keypoints_cur[i])){ - overlap_kfs.back().second++; - } - keypoints_cur[i].class_id = 0; - - } - for(int i=0; i < keypoints_cur.size(); i++) - { - if(keypoints_cur[i].class_id == 0) - continue; - if(reprojectPoint1(frame, nullptr, &keypoints_cur[i])) - 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()) - { - if(!reprojectPoint(frame, it->first)) - { - it->first->n_failed_reproj_ += 3; - if(it->first->n_failed_reproj_ > 30) - { - map_.point_candidates_.deleteCandidate(*it); - it = map_.point_candidates_.candidates_.erase(it); - continue; - } - } - ++it; - } - } - for(size_t i=0; i (size_t) Config::maxFts()) + if (n_matches_ > (size_t) Config::maxFts()) break; } } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO - second reimplementation void Reprojector::reprojectMap2( FramePtr frame, FramePtr last_frame, - std::vector< std::pair >& overlap_kfs) - { + std::vector > &overlap_kfs) { resetGrid(); cv::Mat descriptors_cur; std::vector keypoints_cur; cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4); cv::Ptr 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); - list< pair > close_kfs; + list > 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())); + 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)); overlap_kfs.reserve(options_.max_n_kfs); - for(auto&& it_frame:_for(close_kfs)) { - if(it_frame.index>options_.max_n_kfs)continue; + for (auto &&it_frame:_for(close_kfs)) { + if (it_frame.index > options_.max_n_kfs)continue; std::vector keypoints_kfs; std::vector matches; cv::Mat descriptors_ref; - overlap_kfs.push_back(pair(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(),1)); + overlap_kfs.push_back(pair(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(), 1)); } extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref); matcher->match(descriptors_ref, descriptors_cur, matches); - for(auto&& f: _for(it_frame.item.first->fts_)){ - if(f.item->point==NULL)f.item->point=new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(),f.item->px.y())); - if(f.item->point->last_frame_overlap_id_ == frame->id_)continue; + for (auto &&f: _for(it_frame.item.first->fts_)) { + if (f.item->point == NULL) + f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item); + if (f.item->point->last_frame_overlap_id_ == frame->id_)continue; f.item->point->last_frame_overlap_id_ = frame->id_; - for(auto&& match:matches){ - if(match.queryIdx!=f.index)continue; - assert(keypoints_cur.size()>match.trainIdx); - const int k = static_cast(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols + for (auto &&match:matches) { + if (match.queryIdx != f.index)continue; + assert(keypoints_cur.size() > match.trainIdx); + const int k = static_cast(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * + grid_.grid_n_cols + static_cast(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size); - assert(grid_.cells.at(k)!= nullptr); - 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)); + assert(grid_.cells.at(k) != nullptr); + 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; } } } - for(size_t i=0; i (size_t) Config::maxFts()) + if (n_matches_ > (size_t) Config::maxFts()) break; } } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs) -{ - if(lhs.pt->type_ > rhs.pt->type_) - return true; - return false; -} - -bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) -{ - cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); - Cell::iterator it=cell.begin(); - while(it!=cell.end()) - { - ++n_trials_; - if(it->pt->type_ == Point::TYPE_DELETED) - { - it = cell.erase(it); - continue; - }/* - bool found_match = true; - if(options_.find_match_direct) - found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); - if(!found_match) - { - it->pt->n_failed_reproj_++; - if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) - map_.safeDeletePoint(it->pt); - if(it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) - map_.point_candidates_.deleteCandidatePoint(it->pt); - it = cell.erase(it); - continue; - }*/ - it->pt->n_succeeded_reproj_++; - if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) - it->pt->type_ = Point::TYPE_GOOD; - - Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); - frame->addFeature(new_feature); - - // Here we add a reference in the feature to the 3D point, the other way - // round is only done if this frame is selected as keyframe. - new_feature->point = it->pt; - - if(matcher_.ref_ftr_->type == Feature::EDGELET) - { - new_feature->type = Feature::EDGELET; - new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad; - new_feature->grad.normalize(); + bool Reprojector::pointQualityComparator(Candidate &lhs, Candidate &rhs) { + if (lhs.pt->type_ > rhs.pt->type_) + return true; + return false; } - // If the keyframe is selected and we reproject the rest, we don't have to - // check this point anymore. - it = cell.erase(it); + bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) { + cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2)); + Cell::iterator it = cell.begin(); + while (it != cell.end()) { + ++n_trials_; + if (it->pt->type_ == Point::TYPE_DELETED) { + it = cell.erase(it); + continue; + } + bool found_match = true; + if (options_.find_match_direct) + found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); + if (!found_match) { + it->pt->n_failed_reproj_++; + if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) + map_.safeDeletePoint(it->pt); + if (it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30) + map_.point_candidates_.deleteCandidatePoint(it->pt); + it = cell.erase(it); + continue; + } + it->pt->n_succeeded_reproj_++; + if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) + it->pt->type_ = Point::TYPE_GOOD; + + Feature *new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); + frame->addFeature(new_feature); + + // Here we add a reference in the feature to the 3D point, the other way + // round is only done if this frame is selected as keyframe. + new_feature->point = it->pt; + + if (matcher_.ref_ftr_->type == Feature::EDGELET) { + new_feature->type = Feature::EDGELET; + new_feature->grad = matcher_.A_cur_ref_ * matcher_.ref_ftr_->grad; + new_feature->grad.normalize(); + } + + // If the keyframe is selected and we reproject the rest, we don't have to + // check this point anymore. + it = cell.erase(it); + + // Maximum one point per cell. + return true; + } + return false; + } - // Maximum one point per cell. - return true; - } - return false; -} //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO original implementation -bool Reprojector::reprojectPoint(FramePtr frame, Point* point) -{ - Vector2d px(frame->w2c(point->pos_)); - 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)); - return true; - } - return false; -} -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// TODO first reimplementation - bool Reprojector::reprojectPoint1(FramePtr frame, Point* point, cv::KeyPoint* px_cv) - { - 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; + bool Reprojector::reprojectPoint(FramePtr frame, Point *point) { + Vector2d px(frame->w2c(point->pos_)); + 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)); + 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); - 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; - }*/ + return false; } - -} // namespace vio -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// \ No newline at end of file +} \ No newline at end of file