diff --git a/src/capture_base.cpp b/src/capture_base.cpp index b0d1f0982da56ec28b0bced2de743310bfc6f2bd..60b2858cc7a8a202016d217f209c1e6c659cca56 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 a1a806bee07eec7387b19b99364f06e4ce72e88d..70016a8e60045b2edccaa871e1efc85758d3ca8c 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 ce30ddf8084ec0598e76fc4aa2781628d1d761f7..8475e962ccb35408e2fe10b2d287f11da18fd96e 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); + } + } } }