From 42a7222b430d19eb63fd3df663a25e02951ccaf6 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 28 Apr 2014 16:43:00 +0200 Subject: [PATCH] changed interface of feature detector. now it returns directly a list of features instead of only corners --- svo/include/svo/depth_filter.h | 2 +- svo/include/svo/feature_detection.h | 17 +++++++------ svo/include/svo/frame.h | 2 +- svo/include/svo/point.h | 3 ++- svo/src/depth_filter.cpp | 27 ++++++++------------ svo/src/feature_detection.cpp | 20 +++++++++------ svo/src/frame.cpp | 7 ------ svo/src/initialization.cpp | 23 +++++++---------- svo/src/point.cpp | 18 ++++++++++++- svo/test/test_feature_detection.cpp | 39 ++++++++++++----------------- svo/test/test_pose_optimizer.cpp | 22 +++++----------- svo/test/test_sparse_img_align.cpp | 18 ++++++------- svo_ros/src/benchmark_node.cpp | 19 +++++--------- 13 files changed, 96 insertions(+), 121 deletions(-) diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h index d943764..4ad2e29 100644 --- a/svo/include/svo/depth_filter.h +++ b/svo/include/svo/depth_filter.h @@ -46,7 +46,7 @@ struct Seed float z_range; //!< Max range of the possible depth. float sigma2; //!< Variance of normal distribution. Matrix2d patch_cov; //!< Patch covariance in reference image. - Seed(Feature* ftr, float angle, float depth_mean, float depth_min); + Seed(Feature* ftr, float depth_mean, float depth_min); }; /// Depth filter implements the Bayesian Update proposed in: diff --git a/svo/include/svo/feature_detection.h b/svo/include/svo/feature_detection.h index a5f5d26..d0990a8 100644 --- a/svo/include/svo/feature_detection.h +++ b/svo/include/svo/feature_detection.h @@ -52,15 +52,19 @@ public: virtual ~AbstractDetector() {}; virtual void detect( + Frame* frame, const ImgPyr& img_pyr, - const Features& fts, const double detection_threshold, - Corners* corners) = 0; + Features& fts) = 0; - /// Notify the detector that a cell already has a feature before detection. + /// Flag the grid cell as occupied void setGridOccpuancy(const Vector2d& px); + /// Set grid cells of existing features as occupied + void setExistingFeatures(const Features& fts); + protected: + static const int border_ = 8; //!< no feature should be within 8px of border. const int cell_size_; const int n_pyr_levels_; @@ -70,14 +74,11 @@ protected: void resetGrid(); - void setExistingFeatures(const Features& fts); - inline int getCellIndex(int x, int y, int level) { const int scale = (1< DetectorPtr; @@ -94,10 +95,10 @@ public: virtual ~FastDetector() {} virtual void detect( + Frame* frame, const ImgPyr& img_pyr, - const Features& fts, const double detection_threshold, - Corners* corners); + Features& fts); }; } // namespace feature_detection diff --git a/svo/include/svo/frame.h b/svo/include/svo/frame.h index f4413e7..8fcfd2c 100644 --- a/svo/include/svo/frame.h +++ b/svo/include/svo/frame.h @@ -79,7 +79,7 @@ public: void removeKeyPoint(Feature* ftr); /// Return number of point observations. - size_t nObs() const; + inline size_t nObs() const { return fts_.size(); } /// Check if a point in (w)orld coordinate frame is visible in the image. bool isVisible(const Vector3d& xyz_w) const; diff --git a/svo/include/svo/point.h b/svo/include/svo/point.h index 933b8bc..72a7221 100644 --- a/svo/include/svo/point.h +++ b/svo/include/svo/point.h @@ -56,7 +56,8 @@ public: int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. int last_structure_optim_; //!< Timestamp of last point optimization - Point(Vector3d pos); + Point(const Vector3d& pos); + Point(const Vector3d& pos, Feature* ftr); ~Point(); /// Add a reference to a frame. diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp index 5e26a5e..9278cca 100644 --- a/svo/src/depth_filter.cpp +++ b/svo/src/depth_filter.cpp @@ -34,7 +34,7 @@ namespace svo { int Seed::batch_counter = 0; int Seed::seed_counter = 0; -Seed::Seed(Feature* ftr, float angle, float depth_mean, float depth_min) : +Seed::Seed(Feature* ftr, float depth_mean, float depth_min) : batch_id(batch_counter), id(seed_counter++), ftr(ftr), @@ -113,29 +113,22 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi void DepthFilter::initializeSeeds(FramePtr frame) { - feature_detection::Corners corners; - feature_detector_->detect(frame->img_pyr_, frame->fts_, Config::triangMinCornerScore(), &corners); + Features new_features; + feature_detector_->setExistingFeatures(frame->fts_); + feature_detector_->detect(frame.get(), frame->img_pyr_, + Config::triangMinCornerScore(), new_features); seeds_updating_halt_ = true; lock_t lock(seeds_mut_); // by locking the updateSeeds function stops - size_t new_seeds = 0; ++Seed::batch_counter; - // now for all maximum corners, initialize a new seed - for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it) - { - // we must check again if the score is above the threshold because the Corners - // vector is initialized with dummy corners - if(it->score > Config::triangMinCornerScore()) - { - Feature* new_feature(new Feature(frame.get(), Vector2d(it->x, it->y), it->level)); - seeds_.push_back(Seed(new_feature, it->angle, new_keyframe_mean_depth_, new_keyframe_min_depth_)); - ++new_seeds; - } - } + // initialize a seed for every new feature + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_)); + }); if(options_.verbose) - SVO_INFO_STREAM("DepthFilter: Initialized "<resize(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f)); + Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f)); for(int L=0; L corners->at(k).score) - corners->at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f); + if(score > corners.at(k).score) + corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f); } } + + // Create feature for every corner that has high enough corner score + std::for_each(corners.begin(), corners.end(), [&](Corner& c) { + if(c.score > detection_threshold) + fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level)); + }); + resetGrid(); } diff --git a/svo/src/frame.cpp b/svo/src/frame.cpp index e5f4501..6234d0b 100644 --- a/svo/src/frame.cpp +++ b/svo/src/frame.cpp @@ -137,13 +137,6 @@ void Frame::removeKeyPoint(Feature* ftr) setKeyPoints(); } -size_t Frame::nObs() const -{ - size_t n=0; - std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){if(i->point != NULL) ++n; }); - return n; -} - bool Frame::isVisible(const Vector3d& xyz_w) const { Vector3d xyz_f = T_f_w_*xyz_w; diff --git a/svo/src/initialization.cpp b/svo/src/initialization.cpp index 49a7640..16d1c80 100644 --- a/svo/src/initialization.cpp +++ b/svo/src/initialization.cpp @@ -109,24 +109,19 @@ void detectFeatures( vector& px_vec, vector& f_vec) { - feature_detection::Corners corners; + Features new_features; feature_detection::FastDetector detector( frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels()); - detector.detect(frame->img_pyr_, frame->fts_, Config::triangMinCornerScore(), &corners); + detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features); // now for all maximum corners, initialize a new seed - px_vec.clear(); px_vec.reserve(corners.size()); - f_vec.clear(); f_vec.reserve(corners.size()); - for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it) - { - // we must check again if the score is above the threshold because the Corners - // vector is initialized with dummy corners - if(it->score > Config::triangMinCornerScore()) - { - px_vec.push_back(cv::Point2f(it->x, it->y)); - f_vec.push_back(frame->c2f(it->x, it->y)); - } - } + px_vec.clear(); px_vec.reserve(new_features.size()); + f_vec.clear(); f_vec.reserve(new_features.size()); + std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){ + px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1])); + f_vec.push_back(ftr->f); + delete ftr; + }); } void trackKlt( diff --git a/svo/src/point.cpp b/svo/src/point.cpp index 75f14a0..bdb14c5 100644 --- a/svo/src/point.cpp +++ b/svo/src/point.cpp @@ -24,7 +24,7 @@ namespace svo { int Point::point_counter_ = 0; -Point::Point(Vector3d pos) : +Point::Point(const Vector3d& pos) : id_(point_counter_++), pos_(pos), normal_(Vector3d::Zero()), @@ -38,6 +38,22 @@ Point::Point(Vector3d pos) : last_structure_optim_(0) {} +Point::Point(const Vector3d& pos, Feature* ftr) : + id_(point_counter_++), + pos_(pos), + normal_(Vector3d::Zero()), + n_obs_(1), + v_pt_(NULL), + last_published_ts_(0), + last_projected_kf_id_(-1), + type_(TYPE_UNKNOWN), + n_failed_reproj_(0), + n_succeeded_reproj_(0), + last_structure_optim_(0) +{ + obs_.push_front(ftr); +} + Point::~Point() {} diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp index df28508..b91e97a 100644 --- a/svo/test/test_feature_detection.cpp +++ b/svo/test/test_feature_detection.cpp @@ -17,8 +17,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -37,38 +39,29 @@ void testCornerDetector() cv::Mat img(cv::imread(img_name, 0)); assert(img.type() == CV_8UC1 && !img.empty()); - svo::ImgPyr img_pyr; - svo::frame_utils::createImgPyramid( - img, max(svo::Config::nPyrLevels(), svo::Config::kltMaxLevel()+1), img_pyr); - svo::Features fts; + vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092); + svo::FramePtr frame(new svo::Frame(cam, img, 0.0)); // Corner detection vk::Timer t; - svo::feature_detection::Corners corners; + svo::Features fts; svo::feature_detection::FastDetector fast_detector( img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels()); for(int i=0; i<100; ++i) { - fast_detector.detect(img_pyr, fts, svo::Config::triangMinCornerScore(), &corners); + fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts); } - printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, corners.size()); + printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 8.06878ms, 416)\n", t.stop()*10, fts.size()); + printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n"); + cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3); + cv::cvtColor(img, img_rgb, CV_GRAY2RGB); + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ + cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1); + }); + cv::imshow("ref_img", img_rgb); + cv::waitKey(0); - if(false) - { - cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3); - cv::cvtColor(img, img_rgb, CV_GRAY2RGB); - for(svo::feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it) - { - // we must check again if the score is above the threshold because the Corners - // vector is initialized with dummy corners - if(it->score > svo::Config::triangMinCornerScore()) - { - cv::circle(img_rgb, cv::Point2f(it->x, it->y), 4*(it->level+1), cv::Scalar(0,255,0), 1); - } - } - cv::imshow("ref_img", img_rgb); - cv::waitKey(0); - } + std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; }); } } // namespace diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp index 88035b4..7dd79f8 100644 --- a/svo/test/test_pose_optimizer.cpp +++ b/svo/test/test_pose_optimizer.cpp @@ -58,25 +58,15 @@ class PoseOptimizerTest { vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000001_0.depth", *cam_, depthmap_); // detect features - feature_detection::Corners corners; feature_detection::FastDetector detector( cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()); - detector.detect(frame_->img_pyr_, frame_->fts_, Config::triangMinCornerScore(), &corners); + detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_); size_t n_fts = 0; - for(feature_detection::Corners::iterator it=corners.begin(); it!=corners.end(); ++it) - { - // we must check again if the score is above the threshold because the Corners - // vector is initialized with dummy corners - if(it->score > Config::triangMinCornerScore()) - { - Feature* ftr(new Feature(frame_.get(), Vector2d(it->x, it->y), it->level)); - Point* point(new Point(frame_->f2w(ftr->f*depthmap_.at(it->y, it->x)))); - ftr->point = point; - ftr->frame = frame_.get(); - frame_->addFeature(ftr); - ++n_fts; - } - } + std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){ + Point* point(new Point(frame_->f2w(i->f*depthmap_.at(i->px[1], i->px[0])), i)); + i->point = point; + ++n_fts; + }); printf("Added %zu features to frame.\n", n_fts); } diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp index 9b7fa9e..fe1ece5 100644 --- a/svo/test/test_sparse_img_align.cpp +++ b/svo/test/test_sparse_img_align.cpp @@ -97,20 +97,16 @@ void SparseImgAlignTest::testSequence( dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap); // extract features, generate features with 3D points - svo::feature_detection::Corners corners; feature_detector->detect( - frame_ref_->img_pyr_, frame_ref_->fts_, svo::Config::triangMinCornerScore(), &corners); - for(svo::feature_detection::Corners::iterator corner=corners.begin(); corner!=corners.end(); ++corner) - { - svo::Feature* ftr = new svo::Feature(frame_ref_.get(), Eigen::Vector2d(corner->x, corner->y), corner->level); - Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at(corner->y, corner->x); + frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_); + std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) { + Eigen::Vector3d pt_pos_cur = i->f*depthmap.at(i->px[1], i->px[0]); Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur; - svo::Point* pt = new svo::Point(pt_pos_w); - ftr->point = pt; - frame_ref_->addFeature(ftr); - } + svo::Point* pt = new svo::Point(pt_pos_w, i); + i->point = pt; + }); - printf("Added %zu 3d pts to the reference frame.\n", corners.size()); + printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs()); T_prev_w = frame_ref_->T_f_w_; T_prevgt_w = T_gt_w; continue; diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp index bc94653..0c643eb 100644 --- a/svo_ros/src/benchmark_node.cpp +++ b/svo_ros/src/benchmark_node.cpp @@ -190,21 +190,14 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir) // extract features, generate features with 3D points svo::feature_detection::FastDetector detector( cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()); - svo::feature_detection::Corners corners; - detector.detect(frame_ref->img_pyr_, frame_ref->fts_, svo::Config::triangMinCornerScore(), &corners); - for(auto corner=corners.begin(); corner!=corners.end(); ++corner) - { - if(corner->score < Config::triangMinCornerScore()) - continue; - svo::Feature* ftr = new svo::Feature(frame_ref.get(), Eigen::Vector2d(corner->x, corner->y), corner->level); - Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at(corner->y, corner->x); + detector.detect(frame_ref.get(), frame_ref->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref->fts_); + std::for_each(frame_ref->fts_.begin(), frame_ref->fts_.end(), [&](Feature* ftr) { + Eigen::Vector3d pt_pos_cur = ftr->f*depthmap.at(ftr->px[1], ftr->px[0]); Eigen::Vector3d pt_pos_world = frame_ref->T_f_w_.inverse()*pt_pos_cur; - svo::Point* point = new svo::Point(pt_pos_world); + svo::Point* point = new svo::Point(pt_pos_world, ftr); ftr->point = point; - ftr->point->addFrameRef(ftr); - frame_ref->addFeature(ftr); - } - SVO_INFO_STREAM("Added "<nObs()<<" 3d pts to the reference frame."); vo_->setFirstFrame(frame_ref); SVO_INFO_STREAM("Set reference frame."); }