Change order in Prb::getState(void and ts)

src/problem/problem.cpp

@@ -448,15 +448,7 @@ 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
+    if (not processor_is_motion_list_.empty() ) // Compose from different processor motion
     {
         // compose the states of all processor motions into one only state
         for (const auto& prc : processor_is_motion_list_)
@@ -478,6 +470,15 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
         }
     }
 
+    else // 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);
+    }
+
     return state;
 }
 
@@ -485,40 +486,45 @@ 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
+    VectorComposite state;
+
+    if (not processor_is_motion_list_.empty() )
     {
-        const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
-        if (last_kf_or_aux)
+        for (const auto& prc : processor_is_motion_list_)
         {
-            return last_kf_or_aux->getState(structure);
+            const auto& prc_state = prc->getState(_ts);
+
+            // transfer processor vector blocks to problem state
+            for (const auto& pair_key_vec : prc_state)
+            {
+                const auto& key = pair_key_vec.first;
+
+                if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
+                    state.insert(pair_key_vec);
+            }
         }
-        else
+
+        // check for empty blocks and fill them with zeros
+        for (const auto& ckey : frame_structure_)
         {
-            return stateZero(structure);
+            const auto& key = string(1,ckey);
+            if (state.count(key) == 0)
+                state.emplace(key, stateZero(key).at(key));
         }
+
     }
 
-    VectorComposite state;
-    for (const auto& prc : processor_is_motion_list_)
+    else // Use last estimated frame's state
     {
-        const auto& prc_state = prc->getState(_ts);
-
-        // transfer processor vector blocks to problem state
-        for (const auto& pair_key_vec : prc_state)
+        const auto& last_kf_or_aux = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
+        if (last_kf_or_aux)
         {
-            const auto& key = pair_key_vec.first;
-
-            if (state.count(key) == 0) // Only write once. This gives priority to processors upfront in the list
-                state.insert(pair_key_vec);
+            state = last_kf_or_aux->getState(structure);
+        }
+        else
+        {
+            state = stateZero(structure);
         }
-    }
-
-    // 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;