diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 203dcd24e54c80675058b75f981a8dd153f836cf..78ecbe638f8efece33c4ae7bb9ad02581b552c53 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -64,11 +64,13 @@ CaptureMotion::~CaptureMotion()
bool CaptureMotion::containsTimeStamp(const TimeStamp& _ts, double _time_tolerance) const
{
assert(_ts.ok() and this->time_stamp_.ok());
+ assert(this->time_stamp_ == this->getBuffer().back().ts_ and
+ "CaptureMotion::containsTimeStamp: The time stamp of the capture is not equal to the last motion in the buffer");
// the same capture is within tolerance
if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance) return true;
- // buffer is empty, and the capture is not within tolerance
+ // buffer is empty (and the capture is not within tolerance)
if (this->getBuffer().empty()) return false;
// buffer encloses timestamp, if ts is:
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 17d1e7ee15fcd07c2341fbdba8ab72386501a205..7dcd5b079f8bc92be8a1f8a9d2ab1516b3467612 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -772,12 +772,6 @@ void Problem::addFrameTypes(const TypeComposite& _types)
frame_types_.emplace(pair.first, pair.second);
else
{
- WOLF_DEBUG_COND(frame_types_.at(pair.first) == pair.second,
- "Type '",
- pair.second,
- "' already in 'frame_types_' in key '",
- pair.first,
- "' doing nothing.");
if (frame_types_.at(pair.first) != pair.second)
{
WOLF_ERROR("Trying to append type '",
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4bba66a5327b652977914b736bc779c593b384cd..0af47ab5d9c472c974190691eeb33689a3514249 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -214,21 +214,47 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
switch (processing_step_) // Things to do before integrating motion data
{
case FIRST_TIME_WITHOUT_KF: {
+ WOLF_DEBUG(getType(), ": '", getName(), "' FIRST_TIME_WITHOUT_KF: KF");
+
// There is no KF: create own origin
new_keyframe = setOrigin(getProblem()->stateZero(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_BEFORE_INCOMING: {
+ WOLF_DEBUG(getType(),
+ ": '",
+ getName(),
+ "' FIRST_TIME_WITH_KF_BEFORE_INCOMING: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
+
// cannot join to the KF: create own origin
new_keyframe = setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
}
case FIRST_TIME_WITH_KF_ON_INCOMING: {
+ WOLF_DEBUG(getType(),
+ ": '",
+ getName(),
+ "' FIRST_TIME_WITH_KF_ON_INCOMING: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
+
// can join to the KF
setOrigin(keyframe_from_callback);
break;
}
case FIRST_TIME_WITH_KF_AFTER_INCOMING: {
+ WOLF_DEBUG(getType(),
+ ": '",
+ getName(),
+ "' FIRST_TIME_WITH_KF_AFTER_INCOMING: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
+
// cannot join to the KF: create own origin
new_keyframe = setOrigin(getProblem()->getState(getStateKeys()), _incoming_ptr->getTimeStamp());
break;
@@ -243,6 +269,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// integrate motion data
// Done at this place because emplaceFirstFrame() needs
integrateOneStep();
+ last_ptr_->setTimeStamp(getBuffer().back().ts_);
// perform bootstrap steps (usually only IMU requires this)
if (bootstrapping_) bootstrap();
@@ -276,6 +303,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* x ----n ----e ... ----o ----------l
* \_f_/ \_f_/
*/
+ WOLF_DEBUG(getType(),
+ ": '",
+ getName(),
+ "' RUNNING_WITH_KF_BEFORE_ORIGIN: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
// extract KF elements
TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
@@ -375,6 +409,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* x ----------x ----o ----l
* \____f____/ \_f_/
*/
+ WOLF_DEBUG(getType(),
+ ": '",
+ getName(),
+ "' RUNNING_WITH_KF_AFTER_ORIGIN: KF",
+ keyframe_from_callback->id(),
+ " callback unpacked with ts= ",
+ keyframe_from_callback->getTimeStamp());
// extract KF elements
TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
@@ -417,10 +458,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- // Update time stamp
- auto ts = getTimeStamp();
- last_ptr_->setTimeStamp(ts);
-
if (permittedKeyFrame() && voteForKeyFrame())
{
/*
@@ -453,6 +490,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
*
*/
+ WOLF_DEBUG(getType(), ": '", getName(), "' VOTED FOR KF at ts= ", last_ptr_->getTimeStamp());
+
// Set the capture's calibration with best results available (in case origin_ptr_ received a solve())
setCalibration(last_ptr_, getCalibration(origin_ptr_));
@@ -504,7 +543,10 @@ VectorComposite ProcessorMotion::getState(StateKeys _keys) const
for (auto key : _keys)
if (getStateTypes().has(key)) has_keys.push_back(key);
- WOLF_DEBUG("Processor has no state for all keys '",
+ WOLF_DEBUG(getType(),
+ " - ",
+ getName(),
+ " has no state for all keys '",
_keys,
"'. Returning a VectorComposite only with the available ones: '",
has_keys,
@@ -521,7 +563,11 @@ VectorComposite ProcessorMotion::getState(StateKeys _keys) const
// Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
if (origin_ptr_ == nullptr or origin_ptr_->isRemoving() or last_ptr_ == nullptr)
{
- WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
+ WOLF_DEBUG(
+ getType(),
+ " - ",
+ getName(),
+ " has no state (origin null or removing or last null). Returning an empty VectorComposite with no blocks");
return VectorComposite(); // return empty state
}
@@ -602,7 +648,10 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, StateKeys _keys)
for (auto key : _keys)
if (getStateTypes().has(key)) has_keys.push_back(key);
- WOLF_DEBUG("Processor has no state for all keys '",
+ WOLF_DEBUG(getType(),
+ " - ",
+ getName(),
+ " has no state for all keys '",
_keys,
"'. Returning a VectorComposite only with the available ones: '",
has_keys,
@@ -620,7 +669,12 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, StateKeys _keys)
if (capture_motion == nullptr) // we do not have any info of where to find a valid state
{
- WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
+ WOLF_DEBUG(getType(),
+ " - ",
+ getName(),
+ " has no state at ts = ",
+ _ts,
+ ". Returning an empty VectorComposite with no blocks");
return VectorComposite(); // return empty state
}
@@ -898,7 +952,6 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
assert(_ts.ok());
// assert(last_ptr_ != nullptr);
- // Need to uncomment this line so that wolf_ros_apriltag doesn't crash
if (last_ptr_ == nullptr) return nullptr;
// First check if last_ptr is the one we are looking for
@@ -906,38 +959,25 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
// Then look in the Wolf tree...
// -----------------------------
- //
-
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
// Note: since the buffer goes from a KF in the past until the next KF, we need to:
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
- FrameBaseConstPtr frame = nullptr;
- CaptureBaseConstPtr capture = nullptr;
- CaptureMotionConstPtr capture_motion = nullptr;
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
{
- frame = frame_rev_iter->second;
- auto sensor = getSensor();
- capture = frame->getCaptureOf(sensor);
- if (capture != nullptr)
+ auto captures = frame_rev_iter->second->getCapturesOf(getSensor());
+ for (auto capture : captures)
{
- assert(std::dynamic_pointer_cast<const CaptureMotion>(capture) != nullptr);
- // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
- capture_motion = std::static_pointer_cast<const CaptureMotion>(capture);
+ // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor and it is a CaptureMotion
+ auto capture_motion = std::dynamic_pointer_cast<const CaptureMotion>(capture);
- if (capture_motion->containsTimeStamp(_ts, getTimeTolerance()))
- {
- // Found time stamp satisfying rule 3 above !! ==> break for loop
- break;
- }
- else
- capture_motion = nullptr;
+ // Found time stamp satisfying rule 3 above !! ==> break for loop
+ if (capture_motion and capture_motion->containsTimeStamp(_ts, getTimeTolerance())) return capture_motion;
}
}
- return capture_motion;
+ return nullptr;
}
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts)
@@ -951,38 +991,25 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
// Then look in the Wolf tree...
// -----------------------------
- //
-
// We need to search in previous keyframes for the capture containing a motion buffer with the queried time stamp
// Note: since the buffer goes from a KF in the past until the next KF, we need to:
// 1. See that the KF contains a CaptureMotion
// 2. See that the TS is smaller than the KF's TS
// 3. See that the TS is bigger than the first Motion in the CaptureMotion's buffer (if any)
- FrameBasePtr frame = nullptr;
- CaptureBasePtr capture = nullptr;
- CaptureMotionPtr capture_motion = nullptr;
- auto frame_map = getProblem()->getTrajectory()->getFrameMap();
+ auto frame_map = getProblem()->getTrajectory()->getFrameMap();
for (auto frame_rev_iter = frame_map.rbegin(); frame_rev_iter != frame_map.rend(); ++frame_rev_iter)
{
- frame = frame_rev_iter->second;
- auto sensor = getSensor();
- capture = frame->getCaptureOf(sensor);
- if (capture != nullptr)
+ auto captures = frame_rev_iter->second->getCapturesOf(getSensor());
+ for (auto capture : captures)
{
- assert(std::dynamic_pointer_cast<CaptureMotion>(capture) != nullptr);
- // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
- capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
+ // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor and it is a CaptureMotion
+ auto capture_motion = std::dynamic_pointer_cast<CaptureMotion>(capture);
- if (capture_motion->containsTimeStamp(_ts, getTimeTolerance()))
- {
- // Found time stamp satisfying rule 3 above !! ==> break for loop
- break;
- }
- else
- capture_motion = nullptr;
+ // Found time stamp satisfying rule 3 above !! ==> return
+ if (capture_motion and capture_motion->containsTimeStamp(_ts, getTimeTolerance())) return capture_motion;
}
}
- return capture_motion;
+ return nullptr;
}
FrameBasePtr ProcessorMotion::computeProcessingStep()
@@ -997,32 +1024,19 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
{
if (checkTimeTolerance(keyframe_from_callback, incoming_ptr_))
{
- WOLF_DEBUG("PM ", getName(), ": First time with a KF compatible.")
processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
}
else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
- WOLF_DEBUG("PM ",
- getName(),
- ": First time with a KF too old. It seems the prior has been set before receiving the "
- "first capture of this processor.")
processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
}
else
{
- WOLF_DEBUG("PM ",
- getName(),
- ": First time with a KF newer than the first capture. It only can happen if prior mode is "
- "'nothing'")
processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
}
}
else
{
- WOLF_DEBUG(
- "PM ",
- getName(),
- ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
processing_step_ = FIRST_TIME_WITHOUT_KF;
}
@@ -1065,6 +1079,8 @@ void ProcessorMotion::assertsCaptureMotion(CaptureMotionPtr _capture, std::strin
{
assert(_capture != nullptr && (error_prefix + ": null capture").c_str());
assert(not _capture->getBuffer().empty() && (error_prefix + ": empty buffer").c_str());
+ // assert(_capture->getBuffer().back().ts_ == _capture->getTimeStamp() &&
+ // (error_prefix + ": buffer's first motion time stamp is not equal to capture time stamp").c_str());
assert((_capture->getBuffer().front().delta_integr_ - deltaZero()).isZero(Constants::EPS_SMALL) &&
(error_prefix + ": buffer's first motion is not zero!").c_str());
assert(_capture->getBuffer().front().delta_integr_cov_.isZero(Constants::EPS_SMALL) &&
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 0b8c2f7a35051c7b32dd88b19dd980e9224c091e..13f8c60cfde4e0a674a9e9b86cd43ba4fc9c9995 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -66,9 +66,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
buffer_frame_.select(incoming_ptr_->getTimeStamp(), getTimeTolerance());
buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
- WOLF_DEBUG("PT ",
+ WOLF_DEBUG(getType(),
+ ": '",
getName(),
- " FIRST_TIME_WITH_KEYFRAME: KF",
+ "' FIRST_TIME_WITH_KEYFRAME: KF",
keyframe_from_callback->id(),
" callback unpacked with ts= ",
keyframe_from_callback->getTimeStamp());
@@ -81,11 +82,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// If captures match, then frames must match too
assert(incoming_ptr_->getFrame() != nullptr and incoming_ptr_->getFrame() == keyframe_from_callback and
"The keyframe has a Capture from this sensor, but this capture is not incoming!");
- WOLF_DEBUG("PT ", getName(), " Incoming capture has been processed previously by another processor!")
+ WOLF_DEBUG(getType(),
+ ": '",
+ getName(),
+ "' Incoming capture has been processed previously by another processor!")
}
else
{
- WOLF_DEBUG("PT ", getName(), " Incoming capture had not been processed by any other processor!")
+ WOLF_DEBUG(
+ getType(), ": '", getName(), "' Incoming capture had not been processed by any other processor!")
// Join KF
incoming_ptr_->link(keyframe_from_callback);
@@ -110,7 +115,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
case FIRST_TIME_WITHOUT_KEYFRAME: {
- WOLF_DEBUG("PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME");
+ WOLF_DEBUG(getType(), ": '", getName(), "' FIRST_TIME_WITHOUT_KEYFRAME");
// Check if incoming has already a Frame
auto frame_incoming = incoming_ptr_->getFrame();
@@ -118,7 +123,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
" Incoming capture has been processed and linked by another processor, but no keyframe callback "
"was received!");
- WOLF_DEBUG("PT ", getName(), " Incoming capture has not been processed by another processor!")
+ WOLF_DEBUG(getType(), ": '", getName(), "' Incoming capture has not been processed by another processor!")
// make a new KF at incoming
FrameBasePtr keyframe = getProblem()->emplaceFrame(incoming_ptr_->getTimeStamp(), getStateKeys());
@@ -158,16 +163,17 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// which happens to be within tolerance of timestamps.
// In this case we discard it anyway because we already have a KF in last
// and we can't link a capture to two KFs.
- WOLF_DEBUG("PT ",
+ WOLF_DEBUG(getType(),
+ ": '",
getName(),
- " SECOND_TIME_WITH_KEYFRAME: KF",
+ "' 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");
+ WOLF_DEBUG(getType(), ": '", getName(), "' SECOND_TIME_WITHOUT_KEYFRAME");
// Process known information
processKnownProfiling();
@@ -192,9 +198,10 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
FrameBasePtr keyframe_from_callback = buffer_frame_.select(last_ptr_->getTimeStamp(), getTimeTolerance());
buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
- WOLF_DEBUG("PT ",
+ WOLF_DEBUG(getType(),
+ ": '",
getName(),
- " RUNNING_WITH_KEYFRAME: KF",
+ "' RUNNING_WITH_KEYFRAME: KF",
keyframe_from_callback->id(),
" callback unpacked with ts= ",
keyframe_from_callback->getTimeStamp());
@@ -208,7 +215,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// chack if the received KF has a capture of this sensor, and if it matches with last_ptr
if (last_ptr_ == keyframe_from_callback->getCaptureOf(this->getSensor()))
{
- WOLF_DEBUG("PT ", getName(), " Last capture has been processed previously by another processor!")
+ WOLF_DEBUG(
+ getType(), ": '", getName(), "' Last capture has been processed previously by another processor!")
// If captures match, then frames must match too
assert(last_ptr_->getFrame() == keyframe_from_callback and
@@ -218,7 +226,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
}
else
{
- WOLF_DEBUG("PT ", getName(), " Last capture had not been processed previously!")
+ WOLF_DEBUG(getType(), ": '", getName(), "' Last capture had not been processed previously!")
// join KF
last_ptr_->link(keyframe_from_callback);
@@ -241,7 +249,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
case RUNNING_WITHOUT_KEYFRAME: {
- WOLF_DEBUG("PT ", getName(), " RUNNING_WITHOUT_KEYFRAME");
+ WOLF_DEBUG(getType(), ": '", getName(), "' RUNNING_WITHOUT_KEYFRAME");
processKnownProfiling();