Merge branch...

Merge branch '344-problem-getstate-gettimestamp-considering-not-initialized-processors' into 'devel'

Resolve "Problem::getState/getTimeStamp considering not initialized processors"

Closes #344

See merge request !380

src/problem/problem.cpp

@@ -419,56 +419,70 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
 
 TimeStamp Problem::getTimeStamp ( ) const
 {
-    if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-    {
-        auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+    TimeStamp  ts = TimeStamp::Invalid();
 
-        assert(last_kf_or_aux != nullptr && "Problem has no Keyframe so no timestamp can be obtained!");
+    for (const auto& prc : processor_is_motion_list_)
+        if (prc->getTimeStamp().ok())
+            if ( (not ts.ok() ) or prc->getTimeStamp() > ts)
+                ts = prc->getTimeStamp();
 
-        return last_kf_or_aux->getTimeStamp();
-    }
-    else
+
+    if (not ts.ok())
     {
-        TimeStamp  ts(0);
-        for (const auto& prc : processor_is_motion_list_)
-            if (prc->getTimeStamp() > ts)
-                ts = prc->getTimeStamp();
-        return ts;
+        const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+
+        if (last_kf_or_aux)
+            ts = last_kf_or_aux->getTimeStamp(); // Use last estimated frame's state
+
     }
+
+    if (not ts.ok())
+        WOLF_WARN( "Problem has nowhere to find a valid timestamp!");
+
+    return ts;
 }
 
 
 VectorComposite Problem::getState(const StateStructure& _structure) const
 {
     StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
+
     VectorComposite state;
 
-    if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-    {
-        auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
-        if (last_kf_or_aux)
-            state = last_kf_or_aux->getState(structure);
-        else
-            state = stateZero(structure);
-    }
-    else // Compose from different processor motion
+    // compose the states of all processor motions into one only state
+    for (const auto& prc : processor_is_motion_list_)
     {
-        // compose the states of all processor motions into one only state
-        for (const auto& prc : processor_is_motion_list_)
+        const auto& prc_state = prc->getState();
+        for (const auto& pair_key_vec : prc_state)
         {
-            const auto& prc_state = prc->getState();
-            for (const auto& pair_key_vec : prc_state)
-            {
-                if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
-                    state.insert(pair_key_vec);
-            }
+            if (state.count(pair_key_vec.first) == 0) // only add those keys that do not exist yet
+                state.insert(pair_key_vec);
         }
+    }
+
+    // check for empty blocks and fill them with the last KF, with the prior, or with zeros in the worst case
+
+    VectorComposite state_last;
+
+    const auto& last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
 
-        // check for empty blocks and fill them with zeros
-        for (const auto& ckey : frame_structure_)
+    if (last_kf_or_aux)
+        state_last = last_kf_or_aux->getState(structure);
+
+    else if (prior_options_ and prior_options_->mode != "nothing")
+        state_last = prior_options_->state;
+
+    for (const auto& ckey : structure)
+    {
+        const auto& key = string(1,ckey);
+        if (state.count(key) == 0)
         {
-            const auto& key = string(1,ckey);
-            if (state.count(key) == 0)
+            auto state_last_it = state_last.find(key);
+
+            if (state_last_it != state_last.end())
+                state.emplace(key, state_last_it->second);
+
+            else
                 state.emplace(key, stateZero(key).at(key));
         }
     }
@@ -480,20 +494,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
 {
     StateStructure structure = (_structure == "" ? getFrameStructure() : _structure);
 
-    if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
-    {
-        const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
-        if (last_kf_or_aux)
-        {
-            return last_kf_or_aux->getState(structure);
-        }
-        else
-        {
-            return stateZero(structure);
-        }
-    }
-
     VectorComposite state;
+
     for (const auto& prc : processor_is_motion_list_)
     {
         const auto& prc_state = prc->getState(_ts);
@@ -508,12 +510,31 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
         }
     }
 
-    // check for empty blocks and fill them with zeros
-    for (const auto& ckey : frame_structure_)
+    // check for empty blocks and fill them with the closest KF to ts, with the prior, or with zeros in the worst case
+
+    VectorComposite state_last;
+
+    const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+
+    if (last_kf_or_aux)
+        state_last = last_kf_or_aux->getState(structure);
+
+    else if (prior_options_ and prior_options_->mode != "nothing")
+        state_last = prior_options_->state;
+
+    for (const auto& ckey : structure)
     {
         const auto& key = string(1,ckey);
         if (state.count(key) == 0)
-            state.emplace(key, stateZero(key).at(key));
+        {
+            auto state_last_it = state_last.find(key);
+
+            if (state_last_it != state_last.end())
+                state.emplace(key, state_last_it->second);
+
+            else
+                state.emplace(key, stateZero(key).at(key));
+        }
     }
 
     return state;