fixing bootstrap bug

src/processor/processor_imu_3d.cpp

@@ -99,13 +99,13 @@ bool ProcessorImu3d::voteForKeyFrame() const
     return false;
 }
 
-CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr &  _frame_own,
-                                                const SensorBasePtr & _sensor,
-                                                const TimeStamp &     _ts,
-                                                const VectorXd &      _data,
-                                                const MatrixXd &      _data_cov,
-                                                const VectorXd &      _calib,
-                                                const VectorXd &      _calib_preint,
+CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr   &_frame_own,
+                                                const SensorBasePtr  &_sensor,
+                                                const TimeStamp      &_ts,
+                                                const VectorXd       &_data,
+                                                const MatrixXd       &_data_cov,
+                                                const VectorXd       &_calib,
+                                                const VectorXd       &_calib_preint,
                                                 const CaptureBasePtr &_capture_origin)
 {
     auto cap_motion = std::static_pointer_cast<CaptureMotion>(
@@ -177,9 +177,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
                                          const Eigen::MatrixXd &_data_cov,
                                          const Eigen::VectorXd &_calib,
                                          const double           _dt,
-                                         Eigen::VectorXd &      _delta,
-                                         Eigen::MatrixXd &      _delta_cov,
-                                         Eigen::MatrixXd &      _jac_delta_calib) const
+                                         Eigen::VectorXd       &_delta,
+                                         Eigen::MatrixXd       &_delta_cov,
+                                         Eigen::MatrixXd       &_jac_delta_calib) const
 {
     assert(_data.size() == data_size_ && "Wrong data size!");
 
@@ -218,7 +218,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
 void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
                                     const Eigen::VectorXd &_delta,
                                     const double           _dt,
-                                    Eigen::VectorXd &      _delta_preint_plus_delta) const
+                                    Eigen::VectorXd       &_delta_preint_plus_delta) const
 {
     /* MATHS according to Sola-16
      * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2    = Dp + Dv*dt + Dq*dp   if  dp =
@@ -235,7 +235,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
 void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
                                     const Eigen::VectorXd &_delta,
                                     const double           _dt,
-                                    VectorComposite &      _x_plus_delta) const
+                                    VectorComposite       &_x_plus_delta) const
 {
     assert(_delta.size() == 10 && "Wrong _delta vector size");
     assert(_dt >= 0 && "Time interval _dt is negative!");
@@ -255,9 +255,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
 void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
                                     const Eigen::VectorXd &_delta,
                                     const double           _dt,
-                                    Eigen::VectorXd &      _delta_preint_plus_delta,
-                                    Eigen::MatrixXd &      _jacobian_delta_preint,
-                                    Eigen::MatrixXd &      _jacobian_delta) const
+                                    Eigen::VectorXd       &_delta_preint_plus_delta,
+                                    Eigen::MatrixXd       &_jacobian_delta_preint,
+                                    Eigen::MatrixXd       &_jacobian_delta) const
 {
     /*
      * Expression of the delta integration step, D' = D (+) d:
@@ -492,9 +492,16 @@ VectorXd ProcessorImu3d::bootstrapDelta() const
     for (const auto &fac : bootstrap_factor_list_)
     // here, we take advantage of the list of IMU factors to recover all deltas
     {
-        if (std::dynamic_pointer_cast<FactorImu3d>(fac) != nullptr)
+        auto cap_motion = std::dynamic_pointer_cast<CaptureMotion>(fac->getCapture());
+        if (std::dynamic_pointer_cast<FactorImu3d>(fac) and cap_motion)
         {
-            dt = fac->getCapture()->getTimeStamp() - fac->getCapturesFactored().front().lock()->getTimeStamp();
+            dt                = cap_motion->getTimeStamp() - cap_motion->getOriginCapture()->getTimeStamp();
+            const auto &delta = fac->getFeature()->getMeasurement();  // In FeatImu, delta = measurement
+            delta_int         = imu::compose(delta_int, delta, dt);
+        }
+        else if (std::dynamic_pointer_cast<FactorRelativePose3d>(fac) and cap_motion)
+        {
+            dt                = cap_motion->getTimeStamp() - cap_motion->getOriginCapture()->getTimeStamp();
             const auto &delta = fac->getFeature()->getMeasurement();  // In FeatImu, delta = measurement
             delta_int         = imu::compose(delta_int, delta, dt);
         }
@@ -525,8 +532,8 @@ bool ProcessorImu3d::recomputeStates() const
                 const auto &cap_origin = cap->getOriginCapture();
                 const auto &frm_origin = cap_origin->getFrame();
                 const auto &delta      = VectorComposite({{'P', ftr->getMeasurement().head<3>()},
-                                                     {'O', ftr->getMeasurement().segment<4>(3)},
-                                                     {'V', ftr->getMeasurement().tail<3>()}});
+                                                          {'O', ftr->getMeasurement().segment<4>(3)},
+                                                          {'V', ftr->getMeasurement().tail<3>()}});
                 const auto &x_origin   = frm_origin->getState();
                 auto        dt         = cap->getTimeStamp() - cap_origin->getTimeStamp();
                 auto        x          = imu::composeOverState(x_origin, delta, dt);