From 5578939b84cb266f62423f3fee5f5da96219fc5e Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Wed, 30 Apr 2014 15:20:01 +0200 Subject: [PATCH] depth filter now also updates old seeds with new keyframes. previously a keyframe was only used to initialize new seeds. --- svo/include/svo/depth_filter.h | 2 +- svo/include/svo/map.h | 2 +- svo/include/svo/matcher.h | 1 + svo/src/depth_filter.cpp | 35 +++++++++++++++++++++++++++------- svo/src/map.cpp | 11 ++--------- svo/src/matcher.cpp | 18 ++++++++--------- svo/test/test_depth_filter.cpp | 17 +++++++++-------- 7 files changed, 51 insertions(+), 35 deletions(-) diff --git a/svo/include/svo/depth_filter.h b/svo/include/svo/depth_filter.h index 8effbfd..7c4ab19 100644 --- a/svo/include/svo/depth_filter.h +++ b/svo/include/svo/depth_filter.h @@ -62,7 +62,7 @@ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef boost::unique_lock lock_t; - typedef boost::function callback_t; + typedef boost::function callback_t; /// Depth-filter config parameters struct Options diff --git a/svo/include/svo/map.h b/svo/include/svo/map.h index 076ee01..d8e5a71 100644 --- a/svo/include/svo/map.h +++ b/svo/include/svo/map.h @@ -48,7 +48,7 @@ public: ~MapPointCandidates(); /// Add a candidate point. - void newCandidatePoint(const Seed& seed, const Vector3d& xyz_world); + void newCandidatePoint(Point* point, double depth_sigma2); /// Adds the feature to the frame and deletes candidate from list. void addCandidatePointToFrame(FramePtr frame); diff --git a/svo/include/svo/matcher.h b/svo/include/svo/matcher.h index ddd6e6c..368e182 100644 --- a/svo/include/svo/matcher.h +++ b/svo/include/svo/matcher.h @@ -92,6 +92,7 @@ public: double h_inv_; //!< hessian of 1d image alignment along epipolar line int search_level_; Feature* ref_ftr_; + Vector2d px_cur_; Matcher(); ~Matcher(); diff --git a/svo/src/depth_filter.cpp b/svo/src/depth_filter.cpp index 0aba62a..5a56bb2 100644 --- a/svo/src/depth_filter.cpp +++ b/svo/src/depth_filter.cpp @@ -178,13 +178,17 @@ void DepthFilter::updateSeedsLoop() { new_keyframe_set_ = false; clearFrameQueue(); - initializeSeeds(new_keyframe_); - continue; + frame = new_keyframe_; + } + else + { + frame = frame_queue_.front(); + frame_queue_.pop(); } - frame = frame_queue_.front(); - frame_queue_.pop(); } updateSeeds(frame); + if(frame->isKeyframe()) + initializeSeeds(frame); } } @@ -247,13 +251,30 @@ void DepthFilter::updateSeeds(FramePtr frame) updateSeed(1./z, tau_inverse*tau_inverse, &*it); ++n_updates; + if(frame->isKeyframe()) + { + // The feature detector should not initialize new seeds close to this location + feature_detector_->setGridOccpuancy(matcher_.px_cur_); + } + // if the seed has converged, we initialize a new candidate point and remove the seed if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh) { - if(it->ftr->point == NULL) + assert(it->ftr->point == NULL); // TODO this should not happen anymore + Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu))); + Point* point = new Point(xyz_world, it->ftr); + it->ftr->point = point; + if(frame->isKeyframe()) { - Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu))); - seed_converged_cb_(*it, xyz_world); + Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_); + ftr->point = point; + point->addFrameRef(ftr); + frame->addFeature(ftr); + it->ftr->frame->addFeature(it->ftr); + } + else + { + seed_converged_cb_(point, it->sigma2); // put in candidate list } it = seeds_.erase(it); } diff --git a/svo/src/map.cpp b/svo/src/map.cpp index 1decc22..69839ad 100644 --- a/svo/src/map.cpp +++ b/svo/src/map.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include namespace svo { @@ -193,17 +192,11 @@ MapPointCandidates::~MapPointCandidates() reset(); } -void MapPointCandidates::newCandidatePoint(const Seed& seed, const Vector3d& xyz_world) +void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2) { - // initalize a new point - Point* point = new Point(xyz_world); - point->addFrameRef(seed.ftr); point->type_ = Point::TYPE_CANDIDATE; - seed.ftr->point = point; - - // add candidate to list boost::unique_lock lock(mut_); - candidates_.push_back(PointCandidate(point, seed.ftr)); + candidates_.push_back(PointCandidate(point, point->obs_.front())); } void MapPointCandidates::addCandidatePointToFrame(FramePtr frame) diff --git a/svo/src/matcher.cpp b/svo/src/matcher.cpp index 9ccc000..bced525 100644 --- a/svo/src/matcher.cpp +++ b/svo/src/matcher.cpp @@ -213,8 +213,8 @@ bool Matcher::findEpipolarMatchDirect( if(epi_length_ < 2.0) { - Vector2d px = (px_A+px_B)/2.0; - Vector2d px_scaled(px/(1<cam2world(px), depth)) + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) return true; } return false; @@ -282,8 +282,8 @@ bool Matcher::findEpipolarMatchDirect( { if(options_.subpix_refinement) { - Vector2d px(cur_frame.cam_->world2cam(uv_best)); - Vector2d px_scaled(px/(1<world2cam(uv_best); + Vector2d px_scaled(px_cur_/(1<cam2world(px), depth)) + px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) return true; } return false; } - + px_cur_ = cur_frame.cam_->world2cam(uv_best); if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) return true; } diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp index 6072218..4127338 100644 --- a/svo/test/test_depth_filter.cpp +++ b/svo/test/test_depth_filter.cpp @@ -49,7 +49,7 @@ class DepthFilterTest { public: DepthFilterTest(); virtual ~DepthFilterTest(); - void depthFilterCb(const svo::Seed& seed, const Eigen::Vector3d& xyz_world); + void depthFilterCb(svo::Point* point, double depth_sigma2); void testReconstruction(std::string dataset_dir, std::string experiment_name); std::list results_; @@ -76,15 +76,16 @@ DepthFilterTest::~DepthFilterTest() delete cam_; } -void DepthFilterTest::depthFilterCb( - const svo::Seed& seed, - const Eigen::Vector3d& xyz_world) +void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2) { - double depth = (frame_ref_->pos() - xyz_world).norm(); - double error = fabs(depth - depth_ref_.at(seed.ftr->px[1], seed.ftr->px[0])); - results_.push_back(ConvergedSeed(seed.ftr->px[0], seed.ftr->px[1], depth, error)); + double depth = (frame_ref_->pos() - point->pos_).norm(); + double error = fabs(depth - depth_ref_.at(point->obs_.front()->px[1], + point->obs_.front()->px[0])); + results_.push_back(ConvergedSeed( + point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error)); errors_.push_back(error); - delete seed.ftr; + delete point->obs_.front(); + delete point; ++n_converged_seeds_; }