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

Cleanup code... 

still some DEBUG messages remaining that will be cleaned later
parent d392a315
No related branches found
No related tags found
3 merge requests!476spdlog version upgrade,!472Merge ProcessorLandmarkExternal,!468Resolve "multiple processors per sensor"
This commit is part of merge request !468. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
src/processor/processor_tracker.cpp
+ 27
− 61
View file @ 76a2737d
......@@ -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();
......
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