From 727bb82bf8c495299bbe29bef39df99c0f85411a Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Tue, 6 Mar 2018 15:50:42 +0100 Subject: [PATCH] Arrange setPrior() so that first all motions are set, then all trackers --- src/problem.cpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/src/problem.cpp b/src/problem.cpp index 5144a6234..9074a5af5 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -655,21 +655,14 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen: if (processor->isMotion()) // Motion processors will set its origin at the KF (std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe); - else - // Other processors will join the KF or not depending on their received data's time stamp and tolerances - processor->keyFrameCallback(origin_keyframe, _time_tolerance); - prior_is_set_ = true; -// keyFrameCallback(origin_keyframe, nullptr, _time_tolerance); - // Notify all other processors about the origin KF --> they will join it or not depending on their received data -// for (auto sensor : hardware_ptr_->getSensorList()) -// for (auto processor : sensor->getProcessorList()) -// if ( !processor->isMotion() ) -// processor->keyFrameCallback(origin_keyframe, _time_tolerance); - + for (auto sensor : hardware_ptr_->getSensorList()) + for (auto processor : sensor->getProcessorList()) + if ( !processor->isMotion() ) + processor->keyFrameCallback(origin_keyframe, _time_tolerance); return origin_keyframe; } -- GitLab