Merge branch 'devel' into 256-new-data-structure-for-storing-stateblocks

include/core/utils/params_server.hpp

@@ -40,14 +40,19 @@ public:
         _params.insert(std::pair<std::string, std::string>(key, value));
     }
 
-//    template<typename T>
-//    T getParam(std::string key, std::string def_value) const {
-//        if(_params.find(key) != _params.end()){
-//            return converter<T>::convert(_params.find(key)->second);
-//        }else{
-//            return converter<T>::convert(def_value);
-//        }
-//    }
+    void addParams(std::map<std::string, std::string> params)
+    {
+        _params.insert(params.begin(), params.end());
+    }
+
+   // template<typename T>
+   // T getParam(std::string key, std::string def_value) const {
+   //     if(_params.find(key) != _params.end()){
+   //         return converter<T>::convert(_params.find(key)->second);
+   //     }else{
+   //         return converter<T>::convert(def_value);
+   //     }
+   // }
 
     template<typename T>
     T getParam(std::string key) const {

src/processor/processor_tracker.cpp

@@ -87,8 +87,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Issue KF callback with new KF
             getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
 
-            // Update pointers
             resetDerived();
+
+            // Update pointers
             origin_ptr_ = incoming_ptr_;
             last_ptr_   = incoming_ptr_;
             incoming_ptr_ = nullptr;
@@ -175,9 +176,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->setKey();
 
-                // make F; append incoming to new F
-                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
-                incoming_ptr_->link(frm);
+                // // make F; append incoming to new F
+                // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+                // incoming_ptr_->link(frm);
 
                 // Establish factors
                 establishFactors();
@@ -187,6 +188,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
                 // Update pointers
                 resetDerived();
+
+                // make F; append incoming to new F
+                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+                incoming_ptr_->link(frm);
+
                 origin_ptr_ = last_ptr_;
                 last_ptr_   = incoming_ptr_;
                 incoming_ptr_ = nullptr;