diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp index 913932a305c57780ac2a5499d0b0d2c0e2b2c83b..117e5459568041f9e77a4e1ccab6b822d3d13fd5 100644 --- a/demos/demo_factor_imu.cpp +++ b/demos/demo_factor_imu.cpp @@ -1,4 +1,6 @@ //Wolf +#include <core/ceres_wrapper/solver_ceres.h> + #include "core/capture/capture_imu.h" #include "core/processor/processor_imu.h" #include "core/sensor/sensor_imu.h" @@ -8,7 +10,6 @@ #include "core/factor/factor_odom_3D.h" #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" -#include "core/ceres_wrapper/ceres_manager.h" //#define DEBUG_RESULTS @@ -34,7 +35,7 @@ int main(int argc, char** argv) ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); + SolverCeres* ceres_manager_wolf_diff = new SolverCeres(wolf_problem_ptr_, ceres_options, true); // Time and data variables TimeStamp t; diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp index 73f693e491acf81eb4020cfc99099f66f09ed94e..e218a88cbf8f48b7537954f52d02ce7c7ee957f1 100644 --- a/demos/demo_imuDock.cpp +++ b/demos/demo_imuDock.cpp @@ -5,11 +5,12 @@ * \author: Dinesh Atchuthan */ +#include <core/ceres_wrapper/solver_ceres.h> + #include "core/processor/processor_imu.h" #include "core/sensor/sensor_imu.h" #include "core/common/wolf.h" #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/sensor/sensor_odom_3D.h" #include "core/processor/processor_odom_3D.h" @@ -102,7 +103,7 @@ int main(int argc, char** argv) // ___Create the WOLF Problem + define origin of the problem___ ProblemPtr problem = Problem::create("PQVBB 3D"); - CeresManager* ceres_manager = new CeresManager(problem); + SolverCeres* ceres_manager = new SolverCeres(problem); // ___Configure Ceres if needed___ diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp index cfd482b6faea95d6bd606c04fdc1e9370ba9a837..d5bd9aa8bc357f6be1c5135d8c1ad7730c4858da 100644 --- a/demos/demo_imuDock_autoKFs.cpp +++ b/demos/demo_imuDock_autoKFs.cpp @@ -5,11 +5,12 @@ * \author: Dinesh Atchuthan */ +#include <core/ceres_wrapper/solver_ceres.h> + #include "core/processor/processor_imu.h" #include "core/sensor/sensor_imu.h" #include "core/common/wolf.h" #include "core/problem/problem.h" -#include "core/ceres_wrapper/ceres_manager.h" #include "core/sensor/sensor_odom_3D.h" #include "core/processor/processor_odom_3D.h" @@ -111,7 +112,7 @@ int main(int argc, char** argv) // ___Create the WOLF Problem + define origin of the problem___ ProblemPtr problem = Problem::create("PQVBB 3D"); - CeresManager* ceres_manager = new CeresManager(problem); + SolverCeres* ceres_manager = new SolverCeres(problem); // ___Configure Ceres if needed___ diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp index a3565850e74ecc27f58b0e47248813d75fe98bb3..9006e6aa7619117c895fd5bc9cffbbf612f7c7ae 100644 --- a/demos/demo_imuPlateform_Offline.cpp +++ b/demos/demo_imuPlateform_Offline.cpp @@ -1,4 +1,6 @@ //Wolf +#include <core/ceres_wrapper/solver_ceres.h> + #include "core/capture/capture_imu.h" #include "core/processor/processor_imu.h" #include "core/sensor/sensor_imu.h" @@ -9,9 +11,6 @@ #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" #include "core/processor/processor_odom_3D.h" -#include "core/ceres_wrapper/ceres_manager.h" - -//std #include <iostream> #include <fstream> #include <iomanip> @@ -79,7 +78,7 @@ int main(int argc, char** argv) ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); + SolverCeres* ceres_manager_wolf_diff = new SolverCeres(wolf_problem_ptr_, ceres_options); // SENSOR + PROCESSOR IMU SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp index 3f6a245ddec51b5f9f5f7ae043099c2e49021d1d..0512a8ac41b9af5bb1fd83a1147ba2539f336462 100644 --- a/demos/demo_imu_constrained0.cpp +++ b/demos/demo_imu_constrained0.cpp @@ -1,4 +1,6 @@ //Wolf +#include <core/ceres_wrapper/solver_ceres.h> + #include "core/capture/capture_imu.h" #include "core/processor/processor_imu.h" #include "core/sensor/sensor_imu.h" @@ -9,9 +11,6 @@ #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" #include "core/processor/processor_odom_3D.h" -#include "core/ceres_wrapper/ceres_manager.h" - -//std #include <iostream> #include <fstream> #include <iomanip> @@ -100,7 +99,7 @@ int main(int argc, char** argv) ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; - CeresManager* ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); + SolverCeres* ceres_manager_wolf_diff = new SolverCeres(wolf_problem_ptr_, ceres_options); // SENSOR + PROCESSOR IMU SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp index 04f4ef9fc8f02bc2a4006acdaf7e757d72523507..f53f9af38ff6da00ba0f57d815d4b3a336267e9e 100644 --- a/test/gtest_factor_imu.cpp +++ b/test/gtest_factor_imu.cpp @@ -5,6 +5,7 @@ * \author: Dinesh Atchuthan */ +#include <core/ceres_wrapper/solver_ceres.h> #include <core/utils/utils_gtest.h> #include <core/utils/logging.h> @@ -17,9 +18,6 @@ //Wolf #include <core/factor/factor_pose_3d.h> #include <core/processor/processor_odom_3d.h> -#include <core/ceres_wrapper/ceres_manager.h> - -// debug #include <iostream> #include <fstream> @@ -43,7 +41,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr KF0; @@ -66,11 +64,10 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -132,7 +129,7 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr KF0; @@ -154,11 +151,10 @@ class FactorImu_biasTest_Static_NonNullAccBias : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -216,7 +212,7 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -238,11 +234,10 @@ class FactorImu_biasTest_Static_NonNullGyroBias : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; -// ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH -// ceres_options.max_line_search_step_contraction = 1e-3; -// ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + //solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + //solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + //solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -300,7 +295,7 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -322,11 +317,10 @@ class FactorImu_biasTest_Static_NonNullBias : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -386,7 +380,7 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -408,11 +402,10 @@ class FactorImu_biasTest_Move_NullBias : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -473,7 +466,7 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -495,11 +488,10 @@ class FactorImu_biasTest_Move_NonNullBias : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -556,7 +548,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -578,11 +570,10 @@ class FactorImu_biasTest_Move_NonNullBiasRotCst : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -649,7 +640,7 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -671,11 +662,10 @@ class FactorImu_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -750,7 +740,7 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test wolf::TimeStamp t; SensorImuPtr sen_imu; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; FrameBasePtr origin_KF; @@ -772,11 +762,10 @@ class FactorImu_biasTest_Move_NonNullBiasRot : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -846,7 +835,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test public: wolf::TimeStamp t; ProblemPtr problem; - CeresManagerPtr ceres_manager; + SolverCeresPtr ceres_manager; SensorBasePtr sensor; SensorImuPtr sensor_imu; SensorOdom3dPtr sensor_odo; @@ -874,9 +863,8 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; - ceres_manager = std::make_shared<CeresManager>(problem, ceres_options); + ceres_manager = std::make_shared<SolverCeres>(problem); + ceres_manager->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH // SENSOR + PROCESSOR Imu sensor = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -1032,7 +1020,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test SensorImuPtr sen_imu; SensorOdom3dPtr sen_odom3d; ProblemPtr problem; - CeresManager* ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor_; ProcessorImuPtr processor_imu; ProcessorOdom3dPtr processor_odom3d; @@ -1055,11 +1043,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -1173,7 +1160,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test SensorImuPtr sen_imu; SensorOdom3dPtr sen_odom3d; ProblemPtr problem; - CeresManagerPtr ceres_manager_wolf_diff; + SolverCeresPtr solver_wolf_diff; ProcessorBasePtr processor; ProcessorImuPtr processor_imu; ProcessorOdom3dPtr processor_odom3d; @@ -1197,11 +1184,10 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; - ceres_options.max_line_search_step_contraction = 1e-3; - ceres_options.max_num_iterations = 1e4; - ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options); + solver_wolf_diff = std::make_shared<SolverCeres>(problem); + solver_wolf_diff->getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH + solver_wolf_diff->getSolverOptions().max_line_search_step_contraction = 1e-3; + solver_wolf_diff->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -1362,7 +1348,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) KF1->getV()->fix(); KF1->getCaptureOf(sen_imu)->setCalibration((Vector6d()<<-1,-2,-3,-1,-2,-3).finished()); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1396,7 +1382,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1414,7 +1400,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1432,7 +1418,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1452,7 +1438,7 @@ TEST_F(FactorImu_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1478,7 +1464,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK KF1->getO()->fix(); KF1->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1512,7 +1498,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1530,7 +1516,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1548,7 +1534,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1568,7 +1554,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBia KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias @@ -1594,7 +1580,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initO last_KF->getO()->fix(); last_KF->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1626,7 +1612,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); last_KF ->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1641,7 +1627,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1656,7 +1642,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1673,7 +1659,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias); last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1705,7 +1691,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed @@ -1721,7 +1707,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1736,7 +1722,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1753,7 +1739,7 @@ TEST_F(FactorImu_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1775,7 +1761,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) last_KF->getO()->fix(); last_KF->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1806,7 +1792,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1821,7 +1807,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1836,7 +1822,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1853,7 +1839,7 @@ TEST_F(FactorImu_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1874,7 +1860,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) last_KF->getO()->fix(); last_KF->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1905,7 +1891,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1920,7 +1906,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1935,7 +1921,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1952,7 +1938,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -1974,7 +1960,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initO last_KF->getO()->fix(); last_KF->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2005,7 +1991,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2020,7 +2006,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2035,7 +2021,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2052,7 +2038,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2074,7 +2060,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_i last_KF->getO()->fix(); last_KF->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2105,7 +2091,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2120,7 +2106,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2135,7 +2121,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2152,7 +2138,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(x_origin); last_KF->setState(expected_final_state); - report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2174,7 +2160,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_ last_KF->setState(expected_final_state); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2195,7 +2181,7 @@ TEST_F(FactorImu_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_ // // last_KF->setState(expected_final_state); // -// std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport +// std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport // // //Only biases are unfixed // ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3) @@ -2221,8 +2207,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2266,8 +2252,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2313,8 +2299,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport - //ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + //solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2342,7 +2328,7 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2372,7 +2358,7 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2400,7 +2386,7 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2429,8 +2415,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2487,8 +2473,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2548,8 +2534,8 @@ TEST_F(FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_ Eigen::Vector6d bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); + std::string report = solver_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + solver_wolf_diff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index 7fcf66fc8273f27588131ce2e2c1b8642e634257..08f6d459d444f992616b9a5daf56a85ee18d932d 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -6,6 +6,8 @@ */ //Wolf +#include <core/ceres_wrapper/solver_ceres.h> + #include "imu/processor/processor_imu.h" #include "imu/sensor/sensor_imu.h" @@ -14,8 +16,6 @@ #include <core/common/wolf.h> #include <core/sensor/sensor_odom_3d.h> #include <core/processor/processor_odom_3d.h> -#include <core/ceres_wrapper/ceres_manager.h> - #include <core/utils/utils_gtest.h> //#include <core/utils/logging.h> @@ -34,7 +34,7 @@ class Process_Factor_Imu : public testing::Test public: // Wolf objects ProblemPtr problem; - CeresManagerPtr ceres_manager; + SolverCeresPtr solver; SensorImuPtr sensor_imu; ProcessorImuPtr processor_imu; CaptureImuPtr capture_imu; @@ -105,8 +105,7 @@ class Process_Factor_Imu : public testing::Test problem = Problem::create("POV", 3); // CERES WRAPPER - ceres::Solver::Options ceres_options; - ceres_manager = make_shared<CeresManager>(problem, ceres_options); + solver = make_shared<SolverCeres>(problem); // SENSOR + PROCESSOR Imu SensorBasePtr sensor = problem->installSensor ("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); @@ -524,7 +523,7 @@ class Process_Factor_Imu : public testing::Test string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF) { - string report = ceres_manager->solve(verbose); + string report = solver->solve(verbose); bias_0 = C_0->getSensorIntrinsic()->getState(); bias_1 = C_1->getSensorIntrinsic()->getState(); diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp index e178eee948863bc984e7ab1b4b94f448ec740e0d..ad0faaf82d4d2ea9cd13a3c628ffe4d14c625a56 100644 --- a/test/gtest_processor_imu.cpp +++ b/test/gtest_processor_imu.cpp @@ -16,9 +16,6 @@ #include "core/utils/logging.h" #include "core/math/rotations.h" -#include "core/ceres_wrapper/ceres_manager.h" - - #include <cmath> #include <iostream>