diff --git a/src/capture_motion.cpp b/src/capture_motion.cpp index f5a5525ceb8f75592746b672dcddfa83a207042b..93203d74dffb7ade049df63a2874d3882082e320 100644 --- a/src/capture_motion.cpp +++ b/src/capture_motion.cpp @@ -55,22 +55,23 @@ CaptureMotion::~CaptureMotion() Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current) { - VectorXs calib_preint = getCalibrationPreint(); - VectorXs delta_preint = getBuffer().get().back().delta_integr_; - MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_; - VectorXs delta_error = jac_calib * (_calib_current - calib_preint); - VectorXs delta = correctDelta(delta_preint, delta_error); - return delta; + VectorXs calib_preint = getCalibrationPreint(); + VectorXs delta_preint = getBuffer().get().back().delta_integr_; + MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_; + VectorXs delta_error = jac_calib * (_calib_current - calib_preint); + VectorXs delta_corrected = correctDelta(delta_preint, delta_error); + return delta_corrected; } Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) { - VectorXs calib_preint = getCalibrationPreint(); - VectorXs delta_preint = getBuffer().getMotion(_ts).delta_integr_; - MatrixXs jac_calib = getBuffer().getMotion(_ts).jacobian_calib_; - VectorXs delta_error = jac_calib * (_calib_current - calib_preint); - VectorXs delta = correctDelta(delta_preint, delta_error); - return delta; + VectorXs calib_preint = getBuffer().getCalibrationPreint(); + Motion motion = getBuffer().getMotion(_ts); + VectorXs delta_preint = motion.delta_integr_; + MatrixXs jac_calib = motion.jacobian_calib_; + VectorXs delta_error = jac_calib * (_calib_current - calib_preint); + VectorXs delta_corrected = correctDelta(delta_preint, delta_error); + return delta_corrected; } }