diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp index 561a89b5dcd815bd5cd4fcddd9989751bcd4f763..4c19834f3af6c1edcf8ae4951d67a49f3ea4167a 100644 --- a/src/test/gtest_constraint_imu.cpp +++ b/src/test/gtest_constraint_imu.cpp @@ -69,11 +69,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -158,11 +154,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -247,11 +239,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -337,11 +325,7 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -426,11 +410,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -519,11 +499,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -608,11 +584,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -706,11 +678,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -812,11 +780,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test ceres_options.minimizer_type = ceres::TRUST_REGION; ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -917,11 +881,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test ceres_options.minimizer_type = ceres::TRUST_REGION; ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -1057,11 +1017,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot2 : public testing::Test ceres_options.minimizer_type = ceres::TRUST_REGION; ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -1200,11 +1156,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test ceres_options.minimizer_type = ceres::TRUST_REGION; ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -1339,11 +1291,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test ceres_options.minimizer_type = ceres::TRUST_REGION; ceres_options.max_line_search_step_contraction = 1e-3; ceres_options.max_num_iterations = 1e4; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -1543,11 +1491,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex : public testing::Test 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -1706,11 +1650,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasComplex_initOK : public testin 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -1869,11 +1809,7 @@ class ConstraintIMU_ODOM_biasTest_Static_NullBiasNoisyComplex_initOK : public te 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -2032,11 +1968,7 @@ class ConstraintIMU_ODOM_biasTest_Move_BiasedNoisyComplex_initOK : public testin 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; -<<<<<<< HEAD ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options); -======= - ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options, true); ->>>>>>> master // 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"); @@ -2159,11 +2091,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) //wolf_problem_ptr_->print(1,1,1,1); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //wolf_problem_ptr_->print(1,1,1,1); @@ -2187,11 +2115,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD std::string report; -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -2203,11 +2127,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2225,11 +2145,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2247,11 +2163,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2271,11 +2183,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2299,11 +2207,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_in last_KF->getOPtr()->fix(); last_KF->getVPtr()->fix(); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2325,11 +2229,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD std::string report; -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -2341,11 +2241,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2363,11 +2259,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2385,11 +2277,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2409,11 +2297,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2437,11 +2321,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_i last_KF->getOPtr()->fix(); last_KF->getVPtr()->fix(); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2463,11 +2343,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD std::string report; -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -2479,11 +2355,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2501,11 +2373,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2523,11 +2391,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2547,11 +2411,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2574,12 +2434,8 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD std::string report; -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") epsilon_bias = 0.000001; @@ -2590,11 +2446,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2612,11 +2464,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2634,11 +2482,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2658,11 +2502,7 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport; -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2686,11 +2526,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) last_KF->getOPtr()->fix(); last_KF->getVPtr()->fix(); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2712,10 +2548,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -2727,11 +2559,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2749,11 +2577,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2771,11 +2595,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2795,11 +2615,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias) origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2825,11 +2641,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK) //wolf_problem_ptr_->print(4,1,1,1); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master // wolf_problem_ptr_->print(4,1,1,1); @@ -2853,10 +2665,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -2868,11 +2676,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2890,11 +2694,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2912,11 +2712,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2936,11 +2732,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -2966,11 +2758,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_i //wolf_problem_ptr_->print(4,1,1,1); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //wolf_problem_ptr_->print(4,1,1,1); @@ -2994,10 +2782,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -3009,11 +2793,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3031,11 +2811,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3053,11 +2829,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3077,11 +2849,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3107,11 +2875,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 //wolf_problem_ptr_->print(4,1,1,1); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //wolf_problem_ptr_->print(4,1,1,1); @@ -3135,10 +2899,6 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 wolf::Scalar epsilon_bias = 0.0000001; Eigen::VectorXs perturbated_origin_state(x_origin); -<<<<<<< HEAD -======= - ceres::Solver::Summary summary; ->>>>>>> master //============================================================== //WOLF_INFO("Starting error bias 1e-6") @@ -3150,11 +2910,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3172,11 +2928,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3194,11 +2946,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3218,11 +2966,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2 origin_KF->setState(perturbated_origin_state); last_KF->setState(expected_final_state); -<<<<<<< HEAD report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3246,11 +2990,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q last_KF->getOPtr()->fix(); last_KF->getVPtr()->unfix(); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //Only biases are unfixed ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100) @@ -3275,11 +3015,7 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_ini //wolf_problem_ptr_->print(4,1,1,1); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master //wolf_problem_ptr_->print(4,1,1,1); @@ -3313,11 +3049,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2 //wolf_problem_ptr_->print(4,1,1,1); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master ceres_manager_wolf_diff->computeCovariances(ALL); //wolf_problem_ptr_->print(4,1,1,1); @@ -3369,11 +3101,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2 origin_KF->getAccBiasPtr()->setState(accBias + random_err); origin_KF->getGyroBiasPtr()->setState(gyroBias + random_err); -<<<<<<< HEAD std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport -======= - ceres::Solver::Summary summary = ceres_manager_wolf_diff->solve(); ->>>>>>> master ceres_manager_wolf_diff->computeCovariances(ALL); ASSERT_MATRIX_APPROX(origin_KF->getAccBiasPtr()->getState(), origin_bias.head(3), wolf::Constants::EPS*100)