From 830600b7ed9b093b19a4ddfe84ca1f32007d3e16 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:35:58 +0200
Subject: [PATCH] Check factor type in bootstrap operations
---
src/processor/processor_imu.cpp | 32 +++++++++++++++++++-------------
1 file changed, 19 insertions(+), 13 deletions(-)
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 45fcac1a5..c4cba6b04 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -444,9 +444,12 @@ VectorXd ProcessorImu::bootstrapDelta() const
for (const auto& fac : bootstrap_factor_list_)
// here, we take advantage of the list of IMU factors to recover all deltas
{
- dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
- const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
- delta_int = imu::compose(delta_int, delta, dt);
+ if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+ {
+ dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+ const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
+ delta_int = imu::compose(delta_int, delta, dt);
+ }
}
// now compose with delta in last_ptr_
dt = last_ptr_->getBuffer().back().ts_ - origin_ptr_->getTimeStamp();
@@ -466,16 +469,19 @@ bool ProcessorImu::recomputeStates() const
WOLF_DEBUG("Recomputing IMU keyframe states...");
for (const auto& fac : bootstrap_factor_list_)
{
- const auto& ftr = fac->getFeature();
- const auto& cap = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
- const auto& frm = cap->getFrame();
- const auto& cap_origin = cap->getOriginCapture();
- const auto& frm_origin = cap_origin->getFrame();
- const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
- const auto& x_origin = frm_origin->getState();
- auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
- auto x = imu::composeOverState(x_origin, delta, dt);
- frm->setState(x);
+ if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+ {
+ const auto& ftr = fac->getFeature();
+ const auto& cap = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
+ const auto& frm = cap->getFrame();
+ const auto& cap_origin = cap->getOriginCapture();
+ const auto& frm_origin = cap_origin->getFrame();
+ const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
+ const auto& x_origin = frm_origin->getState();
+ auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
+ auto x = imu::composeOverState(x_origin, delta, dt);
+ frm->setState(x);
+ }
}
return true;
}
--
GitLab