Check factor type in bootstrap operations

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;
     }