From 4f0cdb47e4d93441d8932acb6189020d32597675 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 22 Dec 2020 17:59:07 +0100
Subject: [PATCH] Fix ProcImu::getCalibration() to get the intrinsics
---
.../processor/processor_force_torque_preint.h | 2 +-
.../processor_force_torque_preint.cpp | 22 ++++++++++++++-----
2 files changed, 18 insertions(+), 6 deletions(-)
diff --git a/include/bodydynamics/processor/processor_force_torque_preint.h b/include/bodydynamics/processor/processor_force_torque_preint.h
index 3785468..cd38fb9 100644
--- a/include/bodydynamics/processor/processor_force_torque_preint.h
+++ b/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;
diff --git a/src/processor/processor_force_torque_preint.cpp b/src/processor/processor_force_torque_preint.cpp
index c73189a..17cb798 100644
--- a/src/processor/processor_force_torque_preint.cpp
+++ b/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;
}
--
GitLab