From b8dd308b571329b0291a8964bf01251f490db46d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Mon, 20 Jun 2022 15:43:44 +0200 Subject: [PATCH] Add bootstrapEnable(). All tests pass --- include/imu/processor/processor_imu.h | 5 +++-- src/processor/processor_imu.cpp | 6 ++++++ src/yaml/processor_imu_yaml.cpp | 3 ++- test/gtest_imu.cpp | 14 ++++++++++++-- 4 files changed, 23 insertions(+), 5 deletions(-) diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h index 568b32ff2..94388550e 100644 --- a/include/imu/processor/processor_imu.h +++ b/include/imu/processor/processor_imu.h @@ -46,7 +46,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion ParamsProcessorMotion(_unique_name, _server) { bootstrap_enable = _server.getParam<bool>(prefix + _unique_name + "/bootstrap/enable"); - if (bootstrap_enable) + if (_server.hasParam(prefix + _unique_name + "/bootstrap/method")) { string str = _server.getParam<string>(prefix + _unique_name + "/bootstrap/method"); std::transform(str.begin(), str.end(), str.begin(), ::toupper); @@ -81,10 +81,11 @@ class ProcessorImu : public ProcessorMotion{ public: ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu); ~ProcessorImu() override; + WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu); void configure(SensorBasePtr _sensor) override { }; - WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu); void preProcess() override; + void bootstrapEnable(bool _bootstrap_enable = true); protected: void computeCurrentDelta(const Eigen::VectorXd& _data, diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index acc93c8f6..9bc043dc6 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -326,6 +326,12 @@ void ProcessorImu::bootstrap() } } +void ProcessorImu::bootstrapEnable(bool _bootstrap_enable) +{ + params_motion_Imu_->bootstrap_enable = _bootstrap_enable; + bootstrapping_ = _bootstrap_enable; +}; + CaptureBasePtr ProcessorImu::bootstrapOrigin() const { if (list_fac_inactive_bootstrap_.empty()) diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp index 53bc6918d..c5fd0333e 100644 --- a/src/yaml/processor_imu_yaml.cpp +++ b/src/yaml/processor_imu_yaml.cpp @@ -62,7 +62,8 @@ static ParamsProcessorBasePtr createProcessorImuParams(const std::string & _file YAML::Node bootstrap = config["bootstrap"]; params->bootstrap_enable = bootstrap["enable"].as<bool>(); - if (params->bootstrap_enable) + if (bootstrap["method"]) + // if (params->bootstrap_enable) { string str = bootstrap["method"].as<string>(); std::transform(str.begin(), str.end(), str.begin(), ::toupper); diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index fceec6fc8..90d12807f 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -1543,6 +1543,7 @@ TEST_F(Process_Factor_Imu, bootstrap) { processor_imu->setVotingActive(true); processor_imu->setMaxTimeSpan(0.04); + processor_imu->bootstrapEnable(true); auto KF0 = problem->emplaceFrame(0.0); problem->keyFrameCallback(KF0,nullptr); @@ -1559,14 +1560,23 @@ TEST_F(Process_Factor_Imu, bootstrap) { capture_imu->setTimeStamp(t); capture_imu->process(); - problem->print(4, 0, 1, 1); + } + + problem->print(4, 0, 1, 1); + + Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X + for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap()) + { + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10); + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10); + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10); } } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap"; + // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap"; // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*"; // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b"; -- GitLab