Add intrinsics to CaptureImu only if Dynamic

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()