Skip to content
Snippets Groups Projects
Commit 62558489 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Improve code clarity in getDeltaCorrected()

parent 5e7679f0
No related branches found
No related tags found
1 merge request!145Imu improvements
This commit is part of merge request !145. Comments created here will be created in the context of that merge request.
...@@ -55,22 +55,23 @@ CaptureMotion::~CaptureMotion() ...@@ -55,22 +55,23 @@ CaptureMotion::~CaptureMotion()
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current) Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current)
{ {
VectorXs calib_preint = getCalibrationPreint(); VectorXs calib_preint = getCalibrationPreint();
VectorXs delta_preint = getBuffer().get().back().delta_integr_; VectorXs delta_preint = getBuffer().get().back().delta_integr_;
MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_; MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
VectorXs delta_error = jac_calib * (_calib_current - calib_preint); VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
VectorXs delta = correctDelta(delta_preint, delta_error); VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
return delta; return delta_corrected;
} }
Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts)
{ {
VectorXs calib_preint = getCalibrationPreint(); VectorXs calib_preint = getBuffer().getCalibrationPreint();
VectorXs delta_preint = getBuffer().getMotion(_ts).delta_integr_; Motion motion = getBuffer().getMotion(_ts);
MatrixXs jac_calib = getBuffer().getMotion(_ts).jacobian_calib_; VectorXs delta_preint = motion.delta_integr_;
VectorXs delta_error = jac_calib * (_calib_current - calib_preint); MatrixXs jac_calib = motion.jacobian_calib_;
VectorXs delta = correctDelta(delta_preint, delta_error); VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
return delta; VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
} }
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment