diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index b220bbedf106e0bf6c1912ac0abffe28725ae2fc..f283eb6dc5f9270e03c4a5bbc8dc210f20124d8d 100644 --- a/test/gtest_factor_gnss_fix_2d.cpp +++ b/test/gtest_factor_gnss_fix_2d.cpp @@ -5,10 +5,10 @@ * \author: jvallve */ +#include <core/ceres_wrapper/solver_ceres.h> #include <core/utils/utils_gtest.h> #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "gnss/sensor/sensor_gnss.h" #include "gnss/processor/processor_gnss_fix.h" #include "gnss/capture/capture_gnss_fix.h" @@ -50,23 +50,23 @@ void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr) frame_ptr->getO()->fix(); } -void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_reduced, int& num_param_blocks_reduced ) +void computeParamSizes(const SolverCeresPtr& solver_ceres, int& num_params_reduced, int& num_param_blocks_reduced ) { num_param_blocks_reduced = 0; num_params_reduced = 0; std::vector<double*> param_blocks; - ceres_mgr_ptr->getCeresProblem()->GetParameterBlocks(¶m_blocks); + solver_ceres->getCeresProblem()->GetParameterBlocks(¶m_blocks); for (auto pb : param_blocks) { std::vector<ceres::ResidualBlockId> residual_blocks; - ceres_mgr_ptr->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks); + solver_ceres->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks); - if (!ceres_mgr_ptr->getCeresProblem()->IsParameterBlockConstant(pb) && !residual_blocks.empty()) + if (!solver_ceres->getCeresProblem()->IsParameterBlockConstant(pb) && !residual_blocks.empty()) { num_param_blocks_reduced ++; - num_params_reduced += ceres_mgr_ptr->getCeresProblem()->ParameterBlockLocalSize(pb); + num_params_reduced += solver_ceres->getCeresProblem()->ParameterBlockLocalSize(pb); } } } @@ -87,7 +87,7 @@ Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + // WOLF ProblemPtr problem_ptr = Problem::create("PO", 2); -CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr); +SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr); SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>())); FrameBasePtr frame_ptr; @@ -95,7 +95,7 @@ FrameBasePtr frame_ptr; TEST(FactorGnssFix2dTest, configure_tree) { - ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100; + solver_ceres->getSolverOptions().max_num_iterations = 100; // Configure sensor and processor gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map); @@ -146,25 +146,25 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) frame_ptr->setState(frm_dist, "PO", {2,1}); // --------------------------- update solver - ceres_mgr_ptr->update(); + solver_ceres->update(); // --------------------------- check problem parameters int num_params_reduced, num_param_blocks_reduced; - computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced); + computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); ASSERT_EQ(num_param_blocks_reduced, 1); ASSERT_EQ(num_params_reduced, 2); // --------------------------- solve - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL); //std::cout << report << std::endl; // --------------------------- check summary parameters & residuals - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 2); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2); + ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution ASSERT_MATRIX_APPROX(frame_ptr->getState().at("P"), t_map_base.head(2), 1e-6); @@ -191,23 +191,23 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation) frame_ptr->getO()->setState(frm_dist); // --------------------------- update solver - ceres_mgr_ptr->update(); + solver_ceres->update(); // --------------------------- check problem parameters int num_params_reduced, num_param_blocks_reduced; - computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced); + computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); ASSERT_EQ(num_param_blocks_reduced, 1); ASSERT_EQ(num_params_reduced, 1); // --------------------------- solve - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution ASSERT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6); @@ -234,23 +234,23 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw) gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map_dist); // --------------------------- update solver - ceres_mgr_ptr->update(); + solver_ceres->update(); // --------------------------- check problem parameters int num_params_reduced, num_param_blocks_reduced; - computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced); + computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); ASSERT_EQ(num_param_blocks_reduced, 1); ASSERT_EQ(num_params_reduced, 1); // --------------------------- solve - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6); @@ -278,23 +278,23 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_position) gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map_dist); // --------------------------- update solver - ceres_mgr_ptr->update(); + solver_ceres->update(); // --------------------------- check problem parameters int num_params_reduced, num_param_blocks_reduced; - computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced); + computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); ASSERT_EQ(num_param_blocks_reduced, 1); ASSERT_EQ(num_params_reduced, 3); // --------------------------- solve - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6); @@ -322,23 +322,23 @@ TEST(FactorGnssFix2dTest, gnss_1_base_antena) gnss_sensor_ptr->getP()->setState(base_antena_dist); // --------------------------- update solver - ceres_mgr_ptr->update(); + solver_ceres->update(); // --------------------------- check problem parameters int num_params_reduced, num_param_blocks_reduced; - computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced); + computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced); ASSERT_EQ(num_param_blocks_reduced, 1); ASSERT_EQ(num_params_reduced, 3); // --------------------------- solve - std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET); + std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET); // --------------------------- check summary parameters & residuals - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1); - ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3); + ASSERT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1); + ASSERT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6); diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index bc88eb28b90ac53eeb715188cb4a1f0efd6d01e3..402608ab90c694826ed3fb55612d34f53b04ded8 100644 --- a/test/gtest_factor_gnss_pseudo_range.cpp +++ b/test/gtest_factor_gnss_pseudo_range.cpp @@ -1,7 +1,7 @@ +#include <core/ceres_wrapper/solver_ceres.h> #include <core/utils/utils_gtest.h> #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "gnss/sensor/sensor_gnss.h" #include "gnss/capture/capture_gnss.h" #include "gnss/factor/factor_gnss_pseudo_range.h" @@ -28,7 +28,7 @@ GnssUtils::Range range1, range2, range3, range4; // WOLF ProblemPtr prb = Problem::create("PO", 3); -CeresManagerPtr solver = std::make_shared<CeresManager>(prb); +SolverCeresPtr solver = std::make_shared<SolverCeres>(prb); SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>())); FrameBasePtr frm; CaptureGnssPtr cap; diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp index aa2d3d47f22214a91e872bc5c86dd3e0cdfd2268..98e8b75f43b192c9ed9ebdce037c989a401631ae 100644 --- a/test/gtest_factor_gnss_tdcp.cpp +++ b/test/gtest_factor_gnss_tdcp.cpp @@ -1,3 +1,5 @@ +#include <core/ceres_wrapper/solver_ceres.h> + #include "gnss/factor/factor_gnss_tdcp.h" #include <core/utils/utils_gtest.h> @@ -5,7 +7,6 @@ #include "gnss/sensor/sensor_gnss.h" #include "gnss/capture/capture_gnss.h" -#include "core/ceres_wrapper/ceres_manager.h" using namespace Eigen; using namespace wolf; @@ -32,7 +33,7 @@ GnssUtils::Range range1_r, range2_r, range3_r, range4_r, range1_k, range2_k, ran // WOLF ProblemPtr prb = Problem::create("PO", 3); -CeresManagerPtr solver = std::make_shared<CeresManager>(prb); +SolverCeresPtr solver = std::make_shared<SolverCeres>(prb); SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>())); FrameBasePtr frm_r, frm_k; CaptureGnssPtr cap_r, cap_k;