diff --git a/GPU_version/vio/include/vio/reprojector.h b/GPU_version/vio/include/vio/reprojector.h index 9f859e4..6b6e2b9 100644 --- a/GPU_version/vio/include/vio/reprojector.h +++ b/GPU_version/vio/include/vio/reprojector.h @@ -22,6 +22,7 @@ #include #include #include +#include namespace vk { class AbstractCamera; @@ -68,7 +69,10 @@ public: void reprojectMap2( FramePtr frame, FramePtr last_frame, - std::vector< std::pair >& overlap_kfs); + std::vector< std::pair >& overlap_kfs, + opencl* gpu_fast_); + + void fast_detector(cv::Mat img, opencl * gpu_fast_, std::vector& keypoints_cur, double detection_threshold); private: diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 56c102e..e84b1e4 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -221,7 +221,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() } fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount); #endif*/ - reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); + reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_); int n_point=0; for(auto i:overlap_kfs_)n_point+=i.second; #if VIO_DEBUG diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 9dad605..b87c571 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -117,20 +117,50 @@ namespace vio { break; } } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// TODO - fast feature detector + void Reprojector::fast_detector(cv::Mat img, opencl * gpu_fast_, std::vector& keypoints_cur, double detection_threshold = Config::triangMinCornerScore()) { + const int width = img.cols; + const int height = img.rows; + gpu_fast_->reload_buf(0,0,img); + int icorner[1]={0}; + gpu_fast_->reload_buf(0,2, icorner); + gpu_fast_->run(0, img.cols, img.rows); + int count[1]={0}; + gpu_fast_->read(0,2,1, count); + cl_int2 fast_corners[350000]; + gpu_fast_->read(0,1,count[0], fast_corners); + for(uint i = 0; i < count[0]; ++i) + { + if(fast_corners[i].x < 0 || fast_corners[i].x > width || fast_corners[i].y < 0 || fast_corners[i].y > height) + continue; + const float score = vk::shiTomasiScore(img, fast_corners[i].x, fast_corners[i].y); + if(score > detection_threshold) { + cv::KeyPoint kp(fast_corners[i].x, fast_corners[i].y, 7); + keypoints_cur.push_back(kp); + } + } + } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO - second reimplementation void Reprojector::reprojectMap2( FramePtr frame, FramePtr last_frame, - std::vector > &overlap_kfs) { + std::vector > &overlap_kfs, + opencl* gpu_fast_) { if(frame->id_<1)return; 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); + //for (int i = 0; i < keypoints_cur.size(); i++) + // std::cerr<img(), gpu_fast_, keypoints_cur); + //for (int i = 0; i < keypoints_cur.size(); i++) + // std::cerr<compute(frame->img(), keypoints_cur, descriptors_cur); list > close_kfs; map_.getCloseKeyframes(frame, close_kfs);