From d8a6b85ffcb132903e7716cb13eeb76f403c48f8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 2 Sep 2022 08:05:18 +0200
Subject: [PATCH] Add intrinsics to CaptureImu only if Dynamic
---
src/capture/capture_imu.cpp | 17 +++++++++++------
1 file changed, 11 insertions(+), 6 deletions(-)
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index 14a604c92..7f38375ad 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()
--
GitLab