Remove Frame::addStateBlock(...)

include/core/frame/frame_base.h

@@ -84,8 +84,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
 
         // State blocks ----------------------------------------------------
     public:
-        using HasStateBlocks::addStateBlock;
-        StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb);
         StateBlockPtr getV() const;
 
     protected:
@@ -169,12 +167,6 @@ inline unsigned int FrameBase::id() const
     return frame_id_;
 }
 
-//inline bool FrameBase::isKey() const
-//{
-//    return true;
-////    return (type_ == KEY);
-//}
-
 inline void FrameBase::getTimeStamp(TimeStamp& _ts) const
 {
     _ts = time_stamp_;
@@ -210,14 +202,6 @@ inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
     return constrained_by_list_;
 }
 
-inline StateBlockPtr FrameBase::addStateBlock(const char& _sb_type,
-                                              const StateBlockPtr& _sb)
-{
-    HasStateBlocks::addStateBlock(_sb_type, _sb, getProblem());
-
-    return _sb;
-}
-
 inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
 {
     trajectory_ptr_ = _trj_ptr;

src/frame/frame_base.cpp

@@ -28,7 +28,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
 
         StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
 
-        addStateBlock(key, sb);
+        addStateBlock(key, sb, getProblem());
     }
 }
 
@@ -51,7 +51,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
 
         StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
 
-        addStateBlock(key, sb);
+        addStateBlock(key, sb, getProblem());
 
         vec_it++;
     }
@@ -70,15 +70,15 @@ FrameBase::FrameBase(const TimeStamp& _ts,
 {
     if (_p_ptr)
     {
-        addStateBlock('P', _p_ptr);
+        addStateBlock('P', _p_ptr, getProblem());
     }
     if (_o_ptr)
     {
-        addStateBlock('O', _o_ptr);
+        addStateBlock('O', _o_ptr, getProblem());
     }
     if (_v_ptr)
     {
-        addStateBlock('V', _v_ptr);
+        addStateBlock('V', _v_ptr, getProblem());
     }
 }
 

src/processor/processor_motion.cpp

@@ -253,7 +253,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                     keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
                                                           FactoryStateBlock::create(string(1, pair_ckey_vec.first),
                                                                                     pair_ckey_vec.second,
-                                                                                    false));
+                                                                                    false),
+                                                          getProblem());
             keyframe_from_callback->setState(proc_state);
 
             // Find the capture acting as the buffer's origin
@@ -354,7 +355,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                     keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
                                                           FactoryStateBlock::create(string(1, pair_ckey_vec.first),
                                                                                     pair_ckey_vec.second,
-                                                                                    false));
+                                                                                    false),
+                                                          getProblem());
             keyframe_from_callback->setState(proc_state);
 
             auto & capture_existing = last_ptr_;
@@ -401,6 +403,13 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     last_ptr_->setTimeStamp( ts );
     last_ptr_->getFrame()->setTimeStamp( ts );
     VectorComposite state_propa = getState( ts );
+    for (auto pair_ckey_vec : state_propa)
+        if (!last_ptr_->getFrame()->isInStructure(pair_ckey_vec.first))
+            last_ptr_->getFrame()->addStateBlock(pair_ckey_vec.first,
+                                                 FactoryStateBlock::create(string(1, pair_ckey_vec.first),
+                                                                           pair_ckey_vec.second,
+                                                                           false),
+                                                 getProblem());
     last_ptr_->getFrame()->setState( state_propa );
 
     if (permittedKeyFrame() && voteForKeyFrame())

test/gtest_has_state_blocks.cpp

@@ -64,7 +64,7 @@ TEST_F(HasStateBlocksTest, Notifications_add_makeKF)
 
     // ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
 
-    F0->addStateBlock('V', sbv0);
+    F0->addStateBlock('V', sbv0, nullptr);
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
 
@@ -87,7 +87,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
     // F1->link(problem->getTrajectory());
     F1->link(problem);
 
-    F1->addStateBlock('P', sbp1);
+    F1->addStateBlock('P', sbp1, problem);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
     ASSERT_EQ(n, ADD);
@@ -96,7 +96,7 @@ TEST_F(HasStateBlocksTest, Notifications_makeKF_add)
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
 
-    F1->addStateBlock('V', sbv1);
+    F1->addStateBlock('V', sbv1, problem);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
     ASSERT_EQ(n, ADD);
@@ -115,7 +115,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
 
     ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
 
-    F0->addStateBlock('V', sbv0);
+    F0->addStateBlock('V', sbv0, nullptr);
     F0->link(problem);
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
@@ -142,7 +142,7 @@ TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
 
     // Add again the same SB. This should crash
 
-    ASSERT_DEATH( F0->addStateBlock('V', sbv0) , "" );
+    ASSERT_DEATH( F0->addStateBlock('V', sbv0, nullptr) , "" );
 
 }
 
@@ -164,7 +164,7 @@ TEST_F(HasStateBlocksTest, stateBlockKey)
 
 TEST_F(HasStateBlocksTest, getState_structure)
 {
-    F0->addStateBlock('V', sbv0); // now KF0 is POV
+    F0->addStateBlock('V', sbv0, nullptr); // now KF0 is POV
 
     WOLF_DEBUG("Retrieving state from F0 with structure ", F0->getStructure());