diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 65960d878ac414308b1032f913e4a9f9e69cf243..cd2d78caa78427ff2b0e684667b79f3a9e7073d8 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -8,7 +8,7 @@ #include "core/processor/processor_motion.h" - +#include "core/state_block/factory_state_block.h" namespace wolf { @@ -200,7 +200,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // extract pack elements FrameBasePtr keyframe_from_callback = pack->key_frame; TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp(); - keyframe_from_callback->setState(getState(ts_from_callback)); // find the capture whose buffer is affected by the new keyframe auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k @@ -211,6 +210,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) break; } + // update KF state (adding missing StateBlocks) + auto proc_state = getState(ts_from_callback); + if (keyframe_from_callback->getStructure() != proc_state.getStructure()) + for (auto ckey : proc_state.getStructure()) + if (!keyframe_from_callback->isInStructure(std::string(1,ckey))) + keyframe_from_callback->addStateBlock(std::string(1,ckey), + FactoryStateBlock::create(std::string(1,ckey), + proc_state[std::string(1,ckey)], + false)); + keyframe_from_callback->setState(proc_state); + // Find the capture acting as the buffer's origin auto capture_origin = capture_existing->getOriginCapture(); @@ -301,7 +311,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) // extract pack elements FrameBasePtr keyframe_from_callback = pack->key_frame; TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp(); - keyframe_from_callback->setState(getState(ts_from_callback)); + + // update KF state (adding missing StateBlocks) + auto proc_state = getState(ts_from_callback); + if (keyframe_from_callback->getStructure() != proc_state.getStructure()) + for (auto ckey : proc_state.getStructure()) + if (!keyframe_from_callback->isInStructure(std::string(1,ckey))) + keyframe_from_callback->addStateBlock(std::string(1,ckey), + FactoryStateBlock::create(std::string(1,ckey), + proc_state[std::string(1,ckey)], + false)); + keyframe_from_callback->setState(proc_state); auto & capture_existing = last_ptr_;