Improve state creation when no info is present

src/problem/problem.cpp

@@ -426,6 +426,13 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
                     state.insert(pair_key_vec);
             }
         }
+        // check for empty blocks and fill them with zeros
+        for (const auto& ckey : frame_structure_)
+        {
+            const auto& key = string(1,ckey);
+            if (state.count(key) == 0)
+                state.emplace(key, stateZero(key).at(key));
+        }
     }
 
     return state;
@@ -532,7 +539,7 @@ VectorComposite Problem::stateZero ( const StateStructure& _structure ) const
         if (key == "O")
         {
             if (dim_ == 2) vec = VectorXd::Zero(1);
-            if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
+            else if (dim_ == 3) vec = Quaterniond::Identity().coeffs();
         }
         else
         {

src/processor/processor_motion.cpp

@@ -110,7 +110,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         case FIRST_TIME_WITH_KF_BEFORE_INCOMING :
         {
             // cannot joint to the KF: create own origin
-            setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
+            setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
             break;
         }
         case FIRST_TIME_WITH_KF_ON_INCOMING :
@@ -122,7 +122,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         case FIRST_TIME_WITH_KF_AFTER_INCOMING :
         {
             // cannot joint to the KF: create own origin
-            setOrigin(getProblem()->stateZero(getStateStructure()), _incoming_ptr->getTimeStamp());
+            setOrigin(getProblem()->getState(getStateStructure()), _incoming_ptr->getTimeStamp());
             break;
         }
         case RUNNING_WITHOUT_KF :
@@ -300,9 +300,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
     integrateOneStep();
 
     // Update state and time stamps
-    last_ptr_->setTimeStamp(getTimeStamp());
-    last_ptr_->getFrame()->setTimeStamp(getTimeStamp());
-    last_ptr_->getFrame()->setState(getProblem()->getState(getTimeStamp()));
+    const auto& ts = getTimeStamp();
+    last_ptr_->setTimeStamp( ts );
+    last_ptr_->getFrame()->setTimeStamp( ts );
+    last_ptr_->getFrame()->setState( getProblem()->getState( ts ) );
 
     if (permittedKeyFrame() && voteForKeyFrame())
     {
@@ -384,8 +385,20 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
 VectorComposite ProcessorMotion::getState() const
 {
-    assert ((last_ptr_) && "ProcessorMotion does not have valid last_ptr yet");
-    assert ((last_ptr_->getFrame()) && "ProcessorMotion's last_ptr does not have a frame yet");
+
+    if (last_ptr_ == nullptr or last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
+                                                                  // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
+        return VectorComposite(); // return empty state
+
+
+    // From here on, we do have info to compute a valid state
+
+    // if buffer is empty --> we did not advance from origin!
+    // this may happen when in the very first frame where the capture has no motion info --> empty buffer
+    if (last_ptr_->getBuffer().empty())
+    {
+        return last_ptr_->getFrame()->getState(state_structure_);
+    }
 
     /* Doing this:
      *
@@ -447,14 +460,17 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
     // We need to search for the capture containing a motion buffer with the queried time stamp
     CaptureMotionPtr capture_motion = findCaptureContainingTimeStamp(_ts);
 
+    if (capture_motion == nullptr) // we do not have any info of where to find a valid state
+        return VectorComposite();  // return empty state
 
 
-    if (capture_motion)  // We found a CaptureMotion whose buffer contains the time stamp
+    else // We found a CaptureMotion whose buffer contains the time stamp
     {
         // if buffer is empty --> we did not advance from origin!
+        // this may happen when in the very first frame where the capture has no motion info --> empty buffer
         if (capture_motion->getBuffer().empty())
         {
-            return capture_motion->getFrame()->getState();
+            return capture_motion->getFrame()->getState(state_structure_);
         }
 
         /* Doing this:
@@ -511,8 +527,6 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts) const
         return state;
 
     }
-    else
-        return VectorComposite(); // return empty state
 }
 
 FrameBasePtr ProcessorMotion::setOrigin(const VectorComposite& _x_origin, const TimeStamp& _ts_origin)