Some bugs fixed

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 */
 

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);
 }