diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index eef69b049765ca3b4c37e76abfdfae3d4e79ed5e..b1e8f781fe2d50604e6b5a5f605f8b78ee1cd888 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -254,8 +254,6 @@ TEST_F(FactorDiffDriveTest, constructor) ASSERT_EQ(c1->getType(), "DIFF DRIVE"); ASSERT_EQ(c1->getCaptureOther()->getSensorIntrinsic(), sensor->getIntrinsic()); - problem->print(4,1,1,1); - } TEST_F(FactorDiffDriveTest, residual_zeros) @@ -329,7 +327,6 @@ TEST_F(FactorDiffDriveTest, solve_F1) sensor->fixIntrinsics(); F1->setState(F1->getState() + Vector3s::Random() * 0.1); - problem->print(1,0,1,1); std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // WOLF_TRACE("\n", report); @@ -369,7 +366,6 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics) sensor->unfixIntrinsics(); sensor->getIntrinsic()->setState(calib + Vector3s::Random() * 0.1); - problem->print(1,0,1,1); std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); // WOLF_TRACE("\n", report); @@ -480,13 +476,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) C->process(); } - for (t = 0; t < (2*N+1)*dt; t += dt) - { - WOLF_TRACE("x(", t.getSeconds(), ") = ", problem->getState(t).transpose()); - } - - - auto F2 = problem->getLastKeyFrame(); // this matches the end of the arc precisely so it may be used for checks. + auto F2 = problem->getLastKeyFrame(); // Fix boundaries and unfix S ; @@ -502,10 +492,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); WOLF_TRACE("\n", report); - for (t = 0; t < (2*N+1)*dt; t += dt) - { - WOLF_TRACE("x(", t.getSeconds(), ") = ", problem->getState(t).transpose()); - } WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose()); WOLF_TRACE("x2 : ", F2->getState().transpose()); WOLF_TRACE("calib_preint : ", calib_preint.transpose()); @@ -639,10 +625,6 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); WOLF_TRACE("\n", report); - for (t = 0; t < (2*N+1)*dt; t += dt) - { - WOLF_TRACE("x(", t.getSeconds(), ") = ", problem->getState(t).transpose()); - } WOLF_TRACE("x1 : ", problem->getState(N*dt).transpose()); WOLF_TRACE("x2 : ", F2->getState().transpose()); WOLF_TRACE("calib_preint : ", calib_preint.transpose());