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