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

State machine, and checkTimeTolerance()

parent 635a1326
No related branches found
No related tags found
1 merge request!157Kfpackmanager
This commit is part of merge request !157. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
src/processor_tracker.cpp
+ 148
− 126
View file @ 61ad4c55
......@@ -15,7 +15,11 @@ namespace wolf
{
ProcessorTracker::ProcessorTracker(const std::string& _type, const unsigned int _max_new_features, const Scalar& _time_tolerance) :
ProcessorBase(_type, _time_tolerance), origin_ptr_(nullptr), last_ptr_(nullptr), incoming_ptr_(nullptr),
ProcessorBase(_type, _time_tolerance),
processing_step_(FIRST_TIME),
origin_ptr_(nullptr),
last_ptr_(nullptr),
incoming_ptr_(nullptr),
max_new_features_(_max_new_features)
{
//
......@@ -32,8 +36,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
if (_incoming_ptr == nullptr)
{
WOLF_ERROR("Received capture is nullptr.");
return;
WOLF_ERROR("Received capture is nullptr.");
return;
}
if ( !kf_pack_buffer_.empty() )
......@@ -64,165 +68,173 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
preProcess();
computeProcessingStep();
// FIRST TIME
if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
switch (processing_step_)
{
WOLF_DEBUG( "FIRST TIME" );
case FIRST_TIME :
{
WOLF_DEBUG( "FIRST TIME" );
// advance
advance();
// advance
advance();
// advance this
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
// advance this
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr;
// keyframe creation on last
FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - last_ptr_->getTimeStamp()) <= time_tolerance_)
{
// Set KF
closest_key_frm->addCapture(last_ptr_);
closest_key_frm->setKey();
WOLF_DEBUG( "Last appended to existing F, set KF" , closest_key_frm->id() );
}
else
{
// Make KF
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME,
getProblem()->getState(last_ptr_->getTimeStamp()),
last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
WOLF_DEBUG( "Last appended to new KF" , new_frame_ptr->id() );
getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), time_tolerance_);
}
// keyframe creation on last
FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
if (closest_key_frm && checkTimeTolerance(closest_key_frm, last_ptr_))
{
// Set KF
closest_key_frm->addCapture(last_ptr_);
closest_key_frm->setKey();
WOLF_DEBUG( "Last appended to existing F", closest_key_frm->id(), " ==> set KF" , closest_key_frm->id() );
}
else
{
// Make KF
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(KEY_FRAME,
getProblem()->getState(last_ptr_->getTimeStamp()),
last_ptr_->getTimeStamp());
new_frame_ptr->addCapture(last_ptr_); // Add incoming Capture to the new Frame
WOLF_DEBUG( "Last appended to new KF" , new_frame_ptr->id() );
getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), time_tolerance_);
}
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
// Establish constraints from last
establishConstraints();
}
// Establish constraints from last
establishConstraints();
// SECOND TIME or after KEY FRAME CALLBACK
else if (origin_ptr_ == nullptr)
{
WOLF_DEBUG("SECOND TIME or after KEY FRAME CALLBACK");
break;
}
// First we track the known Features
processKnown();
// Create a new non-key Frame in the Trajectory with the incoming Capture
FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
if (closest_key_frm && abs(closest_key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp()) <= time_tolerance_)
case SECOND_TIME :
{
// Just append the Capture to the existing keyframe
closest_key_frm->addCapture(incoming_ptr_);
WOLF_DEBUG("Incoming appended to F" , closest_key_frm->id() );
}
else
{
// Create a frame to hold what will become the last Capture
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
WOLF_DEBUG("Incoming in new F" , new_frame_ptr->id() );
}
WOLF_DEBUG("SECOND TIME or after KEY FRAME CALLBACK");
// Reset the derived Tracker
reset();
// Reset this
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
// First we track the known Features between last and incoming
processKnown();
// Create a new non-key Frame in the Trajectory with the incoming Capture
FrameBasePtr closest_key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
if (closest_key_frm && checkTimeTolerance(closest_key_frm, incoming_ptr_))
{
// Just append the Capture to the existing keyframe
closest_key_frm->addCapture(incoming_ptr_);
WOLF_DEBUG("Incoming appended to F" , closest_key_frm->id() );
}
else
{
// Create a frame to hold what will become the last Capture
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
WOLF_DEBUG("Incoming in new F" , new_frame_ptr->id() );
}
}
// Reset the derived Tracker
reset();
// OTHER TIMES
else
{
WOLF_DEBUG("OTHER TIMES");
// Reset this
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
// 1. First we track the known Features and create new constraints as needed
// Establish constraints from last
establishConstraints();
processKnown();
break;
}
// 2. Then we see if we want and we are allowed to create a KeyFrame
// Three conditions to make a KF:
// - We vote for KF
// - Problem allows us to make keyframe
// - There is no existing KF very close to our Time Stamp <--- NOT SURE OF THIS
default:
{
WOLF_DEBUG("OTHER TIMES");
FrameBasePtr closest_key_frm_to_last = last_ptr_->getFramePtr(); // start with the same last's frame
if ( ! ( closest_key_frm_to_last && closest_key_frm_to_last->isKey() ) ) // last F is not KF
closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
// 1. First we track the known Features and create new constraints as needed
if (closest_key_frm_to_last && abs(closest_key_frm_to_last->getTimeStamp() - last_ptr_->getTimeStamp()) > time_tolerance_) // closest KF is not close enough
closest_key_frm_to_last = nullptr;
processKnown();
if ( !( (voteForKeyFrame() && permittedKeyFrame() ) || closest_key_frm_to_last ) )
{
// 2. Then we see if we want and we are allowed to create a KeyFrame
// Three conditions to make a KF:
// - We vote for KF
// - Problem allows us to make keyframe
// - There is no existing KF very close to our Time Stamp <--- NOT SURE OF THIS
// 2.a. We did not create a KeyFrame:
FrameBasePtr closest_key_frm_to_last = last_ptr_->getFramePtr(); // start with the same last's frame
if ( ! ( closest_key_frm_to_last && closest_key_frm_to_last->isKey() ) ) // last F is not KF
closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
// advance the derived tracker
advance();
if (closest_key_frm_to_last && !checkTimeTolerance(closest_key_frm_to_last, last_ptr_)) // closest KF is not close enough
closest_key_frm_to_last = nullptr;
// Advance this
last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
last_ptr_->remove();
incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
last_ptr_ = incoming_ptr_; // Incoming Capture takes the place of last Capture
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
if ( !( (voteForKeyFrame() && permittedKeyFrame() ) || closest_key_frm_to_last ) )
{
WOLF_DEBUG("last <-- incoming");
// 2.a. We did not create a KeyFrame:
}
else
{
// advance the derived tracker
advance();
// 2.b. We create a KF
// Advance this
last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
last_ptr_->remove();
incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
last_ptr_ = incoming_ptr_; // Incoming Capture takes the place of last Capture
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
WOLF_DEBUG("last <-- incoming");
FrameBasePtr key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
if ( abs(key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp() ) < time_tolerance_)
{
// Append incoming to existing key-frame
key_frm->addCapture(incoming_ptr_);
WOLF_DEBUG("Incoming adhered to existing KF" , key_frm->id());
}
else
{
// Create a new non-key Frame in the Trajectory with the incoming Capture
// Make a non-key-frame to hold incoming
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
WOLF_DEBUG( "Incoming adhered to new F" , key_frm->id() );
// Make the last Capture's Frame a KeyFrame
setKeyFrame(last_ptr_);
WOLF_DEBUG( "Set KEY to last F" , key_frm->id() );
}
// Establish constraints between last and origin
establishConstraints();
// Reset the derived Tracker
reset();
// Reset this
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
}
// 2.b. We create a KF
// Detect new Features, initialize Landmarks, create Constraints, ...
processNew(max_new_features_);
FrameBasePtr key_frm = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(incoming_ptr_->getTimeStamp());
if ( abs(key_frm->getTimeStamp() - incoming_ptr_->getTimeStamp() ) < time_tolerance_)
{
// Append incoming to existing key-frame
key_frm->addCapture(incoming_ptr_);
WOLF_DEBUG("Incoming adhered to existing KF" , key_frm->id());
}
else
{
// Create a new non-key Frame in the Trajectory with the incoming Capture
// Make a non-key-frame to hold incoming
FrameBasePtr new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
new_frame_ptr->addCapture(incoming_ptr_); // Add incoming Capture to the new Frame
WOLF_DEBUG( "Incoming adhered to new F" , key_frm->id() );
// Make the last Capture's Frame a KeyFrame
setKeyFrame(last_ptr_);
WOLF_DEBUG( "Set KEY to last F" , key_frm->id() );
}
// Reset the derived Tracker
reset();
// Reset this
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
incoming_ptr_ = nullptr; // This line is not really needed, but it makes things clearer.
// Establish constraints between last and origin
establishConstraints();
}
break;
}
}
postProcess();
//std::cout << "-----End of process():" << std::endl;
......@@ -284,6 +296,16 @@ void ProcessorTracker::setKeyFrame(CaptureBasePtr _capture_ptr)
}
}
void ProcessorTracker::computeProcessingStep()
{
if (origin_ptr_ == nullptr && last_ptr_ == nullptr)
processing_step_ = FIRST_TIME;
else if (origin_ptr_ == nullptr)
processing_step_ = SECOND_TIME;
}
} // namespace wolf
src/processor_tracker.h
+ 32
− 0
View file @ 61ad4c55
......@@ -67,7 +67,11 @@ struct ProcessorParamsTracker : public ProcessorParamsBase
*/
class ProcessorTracker : public ProcessorBase
{
public:
typedef enum {FIRST_TIME, SECOND_TIME} ProcessingStep ;
protected:
ProcessingStep processing_step_;
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
......@@ -91,6 +95,11 @@ class ProcessorTracker : public ProcessorBase
virtual bool keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
virtual CaptureBasePtr getOriginPtr();
virtual CaptureBasePtr getLastPtr();
virtual CaptureBasePtr getIncomingPtr();
......@@ -185,11 +194,14 @@ class ProcessorTracker : public ProcessorBase
protected:
void computeProcessingStep();
void addNewFeatureLast(FeatureBasePtr _feature_ptr);
FeatureBaseList& getNewFeaturesListIncoming();
void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
};
inline void ProcessorTracker::setMaxNewFeatures(const unsigned int& _max_new_features)
......@@ -217,6 +229,26 @@ inline FeatureBaseList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
{
return (std::fabs(_ts2 - _ts2) < time_tolerance_);
}
inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
{
return checkTimeTolerance(_cap->getTimeStamp(), _ts);
}
inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts)
{
return checkTimeTolerance(_frm->getTimeStamp(), _ts);
}
inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap)
{
return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
}
inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
{
new_features_incoming_.push_back(_feature_ptr);
......
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