wip

test/gtest_factor_imu2d_with_gravity.cpp

@@ -108,8 +108,8 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f1)
     x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
     // Set states
-    frm0->setState(x0);
-    frm1->setState(x1);
+    frm0->setState(x0,"POV");
+    frm1->setState(x1,"POV");
     sensor->getStateBlock('G')->setState(g);
 
     // feature & factor with delta measurement
@@ -157,8 +157,8 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
     x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
 
     // Set states
-    frm0->setState(x0);
-    frm1->setState(x1);
+    frm0->setState(x0, "POV");
+    frm1->setState(x1, "POV");
     sensor->getStateBlock('G')->setState(g);
 
     // feature & factor with delta measurement
@@ -174,9 +174,9 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
     frm0->perturb(0.01);
 
     // solve 
-     std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+     std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
-    ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+    ASSERT_POSE2d_APPROX(frm0->getStateVector("POV"), x0, 1e-6);
     //WOLF_INFO(frm1->getStateVector());
 
     // remove feature (and factor) for the next loop
@@ -184,7 +184,7 @@ TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
 }
 }
 
-TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
+TEST_F(FactorImu2dWithGravity, bias_zero_solve_b1)
 {
   for(int i = 0; i < N_TESTS; i++){
     // Random delta
@@ -226,7 +226,7 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
     cap1->perturb(0.01);
 
     // solve  to estimate bias at cap1
-    std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
 
     ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
     //WOLF_INFO(cap1->getStateVector());
@@ -236,7 +236,7 @@ TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
   }
 }
 
-TEST(FactorImu2dWithGravity, solve_b0)
+TEST_F(FactorImu2dWithGravity, solve_b0)
 {
   for(int i = 0; i < 50; i++){
     // Random delta