From c61cb0979dfb704953afdf69a62f89225e7d6225 Mon Sep 17 00:00:00 2001 From: artivis <deray.jeremie@gmail.com> Date: Mon, 21 May 2018 20:28:41 +0200 Subject: [PATCH] fix test/example new solver api --- src/examples/test_imuDock.cpp | 4 +- src/examples/test_imuDock_autoKFs.cpp | 4 +- src/examples/test_imuPlateform_Offline.cpp | 4 +- src/examples/test_imu_constrained0.cpp | 6 +- src/examples/test_processor_odom_3D.cpp | 2 +- .../test_processor_tracker_landmark_image.cpp | 2 +- src/examples/test_simple_AHP.cpp | 2 +- src/examples/test_sparsification.cpp | 4 +- src/hello_wolf/hello_wolf.cpp | 6 +- src/test/gtest_IMU.cpp | 38 +++++----- src/test/gtest_constraint_absolute.cpp | 6 +- .../gtest_constraint_autodiff_distance_3D.cpp | 2 +- src/test/gtest_constraint_odom_3D.cpp | 4 +- src/test/gtest_constraint_pose_2D.cpp | 2 +- src/test/gtest_constraint_pose_3D.cpp | 2 +- src/test/gtest_odom_2D.cpp | 22 +++--- src/test/gtest_problem.cpp | 14 ++-- src/test/gtest_trajectory.cpp | 76 +++++++++++++++++-- 18 files changed, 132 insertions(+), 68 deletions(-) diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp index 903e5c520..d68e8dd43 100644 --- a/src/examples/test_imuDock.cpp +++ b/src/examples/test_imuDock.cpp @@ -240,8 +240,8 @@ int main(int argc, char** argv) // ___Solve + compute covariances___ problem->print(4,0,1,0); - std::string report = ceres_manager->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager->computeCovariances(ALL_MARGINALS); + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); problem->print(1,0,1,0); //#################################################### RESULTS PART diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp index 53f2fd9bd..03677027f 100644 --- a/src/examples/test_imuDock_autoKFs.cpp +++ b/src/examples/test_imuDock_autoKFs.cpp @@ -249,8 +249,8 @@ int main(int argc, char** argv) // ___Solve + compute covariances___ problem->print(4,0,1,0); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager->computeCovariances(ALL_MARGINALS); + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF); + ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); problem->print(1,0,1,0); //#################################################### RESULTS PART diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp index 644bf155a..9d834d73e 100644 --- a/src/examples/test_imuPlateform_Offline.cpp +++ b/src/examples/test_imuPlateform_Offline.cpp @@ -201,9 +201,9 @@ int main(int argc, char** argv) origin_KF->getVPtr()->fix(); std::cout << "\t\t\t ______solving______" << std::endl; - std::string report = ceres_manager_wolf_diff->solve(2);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport std::cout << report << std::endl; - ceres_manager_wolf_diff->computeCovariances(ALL); + ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); std::cout << "\t\t\t ______solved______" << std::endl; wolf_problem_ptr_->print(4,1,1,1); diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp index 2ee096b4e..87330497c 100644 --- a/src/examples/test_imu_constrained0.cpp +++ b/src/examples/test_imu_constrained0.cpp @@ -259,16 +259,16 @@ int main(int argc, char** argv) std::cout << "\t\t\t ______solving______" << std::endl; - std::string report = ceres_manager_wolf_diff->solve(2); //0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport std::cout << report << std::endl; last_KF->getAccBiasPtr()->fix(); last_KF->getGyroBiasPtr()->fix(); std::cout << "\t\t\t solving after fixBias" << std::endl; - report = ceres_manager_wolf_diff->solve(1); //0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport std::cout << report << std::endl; - ceres_manager_wolf_diff->computeCovariances(ALL); + ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); std::cout << "\t\t\t ______solved______" << std::endl; wolf_problem_ptr_->print(4,1,1,1); diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp index 6cb5e458d..8cfdb9d88 100644 --- a/src/examples/test_processor_odom_3D.cpp +++ b/src/examples/test_processor_odom_3D.cpp @@ -93,7 +93,7 @@ int main (int argc, char** argv) // frm->setState(problem->zeroState()); // } // problem->print(1,0,1,0); - std::string brief_report = ceres_manager.solve(1);// 0, 1 or 2 + std::string brief_report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL); std::cout << brief_report << std::endl; problem->print(1,0,1,0); diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp index b9ea3772d..4060656a2 100644 --- a/src/examples/test_processor_tracker_landmark_image.cpp +++ b/src/examples/test_processor_tracker_landmark_image.cpp @@ -229,7 +229,7 @@ int main(int argc, char** argv) if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs) { number_of_KFs = problem->getTrajectoryPtr()->getFrameList().size(); - std::string summary = ceres_manager.solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport std::cout << summary << std::endl; } diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp index bc2d27c54..0f4d3f8eb 100644 --- a/src/examples/test_simple_AHP.cpp +++ b/src/examples/test_simple_AHP.cpp @@ -242,7 +242,7 @@ int main(int argc, char** argv) CeresManager ceres_manager(problem, ceres_options); - std::string summary = ceres_manager.solve(2);// 0: nothing, 1: BriefReport, 2: FullReport + std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport std::cout << summary << std::endl; // Test of convergence over the lmk params diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp index bf7978690..ea8113d1b 100644 --- a/src/examples/test_sparsification.cpp +++ b/src/examples/test_sparsification.cpp @@ -291,11 +291,11 @@ int main(int argc, char** argv) // SOLVE // solution - bl_summary = bl_ceres_manager->solve(1); + bl_summary = bl_ceres_manager->solve(SolverManager::ReportVerbosity::FULL); std::cout << bl_summary << std::endl; // covariance - bl_ceres_manager->computeCovariances(ALL);//ALL_MARGINALS + bl_ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);//ALL_MARGINALS // t1 = clock(); diff --git a/src/hello_wolf/hello_wolf.cpp b/src/hello_wolf/hello_wolf.cpp index 27eb81987..653fdcc9e 100644 --- a/src/hello_wolf/hello_wolf.cpp +++ b/src/hello_wolf/hello_wolf.cpp @@ -214,7 +214,7 @@ int main() // SOLVE with exact initial guess WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======") - std::string report = ceres->solve(2); + std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); WOLF_TRACE(report); // should show a very low iteration number (possibly 1) problem->print(4,1,1,1); @@ -232,13 +232,13 @@ int main() // SOLVE again WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") - report = ceres->solve(2); + report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!) problem->print(4,1,1,1); // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") - ceres->computeCovariances(ALL_MARGINALS); + ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); for (auto kf : problem->getTrajectoryPtr()->getFrameList()) if (kf->isKey()) WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance()); diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp index 0a23dff8c..38dfbc223 100644 --- a/src/test/gtest_IMU.cpp +++ b/src/test/gtest_IMU.cpp @@ -521,7 +521,7 @@ class Process_Constraint_IMU : public testing::Test - string solveProblem(int verbose = 1) + string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF) { string report = ceres_manager->solve(verbose); @@ -544,7 +544,7 @@ class Process_Constraint_IMU : public testing::Test - string runAll(int verbose) + string runAll(SolverManager::ReportVerbosity verbose) { configureAll(); integrateAll(); @@ -729,7 +729,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -822,7 +822,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -868,7 +868,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -914,7 +914,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -960,7 +960,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1005,7 +1005,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1050,7 +1050,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1095,7 +1095,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1140,7 +1140,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1185,7 +1185,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1230,7 +1230,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1275,7 +1275,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1320,7 +1320,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1365,7 +1365,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1410,7 +1410,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1455,7 +1455,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim // ===================================== RUN ALL - string report = runAll(1); + string report = runAll(SolverManager::ReportVerbosity::BRIEF); // printAll(report); @@ -1503,7 +1503,7 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) configureAll(); integrateAllTrajectories(); buildProblem(); - string report = solveProblem(1); + string report = solveProblem(SolverManager::ReportVerbosity::BRIEF); assertAll(); diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp index 52adcd099..80a6579ae 100644 --- a/src/test/gtest_constraint_absolute.cpp +++ b/src/test/gtest_constraint_absolute.cpp @@ -70,7 +70,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(1); + std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only orientation is constrained ASSERT_MATRIX_APPROX(frm0->getState().head<3>(), pose10.head<3>(), 1e-6); @@ -97,7 +97,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(1); + std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only velocity is constrained ASSERT_MATRIX_APPROX(frm0->getState().tail<3>(), pose10.tail<3>(), 1e-6); @@ -124,7 +124,7 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(1); + std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF); //only velocity is constrained ASSERT_MATRIX_APPROX(frm0->getState().segment<4>(3), pose10.segment<4>(3), 1e-6); diff --git a/src/test/gtest_constraint_autodiff_distance_3D.cpp b/src/test/gtest_constraint_autodiff_distance_3D.cpp index 768be80e9..bc6e402e4 100644 --- a/src/test/gtest_constraint_autodiff_distance_3D.cpp +++ b/src/test/gtest_constraint_autodiff_distance_3D.cpp @@ -98,7 +98,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, solve) Scalar measurement = 1.400; f2->setMeasurement(Vector1s(measurement)); - std::string report = ceres_manager->solve(2); + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // Check distance between F1 and F2 positions -- must match the measurement ASSERT_NEAR( (F1->getPPtr()->getState() - F2->getPPtr()->getState()).norm(), measurement, 1e-10); diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp index 1fba7cd1d..f3b9e0cca 100644 --- a/src/test/gtest_constraint_odom_3D.cpp +++ b/src/test/gtest_constraint_odom_3D.cpp @@ -69,7 +69,7 @@ TEST(ConstraintOdom3D, fix_0_solve) frm1->setState(x1); // solve for frm1 - std::string report = ceres_mgr.solve(1); + std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(frm1->getState(), delta, 1e-6); @@ -83,7 +83,7 @@ TEST(ConstraintOdom3D, fix_1_solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(1); + std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::BRIEF); ASSERT_MATRIX_APPROX(frm0->getState(), problem->zeroState(), 1e-6); } diff --git a/src/test/gtest_constraint_pose_2D.cpp b/src/test/gtest_constraint_pose_2D.cpp index 220ead9cf..c98de12b0 100644 --- a/src/test/gtest_constraint_pose_2D.cpp +++ b/src/test/gtest_constraint_pose_2D.cpp @@ -59,7 +59,7 @@ TEST(ConstraintPose2D, solve) frm0->setState(x0); // solve for frm0 - std::string report = ceres_mgr.solve(0); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_mgr.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport ASSERT_MATRIX_APPROX(frm0->getState(), pose, 1e-6); diff --git a/src/test/gtest_constraint_pose_3D.cpp b/src/test/gtest_constraint_pose_3D.cpp index a22d361c8..77604701a 100644 --- a/src/test/gtest_constraint_pose_3D.cpp +++ b/src/test/gtest_constraint_pose_3D.cpp @@ -67,7 +67,7 @@ TEST(ConstraintPose3D, solve) frm0->setState(x0); // solve for frm0 - std::string brief_report = ceres_mgr.solve(1); + std::string brief_report = ceres_mgr.solve(SolverManager::ReportVerbosity::FULL); ASSERT_MATRIX_APPROX(frm0->getState(), pose7, 1e-6); diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp index bd101f240..b8bb7388c 100644 --- a/src/test/gtest_odom_2D.cpp +++ b/src/test/gtest_odom_2D.cpp @@ -153,10 +153,10 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D) F0->setState(Vector3s(1,2,3)); F1->setState(Vector3s(2,3,1)); F2->setState(Vector3s(3,1,2)); - std::string report = ceres_manager.solve(1); + std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL); // std::cout << report << std::endl; - ceres_manager.computeCovariances(ALL_MARGINALS); + ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // show(Pr); Matrix3s P1, P2; @@ -211,8 +211,8 @@ TEST(Odom2D, VoteForKfAndSolve) // Origin Key Frame FrameBasePtr origin_frame = problem->setPrior(x0, P0, t0, dt/2); - ceres_manager.solve(0); - ceres_manager.computeCovariances(ALL_MARGINALS); + ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); + ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // std::cout << "Initial pose : " << problem->getCurrentState().transpose() << std::endl; // std::cout << "Initial covariance : " << std::endl << problem->getLastKeyFrameCovariance() << std::endl; @@ -280,9 +280,9 @@ TEST(Odom2D, VoteForKfAndSolve) } // Solve - std::string report = ceres_manager.solve(1); + std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); // std::cout << report << std::endl; - ceres_manager.computeCovariances(ALL_MARGINALS); + ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector[5], 1e-6); } @@ -402,9 +402,9 @@ TEST(Odom2D, KF_callback) MotionBuffer key_buffer_n = key_capture_n->getBuffer(); - std::string report = ceres_manager.solve(1); + std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); // std::cout << report << std::endl; - ceres_manager.computeCovariances(ALL_MARGINALS); + ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6); ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6); @@ -437,8 +437,8 @@ TEST(Odom2D, KF_callback) keyframe_1->setState(Vector3s(2,3,1)); keyframe_2->setState(Vector3s(3,1,2)); - report = ceres_manager.solve(1); - ceres_manager.computeCovariances(ALL_MARGINALS); + report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF); + ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); // check the split KF ASSERT_POSE2D_APPROX(keyframe_1->getState() , integrated_pose_vector[m_split], 1e-6); @@ -455,7 +455,7 @@ TEST(Odom2D, KF_callback) t += dt; // WOLF_DEBUG(" estimated(", t, ") = ", problem->getState(t).transpose()); // WOLF_DEBUG("ground truth(", t, ") = ", integrated_pose_vector[n].transpose()); - ASSERT_MATRIX_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6); + EXPECT_POSE2D_APPROX(problem->getState(t), integrated_pose_vector[n], 1e-6); } } diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp index 1d0a26ae8..420e9b271 100644 --- a/src/test/gtest_problem.cpp +++ b/src/test/gtest_problem.cpp @@ -79,7 +79,7 @@ TEST(Problem, Installers) SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test) - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10)); + ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10); S->addProcessor(pt); // check motion processor IS NOT set @@ -210,21 +210,21 @@ TEST(Problem, StateBlocks) // 2 state blocks, fixed SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); ASSERT_EQ(P->getStateBlockList().size(), 2); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2); // 3 state blocks, fixed SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); ASSERT_EQ(P->getStateBlockList().size(), 2 + 3); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3); - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10)); + ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10); St->addProcessor(pt); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); // 2 state blocks, estimated P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0); ASSERT_EQ(P->getStateBlockList().size(), 2 + 3 + 2); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3 + 2); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3 + 2); // P->print(4,1,1,1); @@ -232,7 +232,7 @@ TEST(Problem, StateBlocks) // change some SB properties St->unfixExtrinsics(); ASSERT_EQ(P->getStateBlockList().size(), 2 + 3 + 2); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3 + 2 + 2); // XXX: 2 more notifications on the same SB! + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3 + 2 /*+ 2*/); // XXX: 2 more notifications on the same SB! // P->print(4,1,1,1); } @@ -246,7 +246,7 @@ TEST(Problem, Covariances) SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml"); SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml"); - ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(ProcessorTrackerFeatureDummy(0.1, 5, 10)); + ProcessorBasePtr pt = std::make_shared<ProcessorTrackerFeatureDummy>(0.1, 5, 10); St->addProcessor(pt); ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml"); diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp index f6835809e..0c1b2cdf7 100644 --- a/src/test/gtest_trajectory.cpp +++ b/src/test/gtest_trajectory.cpp @@ -17,6 +17,63 @@ using namespace wolf; +struct DummyNotificationProcessor +{ + DummyNotificationProcessor(ProblemPtr _problem) + : problem_(_problem) + { + // + } + + void update() + { + if (problem_ == nullptr) + { + FAIL() << "problem_ is nullptr !"; + } + + StateBlockList& states = problem_->getNotifiedStateBlockList(); + + for (StateBlockPtr& state : states) + { + const auto notifications = state->consumeNotifications(); + + for (const auto notif : notifications) + { + switch (notif) + { + case StateBlock::Notification::ADD: + { + break; + } + case StateBlock::Notification::STATE_UPDATE: + { + break; + } + case StateBlock::Notification::FIX_UPDATE: + { + break; + } + case StateBlock::Notification::REMOVE: + { + break; + } + default: + throw std::runtime_error("SolverManager::update: State Block notification " + "must be ADD, STATE_UPDATE, FIX_UPDATE, REMOVE or ENABLED."); + } + } + + ASSERT_FALSE(state->hasNotifications()); + } + + states.clear(); + } + + ProblemPtr problem_; +}; + + /// Set to true if you want debug info bool debug = false; @@ -63,6 +120,8 @@ TEST(TrajectoryBase, Add_Remove_Frame) ProblemPtr P = Problem::create("PO 2D"); TrajectoryBasePtr T = P->getTrajectoryPtr(); + DummyNotificationProcessor N(P); + // Trajectory status: // kf1 kf2 f3 frames // 1 2 3 time stamps @@ -77,29 +136,31 @@ TEST(TrajectoryBase, Add_Remove_Frame) if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), 1); ASSERT_EQ(P->getStateBlockList(). size(), 2); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2); T->addFrame(f2); // KF if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), 2); ASSERT_EQ(P->getStateBlockList(). size(), 4); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 4); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4); T->addFrame(f3); // F if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), 3); ASSERT_EQ(P->getStateBlockList(). size(), 4); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 4); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4); ASSERT_EQ(T->getLastFramePtr()->id(), f3->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id()); + N.update(); + // remove frames and keyframes f2->remove(); // KF if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), 2); ASSERT_EQ(P->getStateBlockList(). size(), 2); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2); ASSERT_EQ(T->getLastFramePtr()->id(), f3->id()); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id()); @@ -108,7 +169,7 @@ TEST(TrajectoryBase, Add_Remove_Frame) if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), 1); ASSERT_EQ(P->getStateBlockList(). size(), 2); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 2); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2); ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id()); @@ -116,8 +177,11 @@ TEST(TrajectoryBase, Add_Remove_Frame) if (debug) P->print(2,0,0,0); ASSERT_EQ(T->getFrameList(). size(), 0); ASSERT_EQ(P->getStateBlockList(). size(), 0); - ASSERT_EQ(P->getStateBlockNotificationList().size(), 0); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4); + + N.update(); + ASSERT_EQ(P->getNotifiedStateBlockList().size(), 0); } TEST(TrajectoryBase, KeyFramesAreSorted) -- GitLab