From 3d3639c65dce97c7006d6916dde09d0e752e009f Mon Sep 17 00:00:00 2001 From: Idril Geer <igeer@iri.upc.edu> Date: Tue, 5 Oct 2021 15:39:57 +0200 Subject: [PATCH] work on the test --- demos/imu_static_init.yaml | 12 ---- test/gtest_imu_static_init.cpp | 92 +++++++++++++++------------ test/yaml/imu_static_init.yaml | 12 ++++ test/yaml/sensor_imu.yaml | 9 +++ test/yaml/sensor_imu_static_init.yaml | 9 +++ 5 files changed, 81 insertions(+), 53 deletions(-) delete mode 100644 demos/imu_static_init.yaml create mode 100644 test/yaml/imu_static_init.yaml create mode 100644 test/yaml/sensor_imu.yaml create mode 100644 test/yaml/sensor_imu_static_init.yaml diff --git a/demos/imu_static_init.yaml b/demos/imu_static_init.yaml deleted file mode 100644 index 343cc5a3a..000000000 --- a/demos/imu_static_init.yaml +++ /dev/null @@ -1,12 +0,0 @@ -type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. - -time_tolerance: 0.0025 # Time tolerance for joining KFs -unmeasured_perturbation_std: 0.00001 - -keyframe_vote: - voting_active: true - voting_aux_active: false - max_time_span: 1.0 # seconds - max_buff_length: 20000 # motion deltas - dist_traveled: 999 # meters - angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp index 65605c232..22d47ae5d 100644 --- a/test/gtest_imu_static_init.cpp +++ b/test/gtest_imu_static_init.cpp @@ -31,7 +31,7 @@ class ProcessorImuStaticInitTest : public testing::Test double dt; Vector6d data; Matrix6d data_cov; - FrameBasePtr KF0; + FrameBasePtr KF0_; FrameBasePtr first_frame_; FrameBasePtr last_frame_; SolverCeresPtr solver_; @@ -51,8 +51,8 @@ class ProcessorImuStaticInitTest : public testing::Test // Wolf problem problem_ptr_ = Problem::create("POV", 3); Vector7d extrinsics = (Vector7d() << 0,0,0,0,0,0,1).finished(); - sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu.yaml"); - ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu_static_init.yaml"); + sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/test/yaml/sensor_imu_static_init.yaml"); + ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu_static_init.yaml"); processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr); // Time and data variables @@ -60,7 +60,7 @@ class ProcessorImuStaticInitTest : public testing::Test t0.set(0); t = t0; data = Vector6d::Random(); - data_cov = Matrix6d::Identity() * 1e-3; + data_cov = Matrix6d::Identity(); last_frame_ = nullptr; first_frame_ = nullptr; @@ -70,37 +70,44 @@ class ProcessorImuStaticInitTest : public testing::Test // Set the origin VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()}); WOLF_INFO("x0c is: \n", x0c); - KF0 = problem_ptr_->setPriorFix(x0c, t0, dt/2); - processor_motion_->setOrigin(KF0); + //KF0_ = problem_ptr_->setPriorFix(x0c, t0, dt/2); + + Vector4d q_init; q_init << 0,0,0,1; + VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()}); + VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()}); + KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01); + + processor_motion_->setOrigin(KF0_); } void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var) { //Data - data = body_magnitudes + bias_groundtruth; - data.head(3) -= Quaterniond(Vector4d(KF0->getO()->getState())).conjugate() * wolf::gravity(); + data = body_magnitudes; + data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity(); + data += bias_groundtruth; //Set bias initial guess sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); #if WRITE_CSV_FILE - std::fstream file_est; - file_est.open("./est-"+test_name+".csv", std::fstream::out); -// std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n"; - std::string header_est; - if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n"; - if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n"; - if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; - file_est << header_est; + std::fstream file_est; + file_est.open("./est-"+test_name+".csv", std::fstream::out); + // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n"; + std::string header_est; + if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n"; + if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + file_est << header_est; #endif int n_frames = 0; - for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){ + for(t = t0; t <= t0 + 3.9 + dt/2; t+=dt){ WOLF_INFO("\n------------------------------------------------------------------------"); //Create and process capture - auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov); + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); C->process(); auto state = problem_ptr_->getState(); @@ -114,8 +121,8 @@ class ProcessorImuStaticInitTest : public testing::Test << state['V'](testing_var) << ";" << state['O'](testing_var) << ";" << bias_est(testing_var) << ";" - << bias_preint(testing_var) << ";" << bias_est(testing_var+3) << ";" + << bias_preint(testing_var) << ";" << bias_preint(testing_var+3) << "\n"; #endif @@ -126,32 +133,35 @@ class ProcessorImuStaticInitTest : public testing::Test last_frame_ = processor_motion_->getOrigin()->getFrame(); // impose static - last_frame_->getP()->setState(KF0->getP()->getState()); - last_frame_->getO()->setState(KF0->getO()->getState()); + last_frame_->getP()->setState(KF0_->getP()->getState()); + last_frame_->getO()->setState(KF0_->getO()->getState()); last_frame_->getV()->setZero(); //Fix frame last_frame_->fix(); //Solve - std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); - //WOLF_INFO("Solver Report: ", report); + if(n_frames > 0) + { + std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO("Solver Report: ", report); - state = problem_ptr_->getState(); - bias_est = sensor_ptr_->getIntrinsic()->getState(); - bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + state = problem_ptr_->getState(); + bias_est = sensor_ptr_->getIntrinsic()->getState(); + bias_preint = processor_motion_->getLast()->getCalibrationPreint(); #if WRITE_CSV_FILE - // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result - file_est << std::fixed << t+dt/2 << ";" - << state['P'](testing_var) << ";" - << state['V'](testing_var) << ";" - << state['O'](testing_var) << ";" - << bias_est(testing_var) << ";" - << bias_preint(testing_var) << ";" - << bias_est(testing_var+3) << ";" - << bias_preint(testing_var+3) << "\n"; + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'](testing_var) << ";" + << bias_est(testing_var) << ";" + << bias_est(testing_var+3) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(testing_var+3) << "\n"; #endif + } } @@ -165,14 +175,14 @@ class ProcessorImuStaticInitTest : public testing::Test WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); if(n_frames == 2) { - //ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-4); + //ASSERT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-4); //ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); } else if (n_frames > 2) { - ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-6); - ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); - ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6); + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6); } } @@ -188,7 +198,7 @@ class ProcessorImuStaticInitTest : public testing::Test TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero) { Vector6d body_magnitudes = Vector6d::Zero(); - Vector6d bias_groundtruth = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished(); + Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); Vector6d bias_initial_guess = Vector6d::Zero(); TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0); @@ -198,7 +208,7 @@ TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX) { Vector6d body_magnitudes = Vector6d::Zero(); Vector6d bias_groundtruth = Vector6d::Zero(); - Vector6d bias_initial_guess = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished(); + Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0); } diff --git a/test/yaml/imu_static_init.yaml b/test/yaml/imu_static_init.yaml new file mode 100644 index 000000000..46393f30b --- /dev/null +++ b/test/yaml/imu_static_init.yaml @@ -0,0 +1,12 @@ +type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +time_tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured_perturbation_std: 0.0001 + +keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.9999 # seconds + max_buff_length: 1000000000 # motion deltas + dist_traveled: 100000000000 # meters + angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml new file mode 100644 index 000000000..3c78a00d3 --- /dev/null +++ b/test/yaml/sensor_imu.yaml @@ -0,0 +1,9 @@ +type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +motion_variances: + a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2 + w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec + ab_initial_stdev: 0.800 # m/s2 - initial bias + wb_initial_stdev: 0.350 # rad/sec - initial bias + ab_rate_stdev: 0.1 # m/s2/sqrt(s) + wb_rate_stdev: 0.0400 # rad/s/sqrt(s) diff --git a/test/yaml/sensor_imu_static_init.yaml b/test/yaml/sensor_imu_static_init.yaml new file mode 100644 index 000000000..700b8d676 --- /dev/null +++ b/test/yaml/sensor_imu_static_init.yaml @@ -0,0 +1,9 @@ +type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +motion_variances: + a_noise: 0.001 # standard deviation of Acceleration noise (same for all the axis) in m/s2 + w_noise: 0.001 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec + ab_initial_stdev: 0.001 # m/s2 - initial bias + wb_initial_stdev: 0.001 # rad/sec - initial bias + ab_rate_stdev: 0.001 # m/s2/sqrt(s) + wb_rate_stdev: 0.001 # rad/s/sqrt(s) -- GitLab