diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 1c02ecaecda4d73a40adaf68dccede16376e833a..02fc7139eee9049be4821ada9fdef25d6d648fc3 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -426,6 +426,13 @@ VectorComposite Problem::getState(const StateStructure& _structure) const state.insert(pair_key_vec); } } + // check for empty blocks and fill them with zeros + for (const auto& ckey : frame_structure_) + { + const auto& key = string(1,ckey); + if (state.count(key) == 0) + state.emplace(key, stateZero(key).at(key)); + } } return state; @@ -532,7 +539,7 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const if (key == "O") { if (dim_ == 2) vec = VectorXd::Zero(1); - if (dim_ == 3) vec = Quaterniond::Identity().coeffs(); + else if (dim_ == 3) vec = Quaterniond::Identity().coeffs(); } else { diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 652a58d7e80071014920fe05645612f4f5c3bcf4..bcae7e627f0bba7ac786a06c9f37eba4becbe509 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -110,7 +110,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) case FIRST_TIME_WITH_KF_BEFORE_INCOMING : { // cannot joint to the KF: create own origin - setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp()); + setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp()); break; } case FIRST_TIME_WITH_KF_ON_INCOMING : @@ -122,7 +122,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) case FIRST_TIME_WITH_KF_AFTER_INCOMING : { // cannot joint to the KF: create own origin - setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp()); + setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp()); break; } case RUNNING_WITHOUT_KF : @@ -300,9 +300,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) integrateOneStep(); // Update state and time stamps - last_ptr_->setTimeStamp(getTimeStamp()); - last_ptr_->getFrame()->setTimeStamp(getTimeStamp()); - last_ptr_->getFrame()->setState(getProblem()->getState(getTimeStamp())); + const auto& ts = getTimeStamp(); + last_ptr_->setTimeStamp( ts ); + last_ptr_->getFrame()->setTimeStamp( ts ); + last_ptr_->getFrame()->setState( getProblem()->getState( ts ) ); if (permittedKeyFrame() && voteForKeyFrame()) { @@ -384,8 +385,20 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) VectorComposite ProcessorMotion::getState() const { - assert ((last_ptr_) && "ProcessorMotion does not have valid last_ptr yet"); - assert ((last_ptr_->getFrame()) && "ProcessorMotion's last_ptr does not have a frame yet"); + + if (last_ptr_ == nullptr or last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state + // Further checking here for origin_ptr is redundant: if last=null, then origin=null too. + return VectorComposite(); // return empty state + + + // From here on, we do have info to compute a valid state + + // if buffer is empty --> we did not advance from origin! + // this may happen when in the very first frame where the capture has no motion info --> empty buffer + if (last_ptr_->getBuffer().empty()) + { + return last_ptr_->getFrame()->getState(state_structure_); + } /* Doing this: * @@ -447,14 +460,17 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const // We need to search for the capture containing a motion buffer with the queried time stamp CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts); + if (capture_motion == nullptr) // we do not have any info of where to find a valid state + return VectorComposite(); // return empty state - if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp + else // We found a CaptureMotion whose buffer contains the time stamp { // if buffer is empty --> we did not advance from origin! + // this may happen when in the very first frame where the capture has no motion info --> empty buffer if (capture_motion->getBuffer().empty()) { - return capture_motion->getFrame()->getState(); + return capture_motion->getFrame()->getState(state_structure_); } /* Doing this: @@ -511,8 +527,6 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const return state; } - else - return VectorComposite(); // return empty state } FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)