diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index d00fa30bf291483edf696086902cab57c913963c..48c7cc51a42ca9f9cbf8f7d586f99d9851666b6a 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -95,25 +95,12 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
}
else
{
- WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed previously!")
- // Append incoming to KF
+ WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed by any other processor!")
+
+ // Join KF
incoming_ptr_->link(keyframe_from_callback);
}
-
- // // Check if incoming has already a Frame
- // auto frame_incoming = incoming_ptr_->getFrame();
- // if (frame_incoming != nullptr and keyframe_from_callback == frame_incoming)
- // {
- // WOLF_DEBUG("PT ", getName(), " Incoming capture had been processed previously!")
- // }
- // else
- // {
- // WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed previously!")
- // // Append incoming to KF
- // incoming_ptr_->link(keyframe_from_callback);
- // }
- // Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
// TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
if (not getProblem()->getMap()->getLandmarkList().empty())
@@ -133,36 +120,19 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
-
- FrameBasePtr keyframe;
-
// Check if incoming has already a Frame
auto frame_incoming = incoming_ptr_->getFrame();
- if (frame_incoming != nullptr)
- {
- WOLF_DEBUG("PT ", getName(), "Incoming capture had been processed previously!")
- assert(frame_incoming->getCaptureOf(getSensor()) == incoming_ptr_ && " Incoming has a frame, but not from a capture of this sensor");
- keyframe = frame_incoming;
+ assert(frame_incoming == nullptr and " Incoming capture has been processed and linked by another processor, but no keyframe callback was received!");
- // We do not link incoming to keyframe since it is already linked
+ WOLF_DEBUG("PT ", getName(), " Incoming capture has not been processed by another processor!")
- // We do not issue KF callback since the KF was already there
- }
- else
- {
- WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed previously!")
-
- // make a new KF at incoming
- keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
- incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- // Append incoming to KF
- incoming_ptr_->link(keyframe);
-
- // Issue KF callback with new KF
- getProblem()->keyFrameCallback(keyframe, shared_from_this());
- }
+ // make a new KF at incoming
+ FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
+ // Append incoming to KF
+ incoming_ptr_->link(keyframe);
// Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
@@ -170,6 +140,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
if (not getProblem()->getMap()->getLandmarkList().empty())
processKnown();
+ // Issue KF callback with new KF
+ getProblem()->keyFrameCallback(keyframe, shared_from_this());
+
// Reset this
resetDerived();
// Update pointers
@@ -204,13 +177,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Make a NON KEY Frame to hold incoming capture
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
- getProblem()->getState());
- // incoming_ptr_->link(frame);
+ getProblem()->getState(incoming_ptr_->getTimeStamp()));
- // Process info
- // If we have known landmarks or features. Process them.
- if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
- processKnown();
+ // Process known information
+ processKnown();
// Both Trackers: We have a last_ Capture with not enough features, so populate it.
processNew(params_tracker_->max_new_features);
@@ -239,21 +209,21 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
processKnown();
// chack if the received KF has a capture of this sensor, and if it matches with last_ptr
- // check if the callback keyframe has a capture of this sensor
- auto capture_from_callback = keyframe_from_callback->getCaptureOf(this->getSensor());
-
- if (last_ptr_ == capture_from_callback)
+ if (last_ptr_ == keyframe_from_callback->getCaptureOf(this->getSensor()))
{
+ WOLF_DEBUG("PT ", getName(), " Last capture has been processed previously by another processor!")
+
// If captures match, then frames must match too
- assert(last_ptr_->getFrame() != nullptr
- and last_ptr_->getFrame() == keyframe_from_callback
+ assert( last_ptr_->getFrame() == keyframe_from_callback
and "The keyframe has a Capture from this sensor, but this capture is not last!");
- WOLF_DEBUG("PT ", getName(), " Last capture has been processed previously by another processor!")
+
+ // No need to join KF since it is the same capture, and it was already joined by the other processor
}
else
{
WOLF_DEBUG("PT ", getName(), " Last capture had not been processed previously!")
- // Append incoming to KF
+
+ // join KF
last_ptr_->link(keyframe_from_callback);
}
@@ -261,7 +231,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- // incoming_ptr_->link(frame);
// Detect new Features, initialize Landmarks, ...
processNew(params_tracker_->max_new_features);
@@ -299,7 +268,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Establish factors
establishFactors();
- // Call the new keyframe callback in order to let the other processors to establish their factors
+ // Call the new keyframe callback in order to let the other processors to join
getProblem()->keyFrameCallback(last_frame_ptr_, shared_from_this());
@@ -307,7 +276,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState(incoming_ptr_->getTimeStamp()));
- // incoming_ptr_ ->link(frame);
// Reset this
resetDerived();
@@ -326,8 +294,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState(incoming_ptr_->getTimeStamp()));
- // incoming_ptr_->link(frame);
- // last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
// Advance this
advanceDerived();