diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index 45fcac1a5296a2a6edab149140fc57e7b9d4998e..c4cba6b04638efb09da580494f56755caaf4d1f9 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; }