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

fixed relocalizer and state machine. pose doesn't jump around anymore when flashed with a camera.

Browse Source
This commit is contained in:
Christian Forster
2014-05-23 15:32:15 +02:00
parent 4bc234e490
commit ab121434e4
2 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo/src/frame_handler_base.cpp
+2 -7
View File
@@ -160,17 +160,12 @@ void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
if(num_observations < Config::qualityMinFts()) if(num_observations < Config::qualityMinFts())
{ {
SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!"); SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
tracking_quality_ = TRACKING_BAD; tracking_quality_ = TRACKING_INSUFFICIENT;
} }
const int feature_drop = static_cast<int>(num_obs_last_) - num_observations; const int feature_drop = static_cast<int>(std::min(num_obs_last_, Config::maxFts())) - num_observations;
if(feature_drop > Config::qualityMaxFtsDrop()) if(feature_drop > Config::qualityMaxFtsDrop())
{ {
SVO_WARN_STREAM("Lost "<< feature_drop <<" features!"); SVO_WARN_STREAM("Lost "<< feature_drop <<" features!");
tracking_quality_ = TRACKING_BAD;
}
if(num_observations < 20)
{
SVO_ERROR_STREAM("Tracking less than 20 features!");
tracking_quality_ = TRACKING_INSUFFICIENT; tracking_quality_ = TRACKING_INSUFFICIENT;
} }
} }
svo/src/frame_handler_mono.cpp
+11 -4
View File
@@ -149,8 +149,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
const size_t repr_n_mps = reprojector_.n_trials_; const size_t repr_n_mps = reprojector_.n_trials_;
SVO_LOG2(repr_n_mps, repr_n_new_references); SVO_LOG2(repr_n_mps, repr_n_new_references);
SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references); SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
if(repr_n_new_references < 20) { if(repr_n_new_references < Config::qualityMinFts())
SVO_ERROR_STREAM("Not enough matched features."); {
SVO_WARN_STREAM_THROTTLE(1.0, "Not enough matched features.");
new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
tracking_quality_ = TRACKING_INSUFFICIENT;
return RESULT_FAILURE; return RESULT_FAILURE;
} }
@@ -177,7 +180,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
core_kfs_.insert(new_frame_); core_kfs_.insert(new_frame_);
setTrackingQuality(sfba_n_edges_final); setTrackingQuality(sfba_n_edges_final);
if(tracking_quality_ == TRACKING_INSUFFICIENT) if(tracking_quality_ == TRACKING_INSUFFICIENT)
{
new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
return RESULT_FAILURE; return RESULT_FAILURE;
}
double depth_mean, depth_min; double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD) if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
@@ -233,12 +239,13 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
const SE3& T_cur_ref, const SE3& T_cur_ref,
FramePtr ref_keyframe) FramePtr ref_keyframe)
{ {
SVO_WARN_STREAM_THROTTLE(0.5, "Relocalizing frame"); SVO_WARN_STREAM_THROTTLE(1.0, "Relocalizing frame");
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false, false); 30, SparseImgAlign::GaussNewton, false, false);
size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_); size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
if(img_align_n_tracked > 30) if(img_align_n_tracked > 30)
{ {
SE3 T_f_w_last = last_frame_->T_f_w_;
last_frame_ = ref_keyframe; last_frame_ = ref_keyframe;
FrameHandlerMono::UpdateResult res = processFrame(); FrameHandlerMono::UpdateResult res = processFrame();
if(res != RESULT_FAILURE) if(res != RESULT_FAILURE)
@@ -247,7 +254,7 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
SVO_INFO_STREAM("Relocalization successful."); SVO_INFO_STREAM("Relocalization successful.");
} }
else else
new_frame_->T_f_w_ = ref_keyframe->T_f_w_; new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose
return res; return res;
} }
return RESULT_FAILURE; return RESULT_FAILURE;