From 7cd8439c269961885ecfe410a6e0512ca7e25b6e Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 26 Apr 2018 16:15:21 +0200
Subject: [PATCH] Fix dynamic/static state block registration in sensors and
 captures

---
 src/capture_base.cpp |  1 -
 src/frame_base.cpp   |  1 +
 src/sensor_base.cpp  | 18 +++++++++++++++---
 3 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index b0d1f0982..60b2858cc 100644
--- a/src/capture_base.cpp
+++ b/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)
     {
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index a1a806bee..70016a8e6 100644
--- a/src/frame_base.cpp
+++ b/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;
 }
 
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index ce30ddf80..8475e962c 100644
--- a/src/sensor_base.cpp
+++ b/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);
+            }
+        }
     }
 }
 
-- 
GitLab