Merge branch '351-using-processor-structure-instead-of-problem-structure' into 'devel'

Resolve "using Processor::structure instead of Problem::structure"

Closes #351

See merge request !382

include/core/frame/frame_base.h

@@ -54,11 +54,11 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
                   StateBlockPtr _v_ptr = nullptr);
 
         FrameBase(const TimeStamp& _ts,
-                  const std::string _frame_structure,
+                  const StateStructure& _frame_structure,
                   const VectorComposite& _state);
 
         FrameBase(const TimeStamp& _ts,
-                  const std::string _frame_structure,
+                  const StateStructure& _frame_structure,
                   const std::list<VectorXd>& _vectors);
 
         ~FrameBase() override;

include/core/state_block/has_state_blocks.h

@@ -21,7 +21,7 @@ class HasStateBlocks
 {
     public:
         HasStateBlocks();
-        HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
+        HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
         virtual ~HasStateBlocks();
 
         const StateStructure& getStructure() const { return structure_; }

include/core/trajectory/trajectory_base.h

@@ -12,6 +12,7 @@ class FrameBase;
 #include "core/common/wolf.h"
 #include "core/common/node_base.h"
 #include "core/common/time_stamp.h"
+#include "core/state_block/state_composite.h"
 
 //std includes
 
@@ -56,7 +57,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
          FrameMap frame_map_;
 
     protected:
-        std::string frame_structure_;  // Defines the structure of the Frames in the Trajectory.
+         StateStructure frame_structure_;  // Defines the structure of the Frames in the Trajectory.
         // FrameBasePtr last_key_frame_ptr_; // keeps pointer to the last key frame
         // FrameBasePtr last_key_or_aux_frame_ptr_; // keeps pointer to the last estimated frame
         

src/frame/frame_base.cpp

@@ -13,7 +13,7 @@ namespace wolf {
 unsigned int FrameBase::frame_id_count_ = 0;
 
 FrameBase::FrameBase(const TimeStamp& _ts,
-                     const std::string _frame_structure,
+                     const StateStructure& _frame_structure,
                      const VectorComposite& _state) :
         NodeBase("FRAME", "FrameBase"),
         HasStateBlocks(_frame_structure),
@@ -34,7 +34,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
 
 
 FrameBase::FrameBase(const TimeStamp& _ts,
-                     const std::string _frame_structure,
+                     const StateStructure& _frame_structure,
                      const std::list<VectorXd>& _vectors) :
                 NodeBase("FRAME", "FrameBase"),
                 HasStateBlocks(_frame_structure),

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
 {
@@ -236,7 +236,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
@@ -247,6 +246,16 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                 break;
             }
 
+            // update KF state (adding missing StateBlocks)
+            auto proc_state = getState(ts_from_callback);
+            for (auto pair_ckey_vec : proc_state)
+                if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
+                    keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
+                                                          FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+                                                                                    pair_ckey_vec.second,
+                                                                                    false));
+            keyframe_from_callback->setState(proc_state);
+
             // Find the capture acting as the buffer's origin
             auto capture_origin     = capture_existing->getOriginCapture();
 
@@ -337,7 +346,16 @@ 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);
+            for (auto pair_ckey_vec : proc_state)
+                if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
+                    keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
+                                                          FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+                                                                                    pair_ckey_vec.second,
+                                                                                    false));
+            keyframe_from_callback->setState(proc_state);
 
             auto & capture_existing = last_ptr_;