Skip to content
Snippets Groups Projects
Commit 7cd8439c authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Fix dynamic/static state block registration in sensors and captures

parent 07b70f03
No related branches found
No related tags found
No related merge requests found
......@@ -50,7 +50,6 @@ CaptureBase::CaptureBase(const std::string& _type,
}
getSensorPtr()->setHasCapture();
registerNewStateBlocks();
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
......
......@@ -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;
}
......
......@@ -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);
}
}
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment