refactoring
This commit is contained in:
@@ -35,8 +35,7 @@ namespace svo
|
|||||||
vk::PerformanceMonitor* g_permon = NULL;
|
vk::PerformanceMonitor* g_permon = NULL;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
FrameHandlerBase::
|
FrameHandlerBase::FrameHandlerBase() :
|
||||||
FrameHandlerBase() :
|
|
||||||
stage_(STAGE_PAUSED),
|
stage_(STAGE_PAUSED),
|
||||||
set_reset_(false),
|
set_reset_(false),
|
||||||
set_start_(false),
|
set_start_(false),
|
||||||
@@ -85,16 +84,14 @@ FrameHandlerBase() :
|
|||||||
SVO_INFO_STREAM("NanoSlam initialized");
|
SVO_INFO_STREAM("NanoSlam initialized");
|
||||||
}
|
}
|
||||||
|
|
||||||
FrameHandlerBase::
|
FrameHandlerBase::~FrameHandlerBase()
|
||||||
~FrameHandlerBase()
|
|
||||||
{
|
{
|
||||||
SVO_INFO_STREAM("SVO destructor invoked.");
|
SVO_INFO_STREAM("SVO destructor invoked.");
|
||||||
delete depth_filter_;
|
delete depth_filter_;
|
||||||
SVO_INFO_STREAM("SVO destructed.");
|
SVO_INFO_STREAM("SVO destructed.");
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FrameHandlerBase::
|
bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
|
||||||
startFrameProcessingCommon(const double timestamp)
|
|
||||||
{
|
{
|
||||||
if(set_start_)
|
if(set_start_)
|
||||||
{
|
{
|
||||||
@@ -115,8 +112,7 @@ startFrameProcessingCommon(const double timestamp)
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
int FrameHandlerBase::
|
int FrameHandlerBase::finishFrameProcessingCommon(
|
||||||
finishFrameProcessingCommon(
|
|
||||||
const size_t update_id,
|
const size_t update_id,
|
||||||
const UpdateResult dropout,
|
const UpdateResult dropout,
|
||||||
const size_t num_observations)
|
const size_t num_observations)
|
||||||
@@ -146,8 +142,7 @@ finishFrameProcessingCommon(
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void FrameHandlerBase::
|
void FrameHandlerBase::resetCommon()
|
||||||
resetCommon()
|
|
||||||
{
|
{
|
||||||
map_.reset();
|
map_.reset();
|
||||||
stage_ = STAGE_PAUSED;
|
stage_ = STAGE_PAUSED;
|
||||||
@@ -159,8 +154,7 @@ resetCommon()
|
|||||||
SVO_INFO_STREAM("RESET");
|
SVO_INFO_STREAM("RESET");
|
||||||
}
|
}
|
||||||
|
|
||||||
void FrameHandlerBase::
|
void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
|
||||||
setTrackingQuality(const size_t num_observations)
|
|
||||||
{
|
{
|
||||||
tracking_quality_ = TRACKING_GOOD;
|
tracking_quality_ = TRACKING_GOOD;
|
||||||
if(num_observations < Config::qualityMinFts())
|
if(num_observations < Config::qualityMinFts())
|
||||||
@@ -186,8 +180,10 @@ bool ptLastOptimComparator(Point* lhs, Point* rhs)
|
|||||||
return (lhs->last_structure_optim_ < rhs->last_structure_optim_);
|
return (lhs->last_structure_optim_ < rhs->last_structure_optim_);
|
||||||
}
|
}
|
||||||
|
|
||||||
void FrameHandlerBase::
|
void FrameHandlerBase::optimizeStructure(
|
||||||
optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter)
|
FramePtr frame,
|
||||||
|
size_t max_n_pts,
|
||||||
|
int max_iter)
|
||||||
{
|
{
|
||||||
deque<Point*> pts;
|
deque<Point*> pts;
|
||||||
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
|
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
|
||||||
|
|||||||
@@ -30,19 +30,16 @@
|
|||||||
|
|
||||||
namespace svo {
|
namespace svo {
|
||||||
|
|
||||||
FrameHandlerMono::
|
FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam) :
|
||||||
FrameHandlerMono(vk::AbstractCamera* cam) :
|
|
||||||
FrameHandlerBase(),
|
FrameHandlerBase(),
|
||||||
cam_(cam),
|
cam_(cam),
|
||||||
reprojector_(cam_, map_)
|
reprojector_(cam_, map_)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
FrameHandlerMono::
|
FrameHandlerMono::~FrameHandlerMono()
|
||||||
~FrameHandlerMono()
|
|
||||||
{}
|
{}
|
||||||
|
|
||||||
void FrameHandlerMono::
|
void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
|
||||||
addImage(const cv::Mat& img, const double timestamp)
|
|
||||||
{
|
{
|
||||||
if(!startFrameProcessingCommon(timestamp))
|
if(!startFrameProcessingCommon(timestamp))
|
||||||
return;
|
return;
|
||||||
@@ -73,8 +70,7 @@ addImage(const cv::Mat& img, const double timestamp)
|
|||||||
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
|
finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
|
||||||
}
|
}
|
||||||
|
|
||||||
void FrameHandlerMono::
|
void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
|
||||||
setFirstFrame(const FramePtr& first_frame)
|
|
||||||
{
|
{
|
||||||
resetAll();
|
resetAll();
|
||||||
last_frame_ = first_frame;
|
last_frame_ = first_frame;
|
||||||
@@ -83,8 +79,7 @@ setFirstFrame(const FramePtr& first_frame)
|
|||||||
stage_ = STAGE_DEFAULT_FRAME;
|
stage_ = STAGE_DEFAULT_FRAME;
|
||||||
}
|
}
|
||||||
|
|
||||||
FrameHandlerMono::UpdateResult FrameHandlerMono::
|
FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
|
||||||
processFirstFrame()
|
|
||||||
{
|
{
|
||||||
new_frame_->T_f_w_ = T_f_w_init_;
|
new_frame_->T_f_w_ = T_f_w_init_;
|
||||||
if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
|
if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
|
||||||
@@ -96,8 +91,7 @@ processFirstFrame()
|
|||||||
return RESULT_IS_KEYFRAME;
|
return RESULT_IS_KEYFRAME;
|
||||||
}
|
}
|
||||||
|
|
||||||
FrameHandlerMono::UpdateResult FrameHandlerMono::
|
FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
|
||||||
processSecondFrame()
|
|
||||||
{
|
{
|
||||||
initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
|
initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
|
||||||
if(res == initialization::FAILURE)
|
if(res == initialization::FAILURE)
|
||||||
@@ -123,8 +117,7 @@ processSecondFrame()
|
|||||||
return RESULT_IS_KEYFRAME;
|
return RESULT_IS_KEYFRAME;
|
||||||
}
|
}
|
||||||
|
|
||||||
FrameHandlerMono::UpdateResult FrameHandlerMono::
|
FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
|
||||||
processFrame()
|
|
||||||
{
|
{
|
||||||
// Set initial pose TODO use prior
|
// Set initial pose TODO use prior
|
||||||
new_frame_->T_f_w_ = last_frame_->T_f_w_;
|
new_frame_->T_f_w_ = last_frame_->T_f_w_;
|
||||||
@@ -225,8 +218,7 @@ processFrame()
|
|||||||
return RESULT_IS_KEYFRAME;
|
return RESULT_IS_KEYFRAME;
|
||||||
}
|
}
|
||||||
|
|
||||||
void FrameHandlerMono::
|
void FrameHandlerMono::resetAll()
|
||||||
resetAll()
|
|
||||||
{
|
{
|
||||||
resetCommon();
|
resetCommon();
|
||||||
last_frame_.reset();
|
last_frame_.reset();
|
||||||
@@ -235,8 +227,7 @@ resetAll()
|
|||||||
overlap_kfs_.clear();
|
overlap_kfs_.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool FrameHandlerMono::
|
bool FrameHandlerMono::needNewKf(double scene_depth_mean)
|
||||||
needNewKf(double scene_depth_mean)
|
|
||||||
{
|
{
|
||||||
for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
|
for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
|
||||||
{
|
{
|
||||||
@@ -249,8 +240,7 @@ needNewKf(double scene_depth_mean)
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void FrameHandlerMono::
|
void FrameHandlerMono::setCoreKfs(size_t n_closest)
|
||||||
setCoreKfs(size_t n_closest)
|
|
||||||
{
|
{
|
||||||
size_t n = min(n_closest, overlap_kfs_.size()-1);
|
size_t n = min(n_closest, overlap_kfs_.size()-1);
|
||||||
std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
|
std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
|
||||||
|
|||||||
Reference in New Issue
Block a user