diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 3790e5ba3936dfbc1f3dde58ff4e2240a513742a..8ec6ebcae42990451909902ee8f245c35fbf16c4 100644
--- a/test/gtest_problem.cpp
+++ b/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);