Merge branch '287-quad-introduce-vector-composites-for-states-API' of...

Merge branch '287-quad-introduce-vector-composites-for-states-API' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf into 287-quad-introduce-vector-composites-for-states-API

test/gtest_problem.cpp

@@ -125,8 +125,8 @@ TEST(Problem, SetOrigin_PO_2d)
     TrajectoryBasePtr T = P->getTrajectory();
     FrameBasePtr F = P->getLastFrame();
     CaptureBasePtr C = F->getCaptureList().front();
-    FeatureBasePtr fo = C->getFeatureList().front();
-    FeatureBasePtr fp = C->getFeatureList().back();
+    FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+    FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
     FactorBasePtrList fac_list;
     F->getFactorList(fac_list);
 
@@ -182,8 +182,10 @@ TEST(Problem, SetOrigin_PO_3d)
     TrajectoryBasePtr T = P->getTrajectory();
     FrameBasePtr F = P->getLastFrame();
     CaptureBasePtr C = F->getCaptureList().front();
-    FeatureBasePtr fo = C->getFeatureList().front();
-    FeatureBasePtr fp = C->getFeatureList().back();
+    // FeatureBasePtr fo = C->getFeatureList().front();
+    // FeatureBasePtr fp = C->getFeatureList().back();
+    FeatureBasePtr fo = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior O";}));
+    FeatureBasePtr fp = (*std::find_if(C->getFeatureList().begin(), C->getFeatureList().end(), [](FeatureBasePtr ftr){ return ftr->getType() == "Prior P";}));
     FactorBasePtrList fac_list;
     F->getFactorList(fac_list);