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

changed interface of feature detector. now it returns directly a list of features instead of only corners

Browse Source
This commit is contained in:
Christian Forster
2014-04-28 16:43:00 +02:00
parent 5e48a22f4d
commit 42a7222b43
13 changed files with 96 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_ros/src/benchmark_node.cpp
+6 -13
View File
@@ -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<float>(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<float>(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 "<<corners.size()<<" 3d pts to the reference frame.");
});
SVO_INFO_STREAM("Added "<<frame_ref->nObs()<<" 3d pts to the reference frame.");
vo_->setFirstFrame(frame_ref);
SVO_INFO_STREAM("Set reference frame.");
}