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::addSecondFrame(FramePtr frame_cur)
{
trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");
if(disparities_.size() < Config::initMinTracked())
return FAILURE;
double disparity = vk::getMedian(disparities_);
SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
if(disparity < Config::initMinDisparity())
return NO_KEYFRAME;
computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
inliers_, xyz_in_cur_, T_cur_from_ref_);
SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
if(inliers_.size() < Config::initMinInliers())
{
SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
return FAILURE;
}
// Rescale the map such that the mean scene depth is equal to the specified scale
vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
frame_cur->T_f_w_.translation() =
-frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
// For each inlier create 3D point and add feature in both frames
SE3 T_world_cur = frame_cur->T_f_w_.inverse();
for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
{
Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
{
Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
Point* new_point = new Point(pos);
Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
frame_cur->addFeature(ftr_cur);
new_point->addFrameRef(ftr_cur);
Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
frame_ref_->addFeature(ftr_ref);
new_point->addFrameRef(ftr_ref);
}
}
return SUCCESS;
}
void DepthFilter::stopThread()
{
SVO_INFO_STREAM("DepthFilter stop thread invoked.");
if(thread_ != NULL)
{
SVO_INFO_STREAM("DepthFilter interrupt and join thread... ");
seeds_updating_halt_ = true;
thread_->interrupt();
thread_->join();
thread_ = NULL;
}
}
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
{
initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
if(res == initialization::FAILURE)
return RESULT_FAILURE;
else if(res == initialization::NO_KEYFRAME)
return RESULT_NO_KEYFRAME;
// two-frame bundle adjustment
#ifdef USE_BUNDLE_ADJUSTMENT
ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
#endif
new_frame_->setKeyframe();
double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
// add frame to map
map_.addKeyframe(new_frame_);
stage_ = STAGE_DEFAULT_FRAME;
klt_homography_init_.reset();
SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
return RESULT_IS_KEYFRAME;
}
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
{
new_frame_->T_f_w_ = SE3d(Matrix3d::Identity(), Vector3d::Zero());
if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
return RESULT_NO_KEYFRAME;
new_frame_->setKeyframe();
map_.addKeyframe(new_frame_);
stage_ = STAGE_SECOND_FRAME;
SVO_INFO_STREAM("Init: Selected first frame.");
return RESULT_IS_KEYFRAME;
}
void DepthFilter::reset()
{
seeds_updating_halt_ = true;
{
lock_t lock(seeds_mut_);
seeds_.clear();
}
lock_t lock();
while(!frame_queue_.empty())
frame_queue_.pop();
seeds_updating_halt_ = false;
if(options_.verbose)
SVO_INFO_STREAM("DepthFilter: RESET.");
}
void DepthFilter::initializeSeeds(FramePtr frame)
{
list<PointFeat*> new_pt_features;
list<LineFeat*> new_seg_features;
if(Config::hasPoints())
{
/* detect new point features in point-unpopulated cells of the grid */
// populate the occupancy grid of the detector with current features
pt_feature_detector_->setExistingFeatures(frame->pt_fts_);
// detect features to fill the free cells in the image
pt_feature_detector_->detect(frame.get(), frame->img_pyr_,
Config::triangMinCornerScore(), new_pt_features);
}
if(Config::hasLines())
{
/* detect new segment features in line-unpopulated cells of the grid */
// populate the occupancy grid of the detector with current features
seg_feature_detector_->setExistingFeatures(frame->seg_fts_);
// detect features
seg_feature_detector_->detect(frame.get(), frame->img_pyr_,
Config::lsdMinLength(), new_seg_features);
}
// lock the parallel updating thread?
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
// increase by one the keyframe counter (to account for this new one)
++PointSeed::batch_counter;
// initialize a point seed for every new point feature
std::for_each(new_pt_features.begin(), new_pt_features.end(), [&](PointFeat* ftr){
pt_seeds_.push_back(PointSeed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
});
// initialize a segment seed for every new segment feature
std::for_each(new_seg_features.begin(), new_seg_features.end(), [&](LineFeat* ftr){
seg_seeds_.push_back(LineSeed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
});
if(options_.verbose)
SVO_INFO_STREAM("DepthFilter: Initialized "<<new_pt_features.size()<<" new point seeds and "
<<new_seg_features.size()<<" line segment seeds.");
seeds_updating_halt_ = false;
}
void DepthFilter::initializeSeeds(FramePtr frame)
{
Features new_features;
feature_detector_->setExistingFeatures(frame->fts_);
feature_detector_->detect(frame.get(), frame->img_pyr_,
Config::triangMinCornerScore(), new_features);
// initialize a seed for every new feature
seeds_updating_halt_ = true;
lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
++Seed::batch_counter;
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 "<<new_features.size()<<" new seeds");
seeds_updating_halt_ = false;
}
DepthFilter::~DepthFilter()
{
stopThread();
SVO_INFO_STREAM("DepthFilter destructed.");
}
InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur, Matrix3d orient, Vector3d pos)
{
trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");
if(disparities_.size() < Config::initMinTracked())
return FAILURE;
double disparity = vk::getMedian(disparities_);
SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
if(disparity < Config::initMinDisparity())
return NO_KEYFRAME;
computeHomography(
f_ref_, f_cur_,
frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
inliers_, xyz_in_cur_, T_cur_from_ref_);
SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
if(inliers_.size() < Config::initMinInliers())
{
SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
return FAILURE;
}
// Transformation in real world
T_cur_frame_real_scale = SE3(orient, pos);
Vector3d trans = T_cur_frame_real_scale.translation() - T_first_frame_real_scale.translation();
double length_real = sqrt(pow(trans[0],2) + pow(trans[1],2) + pow(trans[2],2));
SVO_INFO_STREAM("Real world transform x: " << trans[0] << " y:" << trans[1] << " z:" << trans[2] << " length:" << length_real);
double x = T_cur_from_ref_.translation()[0];
double y = T_cur_from_ref_.translation()[1];
double z = T_cur_from_ref_.translation()[2];
double length_svo = sqrt(pow(x,2) + pow(y,2) + pow(z,2));
SVO_INFO_STREAM("SVO transform x: " << x << " y:" << y << " z:" << z << " length:" << length_svo);
#ifdef USE_ASE_IMU
// Rescale the map such that the real length of the movement matches with the svo movement length
double scale =length_real / length_svo;
#else
// Rescale the map such that the mean scene depth is equal to the specified scale
vector<double> depth_vec;
for(size_t i=0; i<xyz_in_cur_.size(); ++i)
depth_vec.push_back((xyz_in_cur_[i]).z());
double scene_depth_median = vk::getMedian(depth_vec);
double scale = Config::mapScale()/scene_depth_median;
#endif
frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
frame_cur->T_f_w_.translation() =
-frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
//frame_cur->T_f_w_ = T_cur_frame_real_scale;
//frame_ref_->T_f_w_ = T_first_frame_real_scale;
// // Rescale the map such that the mean scene depth is equal to the specified scale
// vector<double> depth_vec;
// for(size_t i=0; i<xyz_in_cur_.size(); ++i)
// depth_vec.push_back((xyz_in_cur_[i]).z());
// double scene_depth_median = vk::getMedian(depth_vec);
// double scale = Config::mapScale()/scene_depth_median;
// frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
// frame_cur->T_f_w_.translation() =
// -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
// For each inlier create 3D point and add feature in both frames
SE3 T_world_cur = frame_cur->T_f_w_.inverse();
for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
{
Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
{
Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
Point* new_point = new Point(pos);
Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
frame_cur->addFeature(ftr_cur);
new_point->addFrameRef(ftr_cur);
Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
frame_ref_->addFeature(ftr_ref);
new_point->addFrameRef(ftr_ref);
}
}
return SUCCESS;
}