Fix dynamic/static state block registration in sensors and captures

src/capture_base.cpp

@@ -50,7 +50,6 @@ CaptureBase::CaptureBase(const std::string& _type,
         }
 
         getSensorPtr()->setHasCapture();
-        registerNewStateBlocks();
     }
     else if (_p_ptr || _o_ptr || _intr_ptr)
     {

src/frame_base.cpp

@@ -311,6 +311,7 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
     capture_list_.push_back(_capt_ptr);
     _capt_ptr->setFramePtr(shared_from_this());
     _capt_ptr->setProblem(getProblem());
+    _capt_ptr->registerNewStateBlocks();
     return _capt_ptr;
 }
 

src/sensor_base.cpp

@@ -197,9 +197,21 @@ void SensorBase::registerNewStateBlocks()
 {
     if (getProblem() != nullptr)
     {
-        for (auto sbp : getStateBlockVec())
-            if (sbp != nullptr)
-                getProblem()->addStateBlock(sbp);
+        for (int i = 0; i < getStateBlockVec().size(); i++)
+        {
+            if (i < 2 && !isExtrinsicDynamic())
+            {
+                auto sbp = getStateBlockPtrStatic(i);
+                if (sbp != nullptr)
+                    getProblem()->addStateBlock(sbp);
+            }
+            if (i >= 2 && !isIntrinsicDynamic())
+            {
+                auto sbp = getStateBlockPtrStatic(i);
+                if (sbp != nullptr)
+                    getProblem()->addStateBlock(sbp);
+            }
+        }
     }
 }