diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h index 568b32ff21d8fe9e88226e5cbdcd5a94434a6241..94388550eedf13e50e2a16be7d3ca060159e21c4 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 acc93c8f6f5e5f08fddf58d101fd912fad6fb06a..9bc043dc6bc6854a83abe66ded087810c6104db6 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 53bc6918d5eb753f4c801dad35058dd7c0d6174a..c5fd0333e06d6732128516081705339840100665 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 fceec6fc83ad7066f22439267192dd405a3ca63a..90d12807fe10a24496ab97c4c1e7a532ea566797 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";