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

Do not correct delta in CaptureMotion

parent bfa42056
No related branches found
No related tags found
1 merge request!366Resolve "Complete state vector new data structure?"
......@@ -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;
......
......@@ -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
......
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