Fix PM::getState(structure)

src/processor/processor_motion.cpp

@@ -491,11 +491,16 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
 
     else
     {
-        // remove states not requested
-        for (const auto& pair_key_vec : state)
+        // remove states not requested by structure
+        auto pair_key_vec_it = state.begin();
+        while (pair_key_vec_it != state.end())
         {
-            if (_structure.find(pair_key_vec.first) == std::string::npos)
-                state.erase(pair_key_vec.first);
+            if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+                pair_key_vec_it = state.erase(pair_key_vec_it);
+
+            else
+                pair_key_vec_it ++;
+
         }
         return state;
     }
@@ -581,11 +586,16 @@ VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateStruc
 
         else
         {
-            // remove states not requested
-            for (const auto& pair_key_vec : state)
+            // remove states not requested by structure
+            auto pair_key_vec_it = state.begin();
+            while (pair_key_vec_it != state.end())
             {
-                if (_structure.find(pair_key_vec.first) == std::string::npos)
-                    state.erase(pair_key_vec.first);
+                if (_structure.find(pair_key_vec_it->first) == std::string::npos)
+                    pair_key_vec_it = state.erase(pair_key_vec_it);
+
+                else
+                    pair_key_vec_it ++;
+
             }
             return state;
         }

test/gtest_processor_motion.cpp

@@ -101,6 +101,36 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
     ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
 }
 
+TEST_F(ProcessorMotion_test, getState_structure)
+{
+    // Prior
+    Vector3d x0; x0 << 0, 0, 0;
+    Matrix3d P0; P0.setIdentity();
+
+    data << 1, 0; // advance straight
+    data_cov.setIdentity();
+    TimeStamp t(0.0);
+
+    for (int i = 0; i<9; i++)
+    {
+        t += dt;
+        capture->setTimeStamp(t);
+        capture->setData(data);
+        capture->setDataCovariance(data_cov);
+        processor->captureCallback(capture);
+        WOLF_DEBUG("t: ", t, "  x: ", problem->getState().vector("PO").transpose());
+    }
+
+    ASSERT_TRUE (processor->getState("P").count("P"));
+    ASSERT_FALSE(processor->getState("P").count("O"));
+    ASSERT_FALSE(processor->getState("O").count("P"));
+    ASSERT_TRUE (processor->getState("O").count("O"));
+
+    WOLF_DEBUG("processor->getState(\"V\") = ", processor->getState("V"));
+    ASSERT_EQ   (processor->getState("V").size(), 0);
+}
+
+
 
 TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
 {