diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h index 86331e5154e89f75d4cad0a03b71376bdfee5bf3..1a815041b3179026544065d9524519b1c1daab15 100644 --- a/include/core/capture/capture_motion.h +++ b/include/core/capture/capture_motion.h @@ -88,8 +88,6 @@ class CaptureMotion : public CaptureBase MatrixXd getJacobianCalib(const TimeStamp& _ts) const; // Get delta preintegrated, and corrected for changes on calibration params - VectorXd getDeltaCorrected(const VectorXd& _calib_current) const; - VectorXd getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const; VectorXd getDeltaPreint() const; VectorXd getDeltaPreint(const TimeStamp& _ts) const; MatrixXd getDeltaPreintCov() const; diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index d3bae98554c0d15257c19e04e7cfb9c11bb6d1b3..022d7a8187499bb353beaa915c97b87e6cc12a61 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -51,26 +51,6 @@ CaptureMotion::~CaptureMotion() // } -Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const -{ - VectorXd calib_preint = getCalibrationPreint(); - VectorXd delta_preint = getBuffer().back().delta_integr_; - MatrixXd jac_calib = getBuffer().back().jacobian_calib_; - VectorXd delta_error = jac_calib * (_calib_current - calib_preint); - VectorXd delta_corrected = correctDelta(delta_preint, delta_error); - return delta_corrected; -} - -Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const -{ - Motion motion = getBuffer().getMotion(_ts); - VectorXd delta_preint = motion.delta_integr_; - MatrixXd jac_calib = motion.jacobian_calib_; - VectorXd delta_error = jac_calib * (_calib_current - calib_preint_); - VectorXd delta_corrected = correctDelta(delta_preint, delta_error); - return delta_corrected; -} - bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) { // the same capture is within tolerance