diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp index 6224c83f27b594c23234449b781b5cf3f56bae77..f8fa7b85bcf3060d9665257c0e5121206ae67fbe 100644 --- a/test/gtest_factor_IMU.cpp +++ b/test/gtest_factor_IMU.cpp @@ -5,17 +5,21 @@ * \author: Dinesh Atchuthan */ -//Wolf -#include "core/capture/capture_IMU.h" -#include "core/factor/factor_pose_3D.h" -#include "core/processor/processor_IMU.h" -#include "core/sensor/sensor_IMU.h" -#include "core/processor/processor_odom_3D.h" -#include "ceres_wrapper/ceres_manager.h" - #include <core/utils/utils_gtest.h> -#include "core/utils/logging.h" +#include <core/utils/logging.h> +// IMU +#include "imu/internal/config.h" +#include "imu/capture/capture_IMU.h" +#include "imu/processor/processor_IMU.h" +#include "imu/sensor/sensor_IMU.h" + +//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> @@ -38,15 +42,16 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr KF0; FrameBasePtr KF1; Eigen::Vector6s origin_bias; Eigen::VectorXs expected_final_state; Eigen::VectorXs x_origin; + Eigen::MatrixXs P_origin; virtual void SetUp() { @@ -54,24 +59,24 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} @@ -80,13 +85,15 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test expected_final_state.resize(10); x_origin.resize(10); x_origin << 0,0,0, 0,0,0,1, 0,0,0; + P_origin.setIdentity(9,9); P_origin *= 0.01; t.set(0); origin_bias << 0,0,0, 0,0,0; expected_final_state = x_origin; //null bias + static //set origin of the problem - KF0 = processor_ptr_imu->setOrigin(x_origin, t); + KF0 = problem->setPrior(x_origin, P_origin, t, 0.005); +// KF0 = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} @@ -108,7 +115,7 @@ class FactorIMU_biasTest_Static_NullBias : public testing::Test } - KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + KF1 = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests //===================================================== END{PROCESS DATA} @@ -124,10 +131,10 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr KF0; FrameBasePtr KF1; Eigen::Vector6s origin_bias; @@ -140,24 +147,24 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -171,7 +178,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test expected_final_state = x_origin; //null bias + static //set origin of the problem - KF0 = processor_ptr_imu->setOrigin(x_origin, t); + KF0 = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -192,7 +199,7 @@ class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test sen_imu->process(imu_ptr); } - KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + KF1 = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests //===================================================== END{PROCESS DATA} @@ -208,10 +215,10 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -224,24 +231,24 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -255,7 +262,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test expected_final_state = x_origin; //null bias + static, //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -276,7 +283,7 @@ class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test sen_imu->process(imu_ptr); } - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests //===================================================== END{PROCESS DATA} @@ -292,10 +299,10 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -308,24 +315,24 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -341,7 +348,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -362,7 +369,7 @@ class FactorIMU_biasTest_Static_NonNullBias : public testing::Test sen_imu->process(imu_ptr); } - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests //===================================================== END{PROCESS DATA} @@ -378,10 +385,10 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -394,24 +401,24 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} @@ -431,7 +438,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test expected_final_state = x_origin; //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} @@ -449,7 +456,7 @@ class FactorIMU_biasTest_Move_NullBias : public testing::Test sen_imu->process(imu_ptr); } - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); last_KF->setState(expected_final_state); //===================================================== END{PROCESS DATA} @@ -465,10 +472,10 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -481,24 +488,24 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -515,7 +522,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10 //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -532,7 +539,7 @@ class FactorIMU_biasTest_Move_NonNullBias : public testing::Test sen_imu->process(imu_ptr); } - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); last_KF->setState(expected_final_state); //===================================================== END{PROCESS DATA} @@ -548,10 +555,10 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -564,24 +571,24 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -605,7 +612,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test data_imu = data_imu + origin_bias; // bias measurements //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -625,7 +632,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test sen_imu->process(imu_ptr); } - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); last_KF->setState(expected_final_state); //===================================================== END{PROCESS DATA} @@ -641,10 +648,10 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -657,24 +664,24 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -703,7 +710,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test data_imu = data_imu + origin_bias; // bias measurements //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -724,7 +731,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test sen_imu->process(imu_ptr); } - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t); last_KF->setState(expected_final_state); //===================================================== END{PROCESS DATA} @@ -742,10 +749,10 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test public: wolf::TimeStamp t; SensorIMUPtr sen_imu; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -758,24 +765,24 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -788,7 +795,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test Eigen::Quaternions quat_current(Eigen::Quaternions::Identity()); //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); + origin_KF = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -823,7 +830,7 @@ class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test } expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0; - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(ts); last_KF->setState(expected_final_state); //===================================================== END{PROCESS DATA} @@ -860,7 +867,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM @@ -872,13 +879,13 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test ceres_manager = std::make_shared<CeresManager>(problem, ceres_options); // SENSOR + PROCESSOR IMU - sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); + sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); sensor_imu = std::static_pointer_cast<SensorIMU>(sensor); - processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml"); + processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); processor_imu = std::static_pointer_cast<ProcessorIMU>(processor); // SENSOR + PROCESSOR ODOM 3D - sensor = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"); + sensor = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml"); sensor_odo = std::static_pointer_cast<SensorOdom3D>(sensor); sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval()); @@ -1024,11 +1031,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test wolf::TimeStamp t; SensorIMUPtr sen_imu; SensorOdom3DPtr sen_odom3D; - ProblemPtr wolf_problem_ptr_; + ProblemPtr problem; CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; - ProcessorOdom3DPtr processor_ptr_odom3D; + ProcessorBasePtr processor_; + ProcessorIMUPtr processor_imu; + ProcessorOdom3DPtr processor_odom3D; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; @@ -1041,36 +1048,36 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = new CeresManager(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor_ = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor_); // SENSOR + PROCESSOR ODOM 3D - SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"); + SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml"); ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>(); prc_odom3D_params->max_time_span = 0.9999; prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass prc_odom3D_params->dist_traveled = 1000000000; prc_odom3D_params->angle_turned = 1000000000; - ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params); + ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params); sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr); - processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom); + processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -1085,8 +1092,8 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity()); //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); - processor_ptr_odom3D->setOrigin(origin_KF); + origin_KF = processor_imu->setOrigin(x_origin, t); + processor_odom3D->setOrigin(origin_KF); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -1100,7 +1107,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test Scalar dt(0.0010), dt_odom(1.0); TimeStamp ts(0.0), t_odom(0.0); wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); - wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr); + wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), nullptr); sen_odom3D->process(mot_ptr); //first odometry data will be processed at this timestamp t_odom.set(t_odom.get() + dt_odom); @@ -1125,11 +1132,11 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test imu_ptr->setData(data_imu); sen_imu->process(imu_ptr); - D_cor = processor_ptr_imu->getLast()->getDeltaCorrected(origin_bias); + D_cor = processor_imu->getLast()->getDeltaCorrected(origin_bias); if(ts.get() >= t_odom.get()) { - WOLF_TRACE("X_preint(t) : ", wolf_problem_ptr_->getState(ts).transpose()); + WOLF_TRACE("X_preint(t) : ", problem->getState(ts).transpose()); // PROCESS ODOM 3D DATA data_odom3D.head(3) << 0,0,0; @@ -1145,7 +1152,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test } expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0; - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(ts); last_KF->setState(expected_final_state); WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose()); @@ -1165,16 +1172,17 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test wolf::TimeStamp t; SensorIMUPtr sen_imu; SensorOdom3DPtr sen_odom3D; - ProblemPtr wolf_problem_ptr_; - CeresManager* ceres_manager_wolf_diff; - ProcessorBasePtr processor_ptr_; - ProcessorIMUPtr processor_ptr_imu; - ProcessorOdom3DPtr processor_ptr_odom3D; + ProblemPtr problem; + CeresManagerPtr ceres_manager_wolf_diff; + ProcessorBasePtr processor; + ProcessorIMUPtr processor_imu; + ProcessorOdom3DPtr processor_odom3D; FrameBasePtr origin_KF; FrameBasePtr last_KF; Eigen::Vector6s origin_bias; Eigen::VectorXs expected_final_state; Eigen::VectorXs x_origin; + Eigen::MatrixXs P_origin; virtual void SetUp() { @@ -1182,36 +1190,36 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test using std::make_shared; using std::static_pointer_cast; - std::string wolf_root = _WOLF_ROOT_DIR; + std::string wolf_root = _WOLF_IMU_ROOT_DIR; //===================================================== SETTING PROBLEM // WOLF PROBLEM - wolf_problem_ptr_ = Problem::create("POV", 3); + 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(wolf_problem_ptr_, ceres_options); + ceres_manager_wolf_diff = std::make_shared<CeresManager>(problem, ceres_options); // SENSOR + PROCESSOR IMU - SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml"); - processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_imu.yaml"); + processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/demos/processor_imu.yaml"); sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr); - processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_); + processor_imu = std::static_pointer_cast<ProcessorIMU>(processor); // SENSOR + PROCESSOR ODOM 3D - SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml"); + SensorBasePtr sen1_ptr = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/demos/sensor_odom_3D.yaml"); ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>(); prc_odom3D_params->max_time_span = 0.9999; prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass prc_odom3D_params->dist_traveled = 1000000000; prc_odom3D_params->angle_turned = 1000000000; - ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params); + ProcessorBasePtr processor_odom = problem->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params); sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr); - processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom); + processor_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_odom); //===================================================== END{SETTING PROBLEM} //===================================================== INITIALIZATION @@ -1219,14 +1227,16 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test expected_final_state.resize(10); x_origin.resize(10); x_origin << 0,0,0, 0,0,0,1, 0,0,0; + P_origin.setIdentity(9,9); P_origin *= .01; origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003; t.set(0); Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity()); Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity()); //set origin of the problem - origin_KF = processor_ptr_imu->setOrigin(x_origin, t); - processor_ptr_odom3D->setOrigin(origin_KF); + origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005); +// origin_KF = processor_imu->setOrigin(x_origin, t); +// processor_odom3D->setOrigin(origin_KF); //===================================================== END{INITIALIZATION} //===================================================== PROCESS DATA @@ -1239,7 +1249,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test Scalar dt(0.0010), dt_odom(1.0); TimeStamp ts(0.0), t_odom(1.0); wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); - wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr); + wolf::CaptureOdom3DPtr mot_ptr = std::make_shared<CaptureOdom3D>(t, sen_odom3D, data_odom3D, nullptr); sen_odom3D->process(mot_ptr); //first odometry data will be processed at this timestamp //t_odom.set(t_odom.get() + dt_odom); @@ -1321,7 +1331,7 @@ class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test } expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0; - last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts); + last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(ts); last_KF->setState(expected_final_state); //===================================================== END{PROCESS DATA} @@ -1352,7 +1362,7 @@ TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) KF1->getV()->fix(); KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished()); - std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; + std::string report = ceres_manager_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 @@ -1386,7 +1396,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1404,7 +1414,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1422,7 +1432,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1442,7 +1452,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1468,7 +1478,7 @@ TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK KF1->getO()->fix(); KF1->getV()->fix(); - std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; + std::string report = ceres_manager_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 @@ -1502,7 +1512,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1520,7 +1530,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1538,7 +1548,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1558,7 +1568,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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 @@ -1584,7 +1594,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + std::string report = ceres_manager_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) @@ -1616,7 +1626,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1631,7 +1641,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1646,7 +1656,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1663,7 +1673,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1695,7 +1705,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); // 0: nothing, 1: BriefReport, 2: FullReport; //Only biases are unfixed @@ -1711,7 +1721,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1726,7 +1736,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1743,7 +1753,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(1); // 0: nothing, 1: BriefReport, 2: FullReport; + report = ceres_manager_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) @@ -1765,7 +1775,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -1796,7 +1806,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -1811,7 +1821,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -1826,7 +1836,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -1843,7 +1853,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -1864,7 +1874,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -1895,7 +1905,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(1); // 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -1910,7 +1920,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -1925,7 +1935,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -1942,7 +1952,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -1964,7 +1974,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -1995,7 +2005,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2010,7 +2020,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -2025,7 +2035,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -2042,7 +2052,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -2064,7 +2074,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2095,7 +2105,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2110,7 +2120,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -2125,7 +2135,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -2142,7 +2152,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(1);// 0: nothing, 1: BriefReport, 2: FullReport + report = ceres_manager_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) @@ -2164,7 +2174,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_ last_KF->setState(expected_final_state); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2185,7 +2195,7 @@ TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_ // // last_KF->setState(expected_final_state); // -// std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport +// std::string report = ceres_manager_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) @@ -2211,8 +2221,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(ALL); + 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); //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5) @@ -2222,7 +2232,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_i Eigen::MatrixXs covX(10,10); //get data from covariance blocks - wolf_problem_ptr_->getFrameCovariance(last_KF, covX); + problem->getFrameCovariance(last_KF, covX); for(int i = 0; i<10; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) @@ -2256,8 +2266,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(ALL); + 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); 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) @@ -2268,7 +2278,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_i Eigen::MatrixXs covX(10,10); //get data from covariance blocks - wolf_problem_ptr_->getFrameCovariance(last_KF, covX); + problem->getFrameCovariance(last_KF, covX); for(int i = 0; i<10; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) @@ -2303,8 +2313,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - //ceres_manager_wolf_diff->computeCovariances(ALL); + 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); 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) @@ -2332,7 +2342,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2362,7 +2372,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2390,7 +2400,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager_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) @@ -2419,8 +2429,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(ALL); + 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); 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) @@ -2435,7 +2445,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_i Eigen::MatrixXs covX(10,10); //get data from covariance blocks - wolf_problem_ptr_->getFrameCovariance(last_KF, covX); + problem->getFrameCovariance(last_KF, covX); for(int i = 0; i<10; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) @@ -2457,12 +2467,9 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i Eigen::MatrixXs featureFix_cov(6,6); featureFix_cov = Eigen::MatrixXs::Identity(6,6); featureFix_cov(5,5) = 0.1; - // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); - auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr); - // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); + auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr); auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov); - // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); - FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); + FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); //prepare problem for solving origin_KF->getP()->fix(); @@ -2480,8 +2487,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(ALL); + 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); 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) @@ -2496,7 +2503,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_i Eigen::MatrixXs covX(10,10); //get data from covariance blocks - wolf_problem_ptr_->getFrameCovariance(last_KF, covX); + problem->getFrameCovariance(last_KF, covX); for(int i = 0; i<10; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) @@ -2518,12 +2525,12 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_ Eigen::MatrixXs featureFix_cov(6,6); featureFix_cov = Eigen::MatrixXs::Identity(6,6); featureFix_cov(5,5) = 0.1; - // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr)); - auto capfix = CaptureBase::emplace<CaptureMotion>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr); + // CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureOdom3D>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr)); + auto capfix = CaptureBase::emplace<CaptureOdom3D>(origin_KF, 0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), nullptr); // FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov)); auto ffix = FeatureBase::emplace<FeatureBase>(capfix, "ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov); - // FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); - FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); + // FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix))); + FactorPose3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(FactorBase::emplace<FactorPose3D>(ffix, ffix)); //prepare problem for solving origin_KF->getP()->fix(); @@ -2541,8 +2548,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_ Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager_wolf_diff->computeCovariances(ALL); + 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); 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) @@ -2557,7 +2564,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_ Eigen::MatrixXs covX(10,10); //get data from covariance blocks - wolf_problem_ptr_->getFrameCovariance(last_KF, covX); + problem->getFrameCovariance(last_KF, covX); for(int i = 0; i<10; i++) cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value) @@ -2596,9 +2603,9 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_in // WOLF_TRACE("last bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose()); // WOLF_TRACE("jacob bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0)); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport std::cout << report << std::endl; - ceres_manager->computeCovariances(ALL); + ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2642,8 +2649,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_in Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager->computeCovariances(ALL); + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5) ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2688,7 +2695,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_in Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager->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(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2715,7 +2722,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_in Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager->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(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2744,7 +2751,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_in Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager->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(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2771,7 +2778,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_in Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport + std::string report = ceres_manager->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(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2800,8 +2807,8 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); - std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport - ceres_manager->computeCovariances(ALL); + std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport + ceres_manager->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(sensor_imu)->getCalibration(), origin_bias, 1e-5) @@ -2837,7 +2844,7 @@ TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_in int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*"; +// ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*"; // ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK"; // ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK"; // ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";