Merge branch '336-processor-motion-composite-state-adaptation' into 'devel'

Resolve "Processor motion composite state adaptation"

Closes #336

See merge request !373

include/core/processor/processor_motion.h

@@ -484,7 +484,8 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
         CaptureMotionPtr origin_ptr_;
         CaptureMotionPtr last_ptr_;
         CaptureMotionPtr incoming_ptr_;
-
+        bool last_nullptr_;
+        
     protected:
         // helpers to avoid allocation
         double dt_;                             ///< Time step

src/processor/processor_motion.cpp

@@ -33,6 +33,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         origin_ptr_(),
         last_ptr_(),
         incoming_ptr_(),
+        last_nullptr_(false),
         dt_(0.0), 
         x_(_state_size),
         delta_(_delta_size),
@@ -98,7 +99,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     PackKeyFramePtr pack = computeProcessingStep();
     if (pack)
         buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
-
+    
     switch(processing_step_)
     {
         case FIRST_TIME_WITHOUT_KF :
@@ -129,6 +130,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         case RUNNING_WITH_KF_ON_ORIGIN :
             break;
 
+        default :
+            break;
+    }
+
+
+    // integrate data
+    // Done at this place because setPrior() needs 
+    integrateOneStep();
+
+
+    switch(processing_step_)
+    {
         case RUNNING_WITH_KF_BEFORE_ORIGIN :
         {
 
@@ -161,13 +174,15 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
             // extract pack elements
             FrameBasePtr keyframe_from_callback = pack->key_frame;
+            TimeStamp ts_from_callback          = keyframe_from_callback->getTimeStamp();
+            keyframe_from_callback->setState(getState(ts_from_callback));
 
             // find the capture whose buffer is affected by the new keyframe
-            auto capture_existing   = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k
+            auto capture_existing   = findCaptureContainingTimeStamp(ts_from_callback); // k
 
             if (!capture_existing)
             {
-                WOLF_WARN("A KF before first motion capture (TS = ", keyframe_from_callback->getTimeStamp(), "). ProcessorMotion cannot do anything.");
+                WOLF_WARN("A KF before first motion capture (TS = ", ts_from_callback, "). ProcessorMotion cannot do anything.");
                 break;
             }
 
@@ -178,7 +193,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             VectorXd calib_origin   = getCalibration(capture_origin);
 
             // emplace a new motion capture to the new keyframe
-            TimeStamp ts_from_callback          = keyframe_from_callback->getTimeStamp();
             auto capture_for_keyframe_callback  = emplaceCapture(keyframe_from_callback, // j
                                                                  getSensor(),
                                                                  ts_from_callback,
@@ -260,6 +274,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
             // extract pack elements
             FrameBasePtr keyframe_from_callback = pack->key_frame;
+            TimeStamp ts_from_callback          = keyframe_from_callback->getTimeStamp();
+            keyframe_from_callback->setState(getState(ts_from_callback));
 
             auto & capture_existing = last_ptr_;
 
@@ -270,7 +286,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             VectorXd calib_origin   = getCalibration(capture_origin);
 
             // emplace a new motion capture to the new keyframe
-            TimeStamp    ts_from_callback      = keyframe_from_callback->getTimeStamp();
             auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
                                                                 getSensor(),
                                                                 ts_from_callback,
@@ -300,17 +315,18 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             break;
     }
 
-    ////////////////////////////////////////////////////
-    // NOW on with the received data
+    if (last_nullptr_){
+        integrateOneStep();
+        last_nullptr_ = false;
+    }
 
-    // integrate data
-    integrateOneStep();
 
     // Update state and time stamps
     const auto& ts = getTimeStamp();
     last_ptr_->setTimeStamp( ts );
     last_ptr_->getFrame()->setTimeStamp( ts );
-    last_ptr_->getFrame()->setState( getProblem()->getState( ts ) );
+    VectorComposite state_propa = getState( ts );
+    last_ptr_->getFrame()->setState( state_propa );
 
     if (permittedKeyFrame() && voteForKeyFrame())
     {