diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp index 5cd0b142813938ecbb0ab53984de3c92411d2400..2bc64142ab15731b6bddbc0b61ba32c91436aa12 100644 --- a/src/test/gtest_constraint_imu.cpp +++ b/src/test/gtest_constraint_imu.cpp @@ -896,7 +896,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose()); ProcessorOdom3DParamsPtr prc_odom3D_params = std::make_shared<ProcessorOdom3DParams>(); - prc_odom3D_params->max_time_span = 0.0099; + prc_odom3D_params->max_time_span = 0.9999; prc_odom3D_params->max_buff_length = 1000; //make it very high so that this condition will not pass prc_odom3D_params->dist_traveled = 1000; prc_odom3D_params->angle_turned = 1000; @@ -910,8 +910,9 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test x_origin.resize(10); x_origin << 0,0,0, 0,0,0,1, 0,0,0; origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003; -// origin_bias /= 100; + t.set(0); + sensor_imu->getIntrinsicPtr()->setState(origin_bias); //set origin of the problem origin_KF = processor_imu->setOrigin(x_origin, t); @@ -926,27 +927,28 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s TimeStamp t_imu(0.0), t_odo(0.0); - Scalar dt_imu(0.001), dt_odo(.01); + Scalar dt_imu(0.001), dt_odo(1.0); capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr); capture_odo = std::make_shared<CaptureOdom3D>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr); sensor_odo->process(capture_odo); - t_odo += dt_odo; //first odometry data will be processed at this timestamp + + t_odo.set(t_odo.get() + dt_odo); //first odometry data will be processed at this timestamp // ground truth for quaternion: Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity()); Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity()); - WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose()); - WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0)); + //WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose()); + //WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0)); for(unsigned int i = 1; i<=1000; i++) { // PROCESS IMU DATA // Time and data variables - t_imu += dt_imu; + t_imu.set(i*dt_imu); //rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s rateOfTurn << 5, 10, 15; // deg/s @@ -963,8 +965,8 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test capture_imu->setData(data_imu); sensor_imu->process(capture_imu); - WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose()); - WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0)); + //WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose()); + //WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0)); //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement if(t_imu.get() >= t_odo.get()) @@ -992,7 +994,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test //prepare next odometry measurement quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF - t_odo += dt_odo; + t_odo.set(t_odo.get() + dt_odo); break; } } @@ -1020,7 +1022,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test // ==================================================== show problem status - problem->print(4,1,1,1); + //problem->print(4,1,1,1); } @@ -1108,9 +1110,12 @@ class ConstraintIMU_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(), origin_bias); + wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr); sen_odom3D->process(mot_ptr); + //first odometry data will be processed at this timestamp t_odom.set(t_odom.get() + dt_odom); @@ -2290,7 +2295,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2_InvarP1Q1V1Q2 last_KF->getVPtr()->fix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); @@ -2316,7 +2321,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2_InvarP1Q1V1 last_KF->getVPtr()->fix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); @@ -2342,7 +2347,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2V2_InvarP1Q1V1 last_KF->getVPtr()->unfix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); @@ -2393,7 +2398,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1 last_KF->getVPtr()->unfix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); @@ -2424,7 +2429,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1 WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff()) } -TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_init0) +TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2Q1P2Q2_InvarP1V1V2_init0) { //Add fix constraint on yaw to make the problem observable Eigen::MatrixXs featureFix_cov(6,6); @@ -2446,7 +2451,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1 last_KF->getVPtr()->fix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); @@ -2500,7 +2505,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P1Q1V1_InvarP2Q2 last_KF->getVPtr()->fix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration(); origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err); @@ -2530,6 +2535,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P1Q1V1_InvarP2Q2 if(cov_stdev.tail(6).maxCoeff()>=1) WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff()) } + TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarB1B2Q1P2Q2_InvarP1V1V2_initO) { //Add fix constraint on yaw to make the problem observable @@ -2552,21 +2558,17 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarB1B2Q1P2Q2_InvarP1V last_KF->getVPtr()->fix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); 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); - ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) + ASSERT_MATRIX_APPROX(origin_KF->getState(), x_origin, 1e-6) + ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-6) 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) - - ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100) - ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000) - Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data()); - ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100) Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState()); Eigen::MatrixXs covX(10,10); @@ -2591,18 +2593,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarB1B2Q1P2Q2_InvarP1V TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK) { //prepare problem for solving - origin_KF->getPPtr()->fix(); - origin_KF->getOPtr()->fix(); - origin_KF->getVPtr()->fix(); + origin_KF->fix(); last_KF->setState(expected_final_state); - - last_KF->getPPtr()->unfix(); - last_KF->getOPtr()->unfix(); - last_KF->getVPtr()->fix(); + last_KF->fix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration(); origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err); @@ -2616,36 +2613,14 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V ceres_manager->computeCovariances(ALL); //Only biases are unfixed - ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-4) - 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) - - Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState()); - Eigen::MatrixXs covX(10,10); - - //get data from covariance blocks - 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) - - /*TEST_COUT << "2*std : " << cov_stdev.transpose(); - TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state - TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/ - - for(unsigned int i = 0; i<10; i++) - assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i))); - - if(cov_stdev.tail(6).maxCoeff()>=1) - WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff()) + ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) + ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) } TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK) { //prepare problem for solving - origin_KF->getPPtr()->fix(); - origin_KF->getOPtr()->fix(); - origin_KF->getVPtr()->fix(); + origin_KF->fix(); last_KF->setState(expected_final_state); @@ -2661,10 +2636,9 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport ceres_manager->computeCovariances(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) - - ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100) + ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-8) + ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) + ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState()); Eigen::MatrixXs covX(10,10); @@ -2686,7 +2660,14 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff()) } -TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK) +/* VarB1B2V1V2_InvarP1Q1P2Q2_initOK Test not working : + * IMU data integration is done with correct biases (so this is the case of a calibrated IMU). Before solving the problem, we perturbate the initial bias. + * We solve the problem by fixing all states excepted V1 and V2. while creating the constraints, both velocities are corrected using the difference between the actual + * bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the + * difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this + * solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other constraints here.) + */ +/*TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK) { //prepare problem for solving origin_KF->getPPtr()->fix(); @@ -2700,25 +2681,22 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q last_KF->getVPtr()->unfix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); 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 - ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) + ASSERT_MATRIX_APPROX(origin_KF->getState(), x_origin, 1e-4) + ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-4) 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) +}*/ - ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000) -} - -TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK) +TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2Q2V2_InvarP1Q1V1P2_initOK) { //prepare problem for solving - origin_KF->getPPtr()->fix(); - origin_KF->getOPtr()->fix(); - origin_KF->getVPtr()->unfix(); + origin_KF->fix(); last_KF->setState(expected_final_state); @@ -2727,27 +2705,21 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P last_KF->getVPtr()->unfix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); 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 - ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) - 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) - - ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000) - Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data()); - ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100) + ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-8) + ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) + ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) } -TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK) +TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2V2_InvarP1Q1V1Q2_initOK) { //prepare problem for solving - origin_KF->getPPtr()->fix(); - origin_KF->getOPtr()->fix(); - origin_KF->getVPtr()->unfix(); + origin_KF->fix(); last_KF->setState(expected_final_state); @@ -2756,17 +2728,15 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q last_KF->getVPtr()->unfix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); 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 - ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) - 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) - - ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000) + ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-8) + ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) + ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) } TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK) @@ -2801,9 +2771,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK) { //prepare problem for solving - origin_KF->getPPtr()->fix(); - origin_KF->getOPtr()->fix(); - origin_KF->getVPtr()->fix(); + origin_KF->fix(); last_KF->setState(expected_final_state); @@ -2812,21 +2780,16 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V last_KF->getVPtr()->unfix(); //perturbation of origin bias - Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001); + Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001); 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); - ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000) - 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) - - ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100) - ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000) - Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data()); - ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100) + ASSERT_MATRIX_APPROX(last_KF->getState(), expected_final_state, 1e-8) + ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) + ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-8) Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState()); Eigen::MatrixXs covX(10,10); @@ -2854,7 +2817,7 @@ int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); // ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*:ConstraintIMU_biasTest_Static_NonNullBias.*:ConstraintIMU_biasTest_Move_NullBias.*:ConstraintIMU_biasTest_Move_NonNullBiasRotCst.*"; - ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY.*"; + ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2P2Q2V2_InvarP1Q1V1_initOK"; // ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK"; return RUN_ALL_TESTS();