diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 596521ae3a338b3779774f6bb247efc0c8643e96..f6806ab4d8d6d2dbd6056575a096a74a4af121af 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -151,7 +151,7 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
- // adding processor is motion to the processor is motion vector
+ // adding motion provider to the motion providers vector
auto motion_provider_ptr = std::dynamic_pointer_cast<MotionProvider>(shared_from_this());
if (motion_provider_ptr)
motion_provider_ptr->addToProblem(_problem, motion_provider_ptr);
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index d1d64a811f08e0608bbc1b50f1710ffa7c332d67..777dddcacbc072c53396a7e5c1f83bc7d4f8d6a5 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.cpp
@@ -78,7 +78,7 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
buffer_capture_.add(_capture->getTimeStamp(), _capture);
}
-void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe, const double& _time_tolerance)
{
/* This function has 4 scenarios:
* 1. Frame already have a capture of the sensor -> process
@@ -87,31 +87,31 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
* 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
*/
- WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+ WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _keyframe->id());
// CASE 1:
- auto cap = _frame->getCaptureOf(getSensor());
- if (cap)
+ auto capture = _keyframe->getCaptureOf(getSensor());
+ if (capture)
{
WOLF_DEBUG("CASE 1");
- process(cap);
+ process(capture);
// remove the capture (if stored)
- buffer_capture_.getContainer().erase(cap->getTimeStamp());
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
return;
}
// Search for any stored capture within time tolerance of frame
- auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+ capture = buffer_capture_.select(_keyframe->getTimeStamp(), params_->time_tolerance);
// CASE 2:
if (capture and not capture->getFrame())
{
WOLF_DEBUG("CASE 2");
- capture->link(_frame);
+ capture->link(_keyframe);
process(capture);
@@ -119,17 +119,17 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
buffer_capture_.getContainer().erase(capture->getTimeStamp());
// remove old captures (10s of old captures are kept in case frames arrives unordered)
- buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+ buffer_capture_.removeUpTo(_keyframe->getTimeStamp() - 10);
return;
}
// CASE 3:
- if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ if (buffer_capture_.selectLastAfter(_keyframe->getTimeStamp(), params_->time_tolerance) == nullptr)
{
WOLF_DEBUG("CASE 3");
// store frame
- buffer_frame_.add(_frame->getTimeStamp(), _frame);
+ buffer_frame_.add(_keyframe->getTimeStamp(), _keyframe);
return;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 0dc8be3a2b98acfde9d9001d264ec8a1b8670806..9445a30ba5feaeb0a4c930c805ae9b3c00a2627a 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -260,19 +260,19 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
*/
// extract KF elements
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k
+ auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback); // k
if (!capture_existing)
{
- WOLF_WARN("A KF before first motion capture (TS = ", ts_from_callback, "). ProcessorMotion cannot do anything.");
+ WOLF_WARN("A KF before first motion capture (TS = ", timestamp_from_callback, "). ProcessorMotion cannot do anything.");
break;
}
// update KF state (adding missing StateBlocks)
- auto proc_state = getState(ts_from_callback);
+ auto proc_state = getState(timestamp_from_callback);
for (auto pair_ckey_vec : proc_state)
if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
@@ -291,7 +291,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
getSensor(),
- ts_from_callback,
+ timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
@@ -300,7 +300,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(capture_existing, timestamp_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_new = emplaceFeature(capture_for_keyframe_callback);
@@ -315,20 +315,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
assert(capture_existing->getFeatureList().empty());// there was only one feature!
+
auto new_feature_existing = emplaceFeature(capture_existing);
- emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
-// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
-//
-// // Modify existing feature --------
-// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
-// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
-//
-// // Modify existing factor --------
-// // Instead of modifying, we remove one ctr, and create a new one.
-// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
-// auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
-// fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
}
break;
@@ -370,15 +360,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
*/
// extract KF elements
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// update KF state (adding missing StateBlocks)
- auto proc_state = getState(ts_from_callback);
- for (auto pair_ckey_vec : proc_state)
- if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
- keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
- FactoryStateBlock::create(string(1, pair_ckey_vec.first),
- pair_ckey_vec.second,
+ auto proc_state = this->getState(timestamp_from_callback);
+ for (auto pair_key_vector : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_key_vector.first))
+ keyframe_from_callback->addStateBlock(pair_key_vector.first,
+ FactoryStateBlock::create(string(1, pair_key_vector.first),
+ pair_key_vector.second,
false),
getProblem());
keyframe_from_callback->setState(proc_state);
@@ -394,7 +384,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
getSensor(),
- ts_from_callback,
+ timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
@@ -403,7 +393,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(capture_existing, timestamp_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 8492ed03147f5cb0302578da2293096d3b62441f..c41ee4f6c963ce45ba3a427a3c2e27dc818b185b 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -77,13 +77,13 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
{
case FIRST_TIME_WITH_KEYFRAME :
{
- FrameBasePtr keyframe = buffer_frame_.select( incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance);
- buffer_frame_.removeUpTo( keyframe->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance);
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe->id() , " callback unpacked with ts= " , keyframe->getTimeStamp() );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
// Append incoming to KF
- incoming_ptr_->link(keyframe);
+ 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.
@@ -92,9 +92,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
@@ -131,18 +131,20 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
case SECOND_TIME_WITH_KEYFRAME :
{
// No-break case only for debug. Next case will be executed too.
- FrameBasePtr keyframe = buffer_frame_.select( incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance);
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe->id() , " callback unpacked with ts= " , keyframe->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(),
+ params_tracker_->time_tolerance);
+
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
}
// Fall through
case SECOND_TIME_WITHOUT_KEYFRAME :
{
WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(frm);
+ FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
+ incoming_ptr_->link(keyframe);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
@@ -156,30 +158,31 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
+ last_frame_ptr_ = keyframe;
incoming_ptr_ = nullptr;
break;
}
case RUNNING_WITH_KEYFRAME :
{
- FrameBasePtr keyframe = buffer_frame_.select( last_ptr_->getTimeStamp() , params_tracker_->time_tolerance);
- buffer_frame_.removeUpTo( keyframe->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp() ,
+ params_tracker_->time_tolerance);
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_KEYFRAME: KF" , keyframe->id() , " callback unpacked with ts= " , keyframe->getTimeStamp() );
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
processKnown();
// Capture last_ is added to the new keyframe
FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_ptr_->move(keyframe);
+ last_ptr_->move(keyframe_from_callback);
last_old_frame->remove();
- // Create new frame
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ // Create new frame for incoming
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- incoming_ptr_->link(frm);
+ incoming_ptr_->link(frame);
// Detect new Features, initialize Landmarks, create Factors, ...
processNew(params_tracker_->max_new_features);
@@ -189,10 +192,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frame;
+ incoming_ptr_ = nullptr;
break;
}
@@ -228,14 +231,14 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
// make F; append incoming to new F
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
- incoming_ptr_->link(frm);
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
- incoming_ptr_ = nullptr;
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_ ->link(frame);
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frame;
+ incoming_ptr_ = nullptr;
}
else
@@ -246,15 +249,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
- incoming_ptr_->link(frm);
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_->link(frame);
last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
// Update pointers
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
+ last_frame_ptr_ = frame;
incoming_ptr_ = nullptr;
}
break;