diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index 45ad4668701ae85aeeb46d84c7c8ae18873fb5ef..acc93c8f6f5e5f08fddf58d101fd912fad6fb06a 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -307,6 +307,13 @@ void ProcessorImu::bootstrap() // Transform problem to new reference getProblem()->transform(transformation); + // Recompute states at keyframes if they were provided by this processor + bool recomputed = recomputeStates(); + if (recomputed) + { + WOLF_DEBUG("IMU Keyframe states have been recomputed!"); + } + // Activate factors that were inactive during bootstrap while (not list_fac_inactive_bootstrap_.empty()) { @@ -349,6 +356,31 @@ VectorXd ProcessorImu::bootstrapDelta() const return delta_int; } +bool ProcessorImu::recomputeStates() const +{ + const auto& mp = getProblem()->getMotionProviderMap(); + if (not mp.empty() and mp.begin()->second == std::static_pointer_cast<const MotionProvider>( + std::static_pointer_cast<const ProcessorMotion>(shared_from_this()))) + { + WOLF_DEBUG("Recomputing IMU keyframe states..."); + for (const auto& fac : list_fac_inactive_bootstrap_) + { + 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; + } + else return false; +} + } // namespace wolf // Register in the FactoryProcessor