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