diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp index 14a604c9261613207b5f73c1a899bcf6f782136d..7f38375ad2335533033fb4352a3871c65a6007b2 100644 --- a/src/capture/capture_imu.cpp +++ b/src/capture/capture_imu.cpp @@ -41,9 +41,11 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, _capture_origin_ptr, nullptr, nullptr, - (_sensor_ptr->getProblem()->getDim() == 2) - ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false)) - : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false))) + (_sensor_ptr->isStateBlockDynamic('I')) + ? ((_sensor_ptr->getProblem()->getDim() == 2) + ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false)) + : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false))) + : nullptr) { // } @@ -63,11 +65,14 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, _capture_origin_ptr, nullptr, nullptr, - (_bias.size() == 3) - ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false)) - : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false))) + (_sensor_ptr->isStateBlockDynamic('I')) + ? ((_sensor_ptr->getProblem()->getDim() == 2) + ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(_bias, false)) + : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(_bias, false))) + : nullptr) { assert((_bias.size() == 3) or (_bias.size() == 6)); + WOLF_WARN_COND(_sensor_ptr->isStateBlockDynamic('I'), "Sensor bias was provided but bias is static in sensor. Bias discarded."); } CaptureImu::~CaptureImu()