From 625584891729f8f30ecb85beea0de8859feb9b6d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 12 Nov 2017 23:25:38 +0100
Subject: [PATCH] Improve code clarity in getDeltaCorrected()
---
src/capture_motion.cpp | 25 +++++++++++++------------
1 file changed, 13 insertions(+), 12 deletions(-)
diff --git a/src/capture_motion.cpp b/src/capture_motion.cpp
index f5a5525ce..93203d74d 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;
}
}
--
GitLab