debugging demo vio

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:

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 '",

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) &&

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();