Skip to content
Snippets Groups Projects
Commit bcc07e16 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

debugging demo vio

parent d621a4af
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #21235 failed
Stage: license
    Stage: clang
      Stage: yaml_templates
        Stage: build_and_test
          Stage: deploy_plugins
            Stage: deploy_ros
              Stage: deploy_ros2
                Hide whitespace changes
                Inline Side-by-side
                src/capture/capture_motion.cpp
                + 3
                − 1
                View file @ bcc07e16
                ......@@ -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
                + 0
                − 6
                View file @ bcc07e16
                ......@@ -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
                + 80
                − 64
                View file @ bcc07e16
                ......@@ -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
                + 22
                − 14
                View file @ bcc07e16
                ......@@ -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();
                ......
                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