Skip to content
Snippets Groups Projects
Commit b8dd308b authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Add bootstrapEnable(). All tests pass

parent 7cbf3a03
No related branches found
No related tags found
2 merge requests!54devel->main,!50Resolve "Bootstrap sequence"
......@@ -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,
......
......@@ -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())
......
......@@ -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);
......
......@@ -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";
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment