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

Fix remove(): it was not updating T::last_keyframe_ptr

parent 5261b666
No related branches found
No related tags found
No related merge requests found
...@@ -50,13 +50,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr ...@@ -50,13 +50,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
FrameBase::~FrameBase() FrameBase::~FrameBase()
{ {
// Remove Frame State Blocks
removeStateBlocks(); removeStateBlocks();
// if (isKey())
// std::cout << "destructed -KF" << id() << std::endl;
// else
// std::cout << "destructed -F" << id() << std::endl;
} }
void FrameBase::remove() void FrameBase::remove()
...@@ -83,6 +77,9 @@ void FrameBase::remove() ...@@ -83,6 +77,9 @@ void FrameBase::remove()
// Remove Frame State Blocks // Remove Frame State Blocks
removeStateBlocks(); removeStateBlocks();
if (getTrajectoryPtr()->getLastKeyFramePtr()->id() == this_F->id())
getTrajectoryPtr()->setLastKeyFramePtr(getTrajectoryPtr()->findLastKeyFramePtr());
// std::cout << "Removed F" << id() << std::endl; // std::cout << "Removed F" << id() << std::endl;
} }
} }
...@@ -252,24 +249,6 @@ void FrameBase::setStatus(StateStatus _st) ...@@ -252,24 +249,6 @@ void FrameBase::setStatus(StateStatus _st)
if (getProblem() != nullptr) if (getProblem() != nullptr)
getProblem()->updateStateBlockPtr(sb); getProblem()->updateStateBlockPtr(sb);
} }
// if (getPPtr() != nullptr)
// {
// getPPtr()->fix();
// if (getProblem() != nullptr)
// getProblem()->updateStateBlockPtr(getPPtr());
// }
// if (getOPtr() != nullptr)
// {
// getOPtr()->fix();
// if (getProblem() != nullptr)
// getProblem()->updateStateBlockPtr(getOPtr());
// }
// if (getVPtr() != nullptr)
// {
// getVPtr()->fix();
// if (getProblem() != nullptr)
// getProblem()->updateStateBlockPtr(getVPtr());
// }
} }
else if (status_ == ST_ESTIMATED) else if (status_ == ST_ESTIMATED)
{ {
...@@ -280,25 +259,6 @@ void FrameBase::setStatus(StateStatus _st) ...@@ -280,25 +259,6 @@ void FrameBase::setStatus(StateStatus _st)
if (getProblem() != nullptr) if (getProblem() != nullptr)
getProblem()->updateStateBlockPtr(sb); getProblem()->updateStateBlockPtr(sb);
} }
// if (getPPtr() != nullptr)
// {
// getPPtr()->unfix();
// if (getProblem() != nullptr)
// getProblem()->updateStateBlockPtr(getPPtr());
// }
// if (getOPtr() != nullptr)
// {
// getOPtr()->unfix();
// if (getProblem() != nullptr)
// getProblem()->updateStateBlockPtr(getOPtr());
// }
// if (getVPtr() != nullptr)
// {
// getVPtr()->unfix();
// if (getProblem() != nullptr)
// getProblem()->updateStateBlockPtr(getVPtr());
// }
} }
} }
......
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