Fix ProcImu::getCalibration() to get the intrinsics

include/bodydynamics/processor/processor_force_torque_preint.h

@@ -71,7 +71,7 @@ class ProcessorForceTorquePreint : public ProcessorMotion{
         Eigen::VectorXd deltaZero() const override;
         Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
                                              const Eigen::VectorXd& delta_step) const override;
-        VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+        VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
         void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
 
         bool voteForKeyFrame() const override;

src/processor/processor_force_torque_preint.cpp

@@ -121,15 +121,27 @@ Eigen::VectorXd ProcessorForceTorquePreint::correctDelta (const Eigen::VectorXd&
 
 VectorXd ProcessorForceTorquePreint::getCalibration (const CaptureBasePtr _capture) const
 {
+
     VectorXd bias_vec(6);
-    CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
 
-    // get calib part from Ikin capture
-    bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
+    if (_capture) // access from capture is quicker
+    {
+        CaptureForceTorquePreintPtr cap_ft(std::static_pointer_cast<CaptureForceTorquePreint>(_capture));
 
-    // get calib part from IMU capture
-    bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+        // get calib part from Ikin capture
+        bias_vec.segment<3>(0) = cap_ft->getIkinCaptureOther()->getSensorIntrinsic()->getState();
 
+        // get calib part from IMU capture
+        bias_vec.segment<3>(3) = cap_ft->getGyroCaptureOther()->getSensorIntrinsic()->getState().tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+    }
+    else // access from sensor is slower
+    {
+        // get calib part from Ikin capture
+        bias_vec.segment<3>(0) = sensor_ikin_->getIntrinsic()->getState();
+
+        // get calib part from IMU capture
+        bias_vec.segment<3>(3) = sensor_angvel_->getIntrinsic()->getState().tail<3>();  // TODO: only valib for IMU bias sb or any other of the structure [XXXX, gyro_bias]
+    }
     return bias_vec;
 }