FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
const SE3d& T_cur_ref,
FramePtr ref_keyframe)
{
SVO_WARN_STREAM_THROTTLE(1.0, "Relocalizing frame");
if(ref_keyframe == nullptr)
{
SVO_INFO_STREAM("No reference keyframe.");
return RESULT_FAILURE;
}
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false, false);
size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
if(img_align_n_tracked > 30)
{
SE3d T_f_w_last = last_frame_->T_f_w_;
last_frame_ = ref_keyframe;
FrameHandlerMono::UpdateResult res = processFrame();
if(res != RESULT_FAILURE)
{
stage_ = STAGE_DEFAULT_FRAME;
SVO_INFO_STREAM("Relocalization successful.");
}
else
new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose
return res;
}
return RESULT_FAILURE;
}
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
{
reset();
detectFeatures(frame_ref, px_ref_, f_ref_);
if(px_ref_.size() < 100)
{
SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
return FAILURE;
}
frame_ref_ = frame_ref;
px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
return SUCCESS;
}
InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref, Matrix3d orient, Vector3d pos)
{
reset();
detectFeatures(frame_ref, px_ref_, f_ref_);
if(px_ref_.size() < 100)
{
SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
return FAILURE;
}
frame_ref_ = frame_ref;
px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
T_first_frame_real_scale = SE3(orient, pos);
frame_ref_->T_f_w_ = T_first_frame_real_scale;
return SUCCESS;
}
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
{
// Set initial pose TODO use prior
new_frame_->T_f_w_ = last_frame_->T_f_w_;
// sparse image align
SVO_START_TIMER("sparse_img_align");
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::GaussNewton, false, false);
size_t img_align_n_tracked = img_align.run(last_frame_, new_frame_);
SVO_STOP_TIMER("sparse_img_align");
SVO_LOG(img_align_n_tracked);
SVO_DEBUG_STREAM("Img Align:\t Tracked = " << img_align_n_tracked);
// map reprojection & feature alignment
SVO_START_TIMER("reproject");
reprojector_.reprojectMap(new_frame_, overlap_kfs_);
SVO_STOP_TIMER("reproject");
const size_t repr_n_new_references = reprojector_.n_matches_;
const size_t repr_n_mps = reprojector_.n_trials_;
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);
if(repr_n_new_references < Config::qualityMinFts())
{
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;
}
// pose optimization
SVO_START_TIMER("pose_optimizer");
size_t sfba_n_edges_final;
double sfba_thresh, sfba_error_init, sfba_error_final;
pose_optimizer::optimizeGaussNewton(
Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
SVO_STOP_TIMER("pose_optimizer");
SVO_LOG4(sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
if(sfba_n_edges_final < 20)
return RESULT_FAILURE;
// structure optimization
SVO_START_TIMER("point_optimizer");
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
SVO_STOP_TIMER("point_optimizer");
// select keyframe
core_kfs_.insert(new_frame_);
setTrackingQuality(sfba_n_edges_final);
if(tracking_quality_ == TRACKING_INSUFFICIENT)
{
new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
return RESULT_FAILURE;
}
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
{
depth_filter_->addFrame(new_frame_);
return RESULT_NO_KEYFRAME;
}
new_frame_->setKeyframe();
SVO_DEBUG_STREAM("New keyframe selected.");
// new keyframe selected
for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)
if((*it)->point != NULL)
(*it)->point->addFrameRef(*it);
map_.point_candidates_.addCandidatePointToFrame(new_frame_);
// optional bundle adjustment
#ifdef USE_BUNDLE_ADJUSTMENT
if(Config::lobaNumIter() > 0)
{
SVO_START_TIMER("local_ba");
setCoreKfs(Config::coreNKfs());
size_t loba_n_erredges_init, loba_n_erredges_fin;
double loba_err_init, loba_err_fin;
ba::localBA(new_frame_.get(), &core_kfs_, &map_,
loba_n_erredges_init, loba_n_erredges_fin,
loba_err_init, loba_err_fin);
SVO_STOP_TIMER("local_ba");
SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin);
SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "
"Error {"<<loba_err_init<<", "<<loba_err_fin<<"}");
}
#endif
// init new depth-filters
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
// if limited number of keyframes, remove the one furthest apart
if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
{
FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better
map_.safeDeleteFrame(furthest_frame);
}
// add keyframe to map
map_.addKeyframe(new_frame_);
return RESULT_IS_KEYFRAME;
}
