diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp index 903e5c5201658143e776ba998180f7e5aaaffd2e..d68e8dd4391d9733c81c55c8f78d821f140fe700 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 53f2fd9bdc893a90698293d383760ac9b5c9621d..03677027f7dc687d4e55a303ccbbefe2030a4c3a 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 644bf155a9d0efc19b7e2221933253a1feafd278..9d834d73e8408b1207a8f48d9225b4ec2f08d989 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 2ee096b4e26cbab0aaa44ff31ec9f7ed972ae4a6..87330497c75beb1e0464382fd4ec1437bc848272 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 6cb5e458dd92f40abfb37d26aa601c0242c8f510..8cfdb9d8822347057fe30751058f253b5ea340b9 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 b9ea3772d7d93882e475263e48903a3d884ed1a2..4060656a2ac762d4bb2ac4904f047cf9ccccd163 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 bc2d27c54fff6bf8aede8e0d127664bde1a662b5..0f4d3f8eb8d7af228db27d3535c808909a780356 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 bf797869098964ffe95683a7155e8657d6622b4f..ea8113d1bccfc185a9ea69fedbd90f0967969922 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 27eb81987b9918e570c7a94420581f18b87b1651..653fdcc9ebd696d9774a97a60cb66d0016500227 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 0a23dff8cd4b85df626891719b345532dffc515d..38dfbc223a2e3137c503eef65ff343e379265365 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 52adcd099403218395990b69ed857da62259d507..80a6579ae459d7932e7c0e4df9b95e48a76ce326 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 768be80e93b39146ebc42a91b63f7b3011e42066..bc6e402e423c2f6531a94591a1add61733872607 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 1fba7cd1dd83b88824adcfb7c623be64ce34589c..f3b9e0cca1770a6b06d6f2a51c408d54d982a69b 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 220ead9cfdd33baddcc525966890ff82b0b73754..c98de12b0f2dd05628ff905b8877e40daa80ba16 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 a22d361c842b21e80c2a63dcae5d04f621fc744d..77604701ad57fc9851169a599822d10e6beb7c77 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 bd101f24074cf8fc426a8551bcce85e1bd94e97a..b8bb7388cad12f319a37e053d747df66ce0b63a5 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 1d0a26ae8e027befecc08dfc62e46a36abe516e6..420e9b271a16e70c41bd72df083f9ae6137cf5eb 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 f6835809e5074b2bcf30c789c8e38aaff0e268bb..0c1b2cdf7597f22c042c3c09ad61a77943cfdcaf 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)