Add calib params in getDelta()

src/capture_motion.cpp

@@ -69,30 +69,22 @@ void CaptureMotion::setCalibration(const VectorXs& _calib)
 }
 
 
-Eigen::VectorXs CaptureMotion::getDelta()
+Eigen::VectorXs CaptureMotion::getDelta(const VectorXs& _calib_current)
 {
     VectorXs calib_preint   = getCalibrationPreint();
-    VectorXs calib          = getCalibration();
     VectorXs delta_preint   = getBuffer().get().back().delta_integr_;
     MatrixXs jac_calib      = getBuffer().get().back().jacobian_calib_;
-    VectorXs delta_error    = jac_calib * (calib - calib_preint);
+    VectorXs delta_error    = jac_calib * (_calib_current - calib_preint);
     VectorXs delta          = correctDelta(delta_preint, delta_error);
-    //    WOLF_DEBUG("cal_preint: ", calib_preint.transpose());
-    //    WOLF_DEBUG("cal       : ", calib.transpose());
-    //    WOLF_DEBUG("delta_preint: ", delta_preint.transpose());
-    //    WOLF_DEBUG("Jac_calib: \n", jac_calib);
-    //    WOLF_DEBUG("delta error: ", delta_error.transpose());
-    //    WOLF_DEBUG("delta: ", delta.transpose());
     return delta;
 }
 
-Eigen::VectorXs CaptureMotion::getDelta(const TimeStamp& _ts)
+Eigen::VectorXs CaptureMotion::getDelta(const VectorXs& _calib_current, const TimeStamp& _ts)
 {
     VectorXs calib_preint   = getCalibrationPreint();
-    VectorXs calib          = getCalibration();
     VectorXs delta_preint   = getBuffer().getMotion(_ts).delta_integr_;
     MatrixXs jac_calib      = getBuffer().getMotion(_ts).jacobian_calib_;
-    VectorXs delta_error    = jac_calib * (calib - calib_preint);
+    VectorXs delta_error    = jac_calib * (_calib_current - calib_preint);
     VectorXs delta          = correctDelta(delta_preint, delta_error);
     return delta;
 }

src/capture_motion.h

@@ -80,8 +80,8 @@ class CaptureMotion : public CaptureBase
         MatrixXs getJacobianCalib(const TimeStamp& _ts);
 
         // Get delta, corrected for changes on calibration params
-        VectorXs getDelta();
-        VectorXs getDelta(const TimeStamp& _ts);
+        VectorXs getDelta(const VectorXs& _calib_current);
+        VectorXs getDelta(const VectorXs& _calib_current, const TimeStamp& _ts);
         virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error);
 
         // Origin frame

src/processor_imu.h

@@ -165,7 +165,6 @@ inline void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
     Vector3s delta_v = a_dt;
     _delta << delta_p , delta_q.coeffs() , delta_v;
 
-
     /* Compute jacobian of delta wrt data
      *
      * MATHS : jacobian dd_dn, of delta wrt noise
@@ -245,7 +244,6 @@ inline void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
      */
 
     imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta);
-
 }
 
 inline void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,

src/processor_motion.h

@@ -472,7 +472,7 @@ inline void ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
     {
         // We found a CaptureMotion whose buffer contains the time stamp
         VectorXs state_0 = capture_motion->getOriginFramePtr()->getState();
-        VectorXs delta   = capture_motion->getDelta(_ts);
+        VectorXs delta   = capture_motion->getDelta(std::static_pointer_cast<CaptureMotion>(origin_ptr_)->getCalibration(), _ts);
         Scalar   dt      = _ts - capture_motion->getBuffer().get().front().ts_;
 
         statePlusDelta(state_0, delta, dt, _x);
@@ -496,7 +496,7 @@ inline Eigen::VectorXs ProcessorMotion::getCurrentState()
 inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
 {
     Scalar Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
-    statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDelta(), Dt, _x);
+    statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDelta(std::static_pointer_cast<CaptureMotion>(origin_ptr_)->getCalibration()), Dt, _x);
 }
 
 inline void ProcessorMotion::getCurrentStateAndStamp(Eigen::VectorXs& _x, TimeStamp& _ts)