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

Use dynamic state blocks from Sensor

parent 505ca347
No related branches found
No related tags found
3 merge requests!31devel->main,!29Add functionality for UAV,!27Fix test with acc_x = 2 m/s2
...@@ -149,9 +149,9 @@ inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(cons ...@@ -149,9 +149,9 @@ inline FactorForceTorqueInertialDynamics::FactorForceTorqueInertialDynamics(cons
_ftr->getFrame()->getStateBlock('O'), // current frame O _ftr->getFrame()->getStateBlock('O'), // current frame O
_ftr->getFrame()->getStateBlock('V'), // current frame V _ftr->getFrame()->getStateBlock('V'), // current frame V
_ftr->getFrame()->getStateBlock('L'), // current frame L _ftr->getFrame()->getStateBlock('L'), // current frame L
_capture_origin->getSensor()->getStateBlock('C'), // sensor C _ftr->getCapture()->getStateBlock('C'), // sensor C
_capture_origin->getSensor()->getStateBlock('i'), // sensor i _ftr->getCapture()->getStateBlock('i'), // sensor i
_capture_origin->getSensor()->getStateBlock('m')), // sensor m _ftr->getCapture()->getStateBlock('m')), // sensor m
dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()), dt_(_ftr->getFrame()->getTimeStamp() - _capture_origin->getFrame()->getTimeStamp()),
delta_preint_(_ftr->getDeltaPreint()), delta_preint_(_ftr->getDeltaPreint()),
calib_preint_(_ftr->getCalibrationPreint()), calib_preint_(_ftr->getCalibrationPreint()),
...@@ -238,9 +238,9 @@ inline VectorXd FactorForceTorqueInertialDynamics::residual() const ...@@ -238,9 +238,9 @@ inline VectorXd FactorForceTorqueInertialDynamics::residual() const
Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data()); Quaterniond q1 = Quaterniond(getFrame()->getStateBlock('O')->getState().data());
Vector3d v1 = getFrame()->getStateBlock('V')->getState(); Vector3d v1 = getFrame()->getStateBlock('V')->getState();
Vector3d L1 = getFrame()->getStateBlock('L')->getState(); Vector3d L1 = getFrame()->getStateBlock('L')->getState();
Vector3d C = getCapture()->getSensor()->getStateBlock('C')->getState(); Vector3d C = getCapture()->getSensor()->getStateBlockDynamic('C')->getState();
Vector3d i = getCapture()->getSensor()->getStateBlock('i')->getState(); Vector3d i = getCapture()->getSensor()->getStateBlockDynamic('i')->getState();
double m = getCapture()->getSensor()->getStateBlock('m')->getState()(0); double m = getCapture()->getSensor()->getStateBlockDynamic('m')->getState()(0);
residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res); residual(p0, q0, v0, L0, bias, p1, q1, v1, L1, C, i, m, res);
return res; return res;
......
...@@ -18,7 +18,7 @@ CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp& _i ...@@ -18,7 +18,7 @@ CaptureForceTorqueInertial::CaptureForceTorqueInertial(const TimeStamp& _i
nullptr, // position is static nullptr, // position is static
nullptr, // orientation is static nullptr, // orientation is static
(_sensor_ptr->isStateBlockDynamic('I') // dynamic intrinsics are IMU bias (_sensor_ptr->isStateBlockDynamic('I') // dynamic intrinsics are IMU bias
? std::make_shared<StateParams6>(_sensor_ptr->getStateBlock('I')->getState(), false) ? std::make_shared<StateParams6>(_sensor_ptr->getStateBlockDynamic('I')->getState(), false)
: nullptr)) : nullptr))
{ {
// //
......
...@@ -469,10 +469,10 @@ VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseC ...@@ -469,10 +469,10 @@ VectorXd ProcessorForceTorqueInertialDynamics::getCalibration(const CaptureBaseC
else else
{ {
VectorXd calibration(13); VectorXd calibration(13);
const Vector6d& I_calib = getSensor()->getStateBlock('I')->getState(); const Vector6d& I_calib = getSensor()->getStateBlockDynamic('I')->getState();
const Vector3d& C_calib = getSensor()->getStateBlock('C')->getState(); const Vector3d& C_calib = getSensor()->getStateBlockDynamic('C')->getState();
const Vector3d& i_calib = getSensor()->getStateBlock('i')->getState(); const Vector3d& i_calib = getSensor()->getStateBlockDynamic('i')->getState();
const Vector1d& m_calib = getSensor()->getStateBlock('m')->getState(); const Vector1d& m_calib = getSensor()->getStateBlockDynamic('m')->getState();
calibration << I_calib, C_calib, i_calib, m_calib; calibration << I_calib, C_calib, i_calib, m_calib;
return calibration; return calibration;
} }
......
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