diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp index fb2d6d3e223899fb658b165cbcbc039582a7ffb8..afdc9446a7f3dc0cc54c8453bc07c190a821f90e 100644 --- a/src/processor/processor_tracker.cpp +++ b/src/processor/processor_tracker.cpp @@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) // We create a KF // set KF on last + last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp())); last_ptr_->getFrame()->setKey(); // make F; append incoming to new F diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp index f2eaa97e0e4c149334984aabac275d729a135516..2094bb2dbae7f76186295a217848cffbbf488f68 100644 --- a/src/processor/processor_tracker_feature.cpp +++ b/src/processor/processor_tracker_feature.cpp @@ -155,17 +155,6 @@ void ProcessorTrackerFeature::establishFactors() feature_in_last ->addFactor(fac_ptr); feature_in_origin->addConstrainedBy(fac_ptr); - if (fac_ptr != nullptr) // factor links - { - FrameBasePtr frm = fac_ptr->getFrameOther(); - if (frm) - frm->addConstrainedBy(fac_ptr); - CaptureBasePtr cap = fac_ptr->getCaptureOther(); - if (cap) - cap->addConstrainedBy(fac_ptr); - } - - WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), " origin: " , feature_in_origin->id() , " from last: " , feature_in_last->id() ); diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp index 1aa241a7fbd92314c9cc0767f5c47d9886c18038..02905a11b78b8104ac01ee11962179eb23f2d3b9 100644 --- a/src/processor/processor_tracker_landmark.cpp +++ b/src/processor/processor_tracker_landmark.cpp @@ -155,12 +155,6 @@ void ProcessorTrackerLandmark::establishFactors() { last_feature->addFactor(fac_ptr); lmk->addConstrainedBy(fac_ptr); - FrameBasePtr frm = fac_ptr->getFrameOther(); - if (frm) - frm->addConstrainedBy(fac_ptr); - CaptureBasePtr cap = fac_ptr->getCaptureOther(); - if (cap) - cap->addConstrainedBy(fac_ptr); } } } diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 17f6e562bb242ba315369effd722391f52f7cf4c..493bc9dcd60683a2836811175a6c57c13c63ff98 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -79,6 +79,9 @@ void SolverManager::update() // UPDATE STATE BLOCKS (state, fix or local parameterization) for (auto state_ptr : wolf_problem_->getStateBlockPtrList()) { + if (state_blocks_.find(state_ptr)==state_blocks_.end()) + continue; + assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !"); // state update @@ -106,8 +109,8 @@ void SolverManager::update() } } - assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update"); - assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update"); + //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update"); + //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update"); } wolf::ProblemPtr SolverManager::getProblem()