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

change the fast detector

Browse Source
This commit is contained in:
gfuse
2021-09-14 15:31:43 +02:00
parent cbbb2a348b
commit b3078fc0cd
8 changed files with 15789 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+14000
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1768
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/feature_detection.h
+9 -5
View File
@@ -20,6 +20,9 @@
#include <vio/global.h>
#include <vio/frame.h>
#include <vio/cl_class.h>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/core.hpp>
namespace vio {
@@ -57,13 +60,15 @@ public:
Frame* frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts) = 0;
Features& fts,
cv::Mat* descriptors= nullptr) = 0;
/// Flag the grid cell as occupied
void setGridOccpuancy(const Vector2d& px);
/// Set grid cells of existing features as occupied
void setExistingFeatures(const Features& fts);
void resetGrid();
protected:
@@ -74,8 +79,6 @@ protected:
const int grid_n_rows_;
vector<bool> grid_occupancy_;
void resetGrid();
inline int getCellIndex(int x, int y, int level)
{
const int scale = (1<<level);
@@ -84,7 +87,7 @@ protected:
};
typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
/// FAST detector by Edward Rosten.
/// FAST detector by Majid Geravand.
class FastDetector : public AbstractDetector
{
public:
@@ -101,7 +104,8 @@ public:
Frame* frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts);
Features& fts,
cv::Mat* descriptors= nullptr);
opencl* gpu_fast_;
};
GPU_version/vio/include/vio/reprojector.h
-1
View File
@@ -72,7 +72,6 @@ public:
std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
opencl* gpu_fast_);
void fast_detector(cv::Mat img, opencl * gpu_fast_, std::vector<cv::KeyPoint>& keypoints_cur, double detection_threshold);
private:
GPU_version/vio/src/feature_detection.cpp
+11 -4
View File
@@ -69,9 +69,10 @@ void FastDetector::detect(
Frame* frame,
const ImgPyr& img_pyr,
const double detection_threshold,
Features& fts)
{
Features& fts,
cv::Mat* descriptors){
Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
std::vector<cv::KeyPoint> keypoints;
for(int L=0; L<n_pyr_levels_; ++L)
{
const int scale = (1<<L);
@@ -100,14 +101,20 @@ void FastDetector::detect(
if(score > corners.at(k).score){
corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
// Create feature for every corner that has high enough corner score
if(corners.at(k).score > detection_threshold)
if(corners.at(k).score > detection_threshold){
fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x, fast_corners[i].y, 1));
}
}
}
}
if(descriptors){
cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
extractor->compute(frame->img(), keypoints, *descriptors);
}
resetGrid();
}
};
} // namespace feature_detection
} // namespace vio
GPU_version/vio/src/frame_handler_mono.cpp
-44
View File
@@ -176,51 +176,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->Cov_ = init_f.first;
// sparse image align
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
/*#if VIO_DEBUG
fprintf(log_,"[%s] Before alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
int nullcount = 0;
int nullpointcount = 0;
for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
{
if(*f == nullptr) {
nullcount++;
continue;
}
if((*f)->point == nullptr) {
nullpointcount++;
continue;
}
fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
}
fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
#endif*/
if(!img_align.run(last_frame_, new_frame_, log_))new_frame_->T_f_w_=init_f.second;
/*#if VIO_DEBUG
fprintf(log_,"[%s] After alignment, distance between two frames x:%f ,z=%f,angle between two frames:%f last_frame feature size : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
new_frame_->T_f_w_.se2().translation().x()-last_frame_->T_f_w_.se2().translation().x(),
new_frame_->T_f_w_.se2().translation().y()-last_frame_->T_f_w_.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()), last_frame_->fts_.size());
nullcount = 0;
nullpointcount = 0;
for(auto f = last_frame_->fts_.begin(); f!= last_frame_->fts_.end(); f++)
{
if(*f == nullptr) {
nullcount++;
continue;
}
if((*f)->point == nullptr) {
nullpointcount++;
continue;
}
fprintf(log_,"[%s] last frame : %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
(*f)->point->pos_.x(), (*f)->point->pos_.y(), (*f)->point->pos_.z());
}
fprintf(log_,"null : %d, null point : %d\n", nullcount, nullpointcount);
#endif*/
reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_);
int n_point=0;
for(auto i:overlap_kfs_)n_point+=i.second;
GPU_version/vio/src/initialization.cpp
-9
View File
@@ -81,15 +81,6 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
return NO_KEYFRAME;
}
frame_cur->T_f_w_ = T_cur_from_ref_;
// Rescale the map such that the mean scene depth is equal to the specified scale
/* vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2());
frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(),
-frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()))));*/
// For each inlier create 3D point and add feature in both frames
for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
{
GPU_version/vio/src/reprojector.cpp
+1 -30
View File
@@ -28,6 +28,7 @@
#include <vio/math_utils.h>
#include <vio/timer.h>
#include <fstream>
#include <vio/feature_detection.h>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/features2d.hpp>
@@ -118,31 +119,6 @@ namespace vio {
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// TODO - fast feature detector
void Reprojector::fast_detector(cv::Mat img, opencl * gpu_fast_, std::vector<cv::KeyPoint>& 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,
@@ -155,12 +131,7 @@ namespace vio {
std::vector<cv::KeyPoint> keypoints_cur;
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::FAST(frame->img(), keypoints_cur, 50);
//for (int i = 0; i < keypoints_cur.size(); i++)
// std::cerr<<keypoints_cur[i].pt.x<<"\t"<<keypoints_cur[i].pt.y<<"\t"<<keypoints_cur[i].size<<"\t"<<keypoints_cur[i].class_id<<"\t"<<keypoints_cur[i].octave<<"\t"<<keypoints_cur[i].angle<<std::endl;
fast_detector(frame->img(), gpu_fast_, keypoints_cur);
//for (int i = 0; i < keypoints_cur.size(); i++)
// std::cerr<<keypoints_cur[i].pt.x<<"\t"<<keypoints_cur[i].pt.y<<"\t"<<keypoints_cur[i].size<<"\t"<<keypoints_cur[i].class_id<<"\t"<<keypoints_cur[i].octave<<"\t"<<keypoints_cur[i].angle<<std::endl;
extractor->compute(frame->img(), keypoints_cur, descriptors_cur);
list<pair<FramePtr, double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);