diff --git a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp index 015839adb79a10fc5dd6d03af1698052769441c5..89622044b57cf5f048325a6825a0b6da594be72e 100644 --- a/src/processor/processor_force_torque_inertial_preint_dynamics.cpp +++ b/src/processor/processor_force_torque_inertial_preint_dynamics.cpp @@ -108,7 +108,10 @@ FactorBasePtr ProcessorForceTorqueInertialPreintDynamics::emplaceFactor(FeatureB void ProcessorForceTorqueInertialPreintDynamics::setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) { - _capture->getStateBlock('I')->setState(_calibration); // Set IMU bias + _capture->getStateBlock('I')->setState(_calibration.segment<6>(0)); // Set IMU bias + _capture->getStateBlock('C')->setState(_calibration.segment<3>(6)); // Set C + _capture->getStateBlock('i')->setState(_calibration.segment<3>(9)); // Set i + _capture->getStateBlock('m')->setState(_calibration.segment<1>(12)); // Set m } void ProcessorForceTorqueInertialPreintDynamics::configure(SensorBasePtr _sensor) @@ -181,9 +184,26 @@ Eigen::VectorXd ProcessorForceTorqueInertialPreintDynamics::correctDelta(const E VectorXd ProcessorForceTorqueInertialPreintDynamics::getCalibration(const CaptureBaseConstPtr _capture) const { if (_capture) - return _capture->getStateBlock('I')->getState(); // IMU bias + { //return _capture->getStateBlock('I')->getState(); // IMU bia + VectorXd calibration(13); + const Vector6d& I_calib = _capture->getStateBlock('I')->getState(); + const Vector3d& C_calib = _capture->getStateBlock('C')->getState(); + const Vector3d& i_calib = _capture->getStateBlock('i')->getState(); + const Vector1d& m_calib = _capture->getStateBlock('m')->getState(); + calibration << I_calib, C_calib, i_calib, m_calib; + return calibration; + } else - return getSensor()->getStateBlock('I')->getState(); // IMU bias + //return getSensor()->getStateBlock('I')->getState(); // IMU bias + { + VectorXd calibration(13); + const Vector6d& I_calib = getSensor()->getStateBlock('I')->getState(); + const Vector3d& C_calib = getSensor()->getStateBlock('C')->getState(); + const Vector3d& i_calib = getSensor()->getStateBlock('i')->getState(); + const Vector1d& m_calib = getSensor()->getStateBlock('m')->getState(); + calibration << I_calib, C_calib, i_calib, m_calib; + return calibration; + } } } /* namespace wolf */ diff --git a/test/gtest_factor_force_torque_inertial_dynamics.cpp b/test/gtest_factor_force_torque_inertial_dynamics.cpp index fd3587e25fc89551c564d59d77b45b7960cc6c5c..af4888344ba6e6ca4312e03475020a226ba4129c 100644 --- a/test/gtest_factor_force_torque_inertial_dynamics.cpp +++ b/test/gtest_factor_force_torque_inertial_dynamics.cpp @@ -25,6 +25,8 @@ #include "bodydynamics/sensor/sensor_force_torque_inertial.h" #include "bodydynamics/internal/config.h" +#include <core/sensor/factory_sensor.h> + #include <core/utils/utils_gtest.h> #include <core/utils/logging.h> #include <core/yaml/parser_yaml.h> @@ -169,6 +171,7 @@ class Test_FactorForceTorqueInertialDynamics : public testing::Test TEST_F(Test_FactorForceTorqueInertialDynamics_yaml, constructor) { + //FactorySensor::registerCreator("SensorForceTorqueInertial", SensorForceTorqueInertial::create); ASSERT_EQ(c1->getCapture(), C1); ASSERT_EQ(c1->getCalibPreint().size(), 13); }