Skip to content
Snippets Groups Projects
Commit a9054f42 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

WIP new process() using new state machine

parent 79f47607
No related branches found
No related tags found
1 merge request!157Kfpackmanager
Pipeline #
Hide whitespace changes
Inline Side-by-side
src/processor_tracker.cpp
+ 182
− 88
View file @ a9054f42
...@@ -42,33 +42,136 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) ...@@ -42,33 +42,136 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
incoming_ptr_ = _incoming_ptr; incoming_ptr_ = _incoming_ptr;
if ( !kf_pack_buffer_.empty() ) preProcess(); // Derived class operations
computeProcessingStep();
switch(processing_step_)
{ {
KFPackPtr pack; case FIRST_TIME_WITH_PACK :
{
// (1)
// pack.KF.addCapture(incoming)
// makeF; F.addCapture(incoming)
// o <- i
// l <- i
KFPackPtr pack = selectPack( incoming_ptr_);
kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
// Select using last_ptr // Append incoming to KF
if (last_ptr_ != nullptr) pack->key_frame->addCapture(incoming_ptr_);
// TODO process info
processNew(max_new_features_); // TODO not sure. Check code inside.
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
// KF_O, F_L, -
break;
}
case FIRST_TIME_WITHOUT_PACK :
{ {
pack = kf_pack_buffer_.selectPack( last_ptr_->getTimeStamp(), time_tolerance_ ); // (4) WARNING No pack on first incoming!
if (pack!=nullptr) // makeKF; KF.addCapture(incoming)
{ // makeF; F.addCapture(incoming)
keyFrameCallback(pack->key_frame,pack->time_tolerance); // o <- i
kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() ); // l <- i
}
FrameBasePtr kfrm = getProblem()->emplaceFrame(KEY_FRAME, incoming_ptr_->getTimeStamp());
kfrm->addCapture(incoming_ptr_);
FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
// TODO process info
// Issue KF callback with new KF
getProblem()->keyFrameCallback(kfrm, shared_from_this(), time_tolerance_);
origin_ptr_ = incoming_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
break;
} }
case SECOND_TIME_WITHOUT_PACK :
{
// (2)
// origin.F.unlink(last)
// makeF; F.addCapture(incoming)
// o <- l
// l <- i
origin_ptr_->getFramePtr()->unlinkCapture(last_ptr_);
FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
// TODO process info
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
// Select using incoming_ptr break;
pack = kf_pack_buffer_.selectPack( incoming_ptr_->getTimeStamp(), time_tolerance_ ); }
if (pack!=nullptr) case RUNNING_WITH_PACK :
{ {
keyFrameCallback(pack->key_frame,pack->time_tolerance); // (1)
kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() ); // pack.KF.addCapture(last)
// makeF; F.addCapture(incoming)
// o <- l
// l <- i
KFPackPtr pack = selectPack( last_ptr_ );
kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
WOLF_DEBUG( "PT: KF" , pack->key_frame->id() , " callback received at ts= " , pack->key_frame->getTimeStamp().get() );
// Capture last_ is added to the new keyframe
FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
last_old_frame->unlinkCapture(last_ptr_);
last_old_frame->remove();
pack->key_frame->addCapture(last_ptr_);
WOLF_DEBUG( " --> appended last capture" );
// Create new frame
FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
// Establish constraints between last and origin
// TODO: revise this fcn: code inside, and placement in here
establishConstraints();
// reset pointers
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
break;
} }
} case RUNNING_WITHOUT_PACK :
{
// (3)
// o <- o // verbose
// l <- i
preProcess(); // TODO process info
computeProcessingStep(); last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
break;
}
default :
break;
}
switch (processing_step_) switch (processing_step_)
{ {
...@@ -245,40 +348,41 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) ...@@ -245,40 +348,41 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other) bool ProcessorTracker::keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other)
{ {
WOLF_DEBUG( "PT: KF" , _keyframe_ptr->id() , " callback received at ts= " , _keyframe_ptr->getTimeStamp().get() ); WOLF_TRACE("This callback doing nothing!!! ");
// WOLF_DEBUG( "PT: KF" , _keyframe_ptr->id() , " callback received at ts= " , _keyframe_ptr->getTimeStamp().get() );
assert((last_ptr_ == nullptr || last_ptr_->getFramePtr() != nullptr) && "ProcessorTracker::keyFrameCallback: last_ptr_ must have a frame always"); //
// assert((last_ptr_ == nullptr || last_ptr_->getFramePtr() != nullptr) && "ProcessorTracker::keyFrameCallback: last_ptr_ must have a frame always");
Scalar time_tol = std::min(time_tolerance_, _time_tol_other); //
// Scalar time_tol = std::min(time_tolerance_, _time_tol_other);
//
// Nothing to do if: //
// - there is no last // // Nothing to do if:
// - last frame is already a key frame // // - there is no last
// - last frame is too far in time from keyframe // // - last frame is already a key frame
if (last_ptr_ == nullptr || last_ptr_->getFramePtr()->isKey() || std::abs(last_ptr_->getTimeStamp() - _keyframe_ptr->getTimeStamp()) > time_tol) // // - last frame is too far in time from keyframe
{ // if (last_ptr_ == nullptr || last_ptr_->getFramePtr()->isKey() || std::abs(last_ptr_->getTimeStamp() - _keyframe_ptr->getTimeStamp()) > time_tol)
WOLF_DEBUG( " --> nothing done" ); // {
return false; // WOLF_DEBUG( " --> nothing done" );
} // return false;
// }
WOLF_DEBUG( " --> appended last capture" ); //
//std::cout << "ProcessorTracker::keyFrameCallback in sensor " << getSensorPtr()->id() << std::endl; // WOLF_DEBUG( " --> appended last capture" );
// //std::cout << "ProcessorTracker::keyFrameCallback in sensor " << getSensorPtr()->id() << std::endl;
// Capture last_ is added to the new keyframe //
FrameBasePtr last_old_frame = last_ptr_->getFramePtr(); // // Capture last_ is added to the new keyframe
last_old_frame->unlinkCapture(last_ptr_); // FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
last_old_frame->remove(); // last_old_frame->unlinkCapture(last_ptr_);
_keyframe_ptr->addCapture(last_ptr_); // last_old_frame->remove();
// _keyframe_ptr->addCapture(last_ptr_);
// Detect new Features, initialize Landmarks, create Constraints, ... //
processNew(max_new_features_); // // Detect new Features, initialize Landmarks, create Constraints, ...
// processNew(max_new_features_);
// Establish constraints between last and origin //
establishConstraints(); // // Establish constraints between last and origin
// establishConstraints();
// Set ready to go to 2nd case in process() //
origin_ptr_ = nullptr; // // Set ready to go to 2nd case in process()
// origin_ptr_ = nullptr;
return true; return true;
...@@ -309,6 +413,7 @@ KFPackPtr ProcessorTracker::selectPack(const CaptureBasePtr & _cap) ...@@ -309,6 +413,7 @@ KFPackPtr ProcessorTracker::selectPack(const CaptureBasePtr & _cap)
void ProcessorTracker::computeProcessingStep() void ProcessorTracker::computeProcessingStep()
{ {
enum {FIRST_TIME, SECOND_TIME, RUNNING} step; enum {FIRST_TIME, SECOND_TIME, RUNNING} step;
if (origin_ptr_ == nullptr && last_ptr_ == nullptr) if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
step = FIRST_TIME; step = FIRST_TIME;
...@@ -317,33 +422,29 @@ void ProcessorTracker::computeProcessingStep() ...@@ -317,33 +422,29 @@ void ProcessorTracker::computeProcessingStep()
else else
step = RUNNING; step = RUNNING;
////////////////////
switch (step) switch (step)
{ {
case FIRST_TIME : case FIRST_TIME :
{
if (selectPack(incoming_ptr_)) if (selectPack(incoming_ptr_))
{
processing_step_ = FIRST_TIME_WITH_PACK; processing_step_ = FIRST_TIME_WITH_PACK;
// (1)
// pack.KF.addCapture(incoming)
// makeF; F.addCapture(incoming)
// o <- i
// l <- i
}
else // ! last && ! pack(incoming) else // ! last && ! pack(incoming)
{ {
WOLF_WARN("\n||*||");
WOLF_INFO("\n ... It seems you missed something!");
WOLF_INFO("\nReceived first Capture without KF pack to associate to");
WOLF_INFO("Creating a KF for the Capture. But...")
WOLF_INFO("Check the following:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed");
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
processing_step_ = FIRST_TIME_WITHOUT_PACK; processing_step_ = FIRST_TIME_WITHOUT_PACK;
// (4) WARNING No pack on first incoming!
// makeKF; KF.addCapture(incoming)
// makeF; F.addCapture(incoming)
// o <- i
// l <- i
} }
}
break; break;
case SECOND_TIME : case SECOND_TIME :
{
if (selectPack(last_ptr_)) if (selectPack(last_ptr_))
{ {
WOLF_WARN("\n||*||"); WOLF_WARN("\n||*||");
...@@ -356,37 +457,30 @@ void ProcessorTracker::computeProcessingStep() ...@@ -356,37 +457,30 @@ void ProcessorTracker::computeProcessingStep()
WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances). Check time tolerances!"); WOLF_ERROR("Pack's KF and origin's KF have matching time stamps (i.e. below time tolerances). Check time tolerances!");
} }
else else
{
processing_step_ = SECOND_TIME_WITHOUT_PACK; processing_step_ = SECOND_TIME_WITHOUT_PACK;
// (2)
// last.F.setKey()
// makeF; F.addCapture(incoming)
// o <- l
// l <- i
}
break; break;
}
case RUNNING : case RUNNING :
default : default :
{
if (selectPack(last_ptr_)) if (selectPack(last_ptr_))
{ {
if (last_ptr_->getFramePtr()->isKey())
{
WOLF_WARN("\n||*||");
WOLF_INFO("\n ... It seems you missed something!");
WOLF_INFO("\nPack's KF and last's KF have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
WOLF_INFO(" - You issued a problem->setPrior() after all processors are installed");
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances). Check time tolerances!");
}
processing_step_ = RUNNING_WITH_PACK; processing_step_ = RUNNING_WITH_PACK;
// (1)
// pack.KF.addCapture(last)
// makeF; F.addCapture(incoming)
// o <- l
// l <- i
} }
else else
{
processing_step_ = RUNNING_WITHOUT_PACK; processing_step_ = RUNNING_WITHOUT_PACK;
// (3)
// o <- o // verbose
// l <- i
}
break; break;
}
} }
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment