diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index d176b3066b5e09dd442ac2bf1a8219f98ccc6d4a..f6cf9647c1066e2786b9ba02b07f51abf328ad4c 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,107 +1,76 @@ -image: labrobotica/ceres:1.14 - -before_script: - ## +############ YAML ANCHORS ############ +.preliminaries_template: &preliminaries_definition ## Install ssh-agent if not already installed, it is required by Docker. ## (change apt-get to yum if you use an RPM-based image) - ## - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )' - ## ## Run ssh-agent (inside the build environment) - ## - eval $(ssh-agent -s) - ## ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store ## We're using tr to fix line endings which makes ed25519 keys work ## without extra base64 encoding. ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556 - ## - mkdir -p ~/.ssh - chmod 700 ~/.ssh - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts - ## - ## Create the SSH directory and give it the right permissions - ## - - ls + # update apt - apt-get update - - apt-get install -y build-essential cmake -# SPDLOG -# - apt-get install -y libspdlog-dev - - if [ -d spdlog ]; then - - echo "directory exists" - - if [ "$(ls -A ./spdlog)" ]; then - - echo "directory not empty" - - cd spdlog - - git pull - - else - - echo "directory empty" - - git clone https://github.com/gabime/spdlog.git - - cd spdlog - - fi +.install_wolf_template: &install_wolf_definition + - if [ -d wolf ]; then + - echo "directory wolf exists" + - cd wolf + - git pull - else - - echo "directory inexistent" - - git clone https://github.com/gabime/spdlog.git - - cd spdlog + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git + - cd wolf - fi - - git fetch - - git checkout v0.17.0 - mkdir -pv build - cd build - - ls - - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - make -j2 + - ctest -j2 - make install - cd ../.. -# YAML -# - apt-get install -y libyaml-cpp-dev - - if [ -d yaml-cpp ]; then - - echo "directory exists" - - if [ "$(ls -A ./yaml-cpp)" ]; then - - echo "directory not empty" - - cd yaml-cpp - - git pull - - else - - echo "directory empty" - - git clone https://github.com/jbeder/yaml-cpp.git - - cd yaml-cpp - - fi - - else - - echo "directory inexistent" - - git clone https://github.com/jbeder/yaml-cpp.git - - cd yaml-cpp - - fi - - mkdir -pv build - - cd build - - ls - - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF .. - - make install - - cd ../.. -#Wolf core - - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git - - cd wolf +.build_and_test_template: &build_and_test_definition - mkdir -pv build - cd build - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. - - make -j$(nproc) - - ctest -j$(nproc) + - make -j2 + - ctest -j2 - make install - - cd ../.. -wolf_build_and_test: - stage: build +############ UBUNTU 16.04 TESTS ############ +wolf_build_and_test:xenial: + image: labrobotica/wolf_deps:16.04 + cache: + - key: wolf-xenial + paths: + - wolf/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + script: + - *build_and_test_definition + +############ UBUNTU 18.04 TESTS ############ +wolf_build_and_test:bionic: + image: labrobotica/wolf_deps:18.04 + cache: + - key: wolf-bionic + paths: + - wolf/ except: - master + before_script: + - *preliminaries_definition + - *install_wolf_definition script: - - mkdir -pv build - - cd build - - ls # we can check whether the directory was already full - - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. - - make -j$(nproc) - - ctest -j$(nproc) - - make install + - *build_and_test_definition diff --git a/README.md b/README.md index dd79d5e177365518822ce675cf4fce278e65e45f..2345eb53e24a8f4179ea43625f727662fba7df26 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ WOLF - Windowed Localization Frames | IMU Plugin =================================== -For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/). \ No newline at end of file +For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/). diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h index 17c80316246bdd977ed59b4edc0aea6f34dca4f7..0d52b68b7a005597b958d90a334a620819f581fe 100644 --- a/include/imu/factor/factor_imu.h +++ b/include/imu/factor/factor_imu.h @@ -4,6 +4,7 @@ //Wolf includes #include "imu/feature/feature_imu.h" #include "imu/sensor/sensor_imu.h" +#include "imu/processor/processor_imu.h" #include "core/factor/factor_autodiff.h" #include "core/math/rotations.h" @@ -235,9 +236,9 @@ Eigen::Vector9d FactorImu::error() Map<const Vector3d > acc_bias(bias.data()); Map<const Vector3d > gyro_bias(bias.data() + 3); - Eigen::Vector9d delta_exp = expectation(); + Eigen::Vector10d delta_exp = expectation(); - Eigen::Vector9d delta_preint = getMeasurement(); + Eigen::Vector10d delta_preint = getMeasurement(); Eigen::Vector9d delta_step; @@ -247,9 +248,9 @@ Eigen::Vector9d FactorImu::error() Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step); - Eigen::Vector9d res = imu::diff(delta_exp, delta_corr); + Eigen::Vector9d err = imu::diff(delta_exp, delta_corr); - return res; + return err; } template<typename D1, typename D2, typename D3> diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h index d9b0b58454132c8abaa11d50a5847d18b829337f..ac93e3fe2228f39fab603cec3b889e598309086f 100644 --- a/include/imu/math/imu_tools.h +++ b/include/imu/math/imu_tools.h @@ -566,7 +566,7 @@ inline void plus(const MatrixBase<D1>& dp1, const MatrixBase<D2>& dq1, const Mat template<typename D1, typename D2, typename D3> inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, - MatrixBase<D3>& d_pert) + MatrixBase<D3>& d) { Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) ); Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); @@ -574,11 +574,11 @@ inline void plus(const MatrixBase<D1>& d1, Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) ); Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) ); Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) ); - Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) ); - Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & d (0) ); + Map<Quaternion<typename D3::Scalar> > dq ( & d (3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & d (7) ); - plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p); + plus(dp1, dq1, dv1, dp2, do2, dv2, dp, dq, dv); } template<typename D1, typename D2> diff --git a/src/yaml/sensor_imu_yaml.cpp b/src/yaml/sensor_imu_yaml.cpp index 87201a4699e047e1873fe12de0ce121bcd96d1db..f2631e901d8d0e27d7564ef523f660820ef6d524 100644 --- a/src/yaml/sensor_imu_yaml.cpp +++ b/src/yaml/sensor_imu_yaml.cpp @@ -28,7 +28,6 @@ static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_d if (config["type"].as<std::string>() == "SensorImu") { YAML::Node variances = config["motion_variances"]; - YAML::Node kf_vote = config["keyframe_vote"]; ParamsSensorImuPtr params = std::make_shared<ParamsSensorImu>(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f89523a4bc2ac3904b444ecce442177311cf48c2..f5e97ef62c6f51996e654b0c4db3c6d7730fffa1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -43,6 +43,12 @@ target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY}) wolf_add_gtest(gtest_factor_imu2d_with_gravity gtest_factor_imu2d_with_gravity.cpp) target_link_libraries(gtest_factor_imu2d_with_gravity ${PLUGIN_NAME} ${wolf_LIBRARY}) +wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp) +target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME} ${wolf_LIBRARY}) + +wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp) +target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME} ${wolf_LIBRARY}) + wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp) target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY}) diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index 161157b8e079a5dd5f3dd4c873e754e021003c6f..d666285f444465fa01dc5d4c20fdc182efea9863 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -475,28 +475,10 @@ class Process_Factor_Imu : public testing::Test { // This perturbs states to estimate around the exact value, then assigns to the keyframe // Perturbations are applied only if the state block is unfixed - - VectorXd x_pert(10); - - // KF 0 - x_pert = x0; - if (!p0_fixed) - x_pert.head(3) += Vector3d::Random() * 0.01; - if (!q0_fixed) - x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized(); - if (!v0_fixed) - x_pert.tail(3) += Vector3d::Random() * 0.01; - KF_0->setState(x_pert); - - // KF 1 - x_pert = x1_exact; - if (!p1_fixed) - x_pert.head(3) += Vector3d::Random() * 0.01; - if (!q1_fixed) - x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized(); - if (!v1_fixed) - x_pert.tail(3) += Vector3d::Random() * 0.01; - KF_1->setState(x_pert); + KF_0->setState(x0); + KF_0->perturb(); + KF_1->setState(x1_exact); + KF_1->perturb(); } virtual void buildProblem() diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp new file mode 100644 index 0000000000000000000000000000000000000000..863147d289cf33dfe4dc272df02d65a9f3dd70b3 --- /dev/null +++ b/test/gtest_imu2d_static_init.cpp @@ -0,0 +1,534 @@ +/* + * gtest_imu2d_static_init.cpp + * + * Created on: Sept 2021 + * Author: igeer + */ + +#include <fstream> +#include <core/ceres_wrapper/solver_ceres.h> +#include <core/utils/utils_gtest.h> +#include "imu/internal/config.h" + +#include "imu/factor/factor_imu2d.h" +#include "imu/math/imu2d_tools.h" +#include "imu/sensor/sensor_imu2d.h" +#include "core/capture/capture_void.h" +#include <core/factor/factor_relative_pose_2d.h> + +using namespace Eigen; +using namespace wolf; + +class ProcessorImu2dStaticInitTest : public testing::Test +{ + + public: //These can be accessed in fixtures + wolf::ProblemPtr problem_ptr_; + wolf::SensorBasePtr sensor_ptr_; + wolf::ProcessorMotionPtr processor_motion_; + wolf::TimeStamp t; + wolf::TimeStamp t0; + double dt; + Vector6d data; + Matrix6d data_cov; + FrameBasePtr KF0_; + FrameBasePtr first_frame_; + FrameBasePtr last_frame_; + SolverCeresPtr solver_; + + //a new of this will be created for each new test + void SetUp() override + { + WOLF_INFO("Doing setup..."); + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + + // Wolf problem + problem_ptr_ = Problem::create("POV", 2); + Vector3d extrinsics = (Vector3d() << 0,0,0).finished(); + sensor_ptr_ = problem_ptr_->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/test/yaml/sensor_imu2d_static_init.yaml"); + ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu2d_static_init.yaml"); + processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr); + + // Time and data variables + dt = 0.1; + t0.set(0); + t = t0; + data = Vector6d::Random(); + data_cov = Matrix6d::Identity(); + last_frame_ = nullptr; + first_frame_ = nullptr; + + // Create the solver + solver_ = make_shared<SolverCeres>(problem_ptr_); + + // Set the origin + VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()}); + VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()}); + //KF0_ = problem_ptr_->setPriorFix(x_origin, t0, dt/2); + KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01); + + } + + void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + data.head(2) = body_magnitudes.head(2); + data(5) = body_magnitudes(2); + data.head(2) += bias_groundtruth.head(2); + data(5) += bias_groundtruth(2); + + //Set bias initial guess + sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./est2d-"+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;o;bax_est;bg_est;bax_preint;bg_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n"; + //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n"; + file_est << header_est; +#endif + + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose static + last_frame_->getP()->setState(KF0_->getP()->getState()); + last_frame_->getO()->setState(KF0_->getO()->getState()); + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->fix(); + + //Solve + 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(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + } + + } + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + 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); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } + + void TestStaticZeroOdom(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + data.head(2) = body_magnitudes.head(2); + data(5) = body_magnitudes(2); + data.head(2) += bias_groundtruth.head(2); + data(5) += bias_groundtruth(2); + + //Set bias initial guess + sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./est2dzeroodom-"+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;o;bax_est;bg_est;bax_preint;bg_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n"; + //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n"; + file_est << header_est; +#endif + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose zero odometry + processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure())); + + // add zero displacement and rotation capture & feature & factor with all previous frames + assert(sensor_ptr_->getProblem()); + for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin(); + frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end(); + frm_pair++) + { + if (frm_pair->second == last_frame_) + break; + auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr); + + auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero, + "FeatureZeroOdom", + Vector3d::Zero(), + Eigen::MatrixXd::Identity(3,3) * 0.01); + + FactorBase::emplace<FactorRelativePose2d>(feature_zero, + feature_zero, + frm_pair->second, + nullptr, + false, + TOP_MOTION); + + } + + // impose static + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->getV()->fix(); + + //Solve + 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(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + } + + } + + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + 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); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } +}; + + + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = Vector3d::Random()*100; + bias_initial_guess(2) = 0; + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = Vector3d::Random()*0.01; + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, realistic_test) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << -0.529550648247, + 0.278316717683, + -0.00122491355676).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7ad48b186079c12af08b6db42cc014207bc7bc38 --- /dev/null +++ b/test/gtest_imu_static_init.cpp @@ -0,0 +1,533 @@ +/* + * gtest_imu_static_init.cpp + * + * Created on: Sept 2021 + * Author: igeer + */ +#include <fstream> + +#include <core/ceres_wrapper/solver_ceres.h> +#include <core/utils/utils_gtest.h> +#include "imu/internal/config.h" + +#include "imu/factor/factor_imu.h" +#include "imu/math/imu_tools.h" +#include "imu/sensor/sensor_imu.h" +#include "core/capture/capture_void.h" +#include <core/factor/factor_relative_pose_3d.h> + +using namespace Eigen; +using namespace wolf; + +/** + * SET TO TRUE TO PRODUCE CSV FILE + * SET TO FALSE TO STOP PRODUCING CSV FILE + */ +#define WRITE_CSV_FILE true + +class ProcessorImuStaticInitTest : public testing::Test +{ + + public: //These can be accessed in fixtures + wolf::ProblemPtr problem_ptr_; + wolf::SensorBasePtr sensor_ptr_; + wolf::ProcessorMotionPtr processor_motion_; + wolf::TimeStamp t; + wolf::TimeStamp t0; + double dt; + Vector6d data; + Matrix6d data_cov; + FrameBasePtr KF0_; + FrameBasePtr first_frame_; + FrameBasePtr last_frame_; + SolverCeresPtr solver_; + + //a new of this will be created for each new test + void SetUp() override + { + WOLF_INFO("Doing setup..."); + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + + // 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 + "/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 + dt = 0.1; + t0.set(0); + t = t0; + data = Vector6d::Random(); + data_cov = Matrix6d::Identity(); + last_frame_ = nullptr; + first_frame_ = nullptr; + + // Create the solver + solver_ = make_shared<SolverCeres>(problem_ptr_); + + // 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); + + 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); + + } + + void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + 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); + processor_motion_->setOrigin(KF0_); + +#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"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n"; + file_est << header_est; +#endif + + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << 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 + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose static + last_frame_->getP()->setState(KF0_->getP()->getState()); + last_frame_->getO()->setState(KF0_->getO()->getState()); + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->fix(); + + //Solve + 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(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + 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 + } + + } + + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + 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); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } + + void TestStaticZeroOdom(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + 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); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./estzeroodom-"+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"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n"; + file_est << header_est; +#endif + + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << 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 + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose zero odometry + processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure())); + + // add zero displacement and rotation capture & feature & factor with all previous frames + assert(sensor_ptr_->getProblem()); + for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin(); + frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end(); + frm_pair++) + { + if (frm_pair->second == last_frame_) + break; + auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr); + + auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero, + "FeatureZeroOdom", + Vector7d::Zero(), + Eigen::MatrixXd::Identity(6,6) * 0.01); + + FactorBase::emplace<FactorRelativePose3d>(feature_zero, + feature_zero, + frm_pair->second, + nullptr, + false, + TOP_MOTION); + + } + + // impose static + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->getV()->fix(); + + //Solve + 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(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + 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 + } + + } + + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + 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); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } +}; + + + +TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + 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, true); +} + +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.42, 0, 0, 0, 0, 0).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_a_random) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = Vector6d::Random()*100; + bias_initial_guess.tail(3) = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = Vector6d::Random()*0.01; + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); +} + + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp index 4dbac962133c7d8da9f76e152a09e0749c1f2e71..a013072abf3064c5a2ac26302a1fcfe145f1a0b5 100644 --- a/test/gtest_processor_motion_intrinsics_update.cpp +++ b/test/gtest_processor_motion_intrinsics_update.cpp @@ -155,7 +155,7 @@ TEST_F(ProcessorImuTest, test1) * SET TO TRUE TO PRODUCE CSV FILE * SET TO FALSE TO STOP PRODUCING CSV FILE */ -#define WRITE_CSV_FILE false +#define WRITE_CSV_FILE true TEST_F(ProcessorImuTest, getState) { diff --git a/test/processor_imu_UnitTester.h b/test/processor_imu_UnitTester.h index f9990707cb56f5ec6d1357683aa749503343ca1b..e816349ecccd892f0ca3f1d7a1a4edbbb043eded 100644 --- a/test/processor_imu_UnitTester.h +++ b/test/processor_imu_UnitTester.h @@ -108,16 +108,16 @@ namespace wolf { jacobian_delta_ = Eigen::MatrixXd::Zero(9,9); } - Imu_jac_deltas(Imu_jac_deltas const & toCopy){ - - Delta_noisy_vect_ = toCopy.Delta_noisy_vect_; - delta_noisy_vect_ = toCopy.delta_noisy_vect_; - - Delta0_ = toCopy.Delta0_; - delta0_ = toCopy.delta0_; - jacobian_delta_preint_ = toCopy.jacobian_delta_preint_; - jacobian_delta_ = toCopy.jacobian_delta_; - } +// Imu_jac_deltas(Imu_jac_deltas const & toCopy){ +// +// Delta_noisy_vect_ = toCopy.Delta_noisy_vect_; +// delta_noisy_vect_ = toCopy.delta_noisy_vect_; +// +// Delta0_ = toCopy.Delta0_; +// delta0_ = toCopy.delta0_; +// jacobian_delta_preint_ = toCopy.jacobian_delta_preint_; +// jacobian_delta_ = toCopy.jacobian_delta_; +// } public: /*The following vectors will contain all the matrices and deltas needed to compute the finite differences. diff --git a/test/yaml/imu2d_static_init.yaml b/test/yaml/imu2d_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..4978c8e2863908df72a336da1225667411d3022a --- /dev/null +++ b/test/yaml/imu2d_static_init.yaml @@ -0,0 +1,12 @@ +type: "ProcessorImu2d" # 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/imu_static_init.yaml b/test/yaml/imu_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..46393f30b8f24bf645bfb1272764e6c8915e5dc2 --- /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 0000000000000000000000000000000000000000..3c78a00d35c785fe73381d8f6ce99a705d27ce77 --- /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_imu2d_static_init.yaml b/test/yaml/sensor_imu2d_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936 --- /dev/null +++ b/test/yaml/sensor_imu2d_static_init.yaml @@ -0,0 +1,9 @@ +type: "SensorImu2d" # 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 0000000000000000000000000000000000000000..700b8d6762b91b38e96391f6032b2c7c4221eabc --- /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)