From 2f3da7a6519e72ebb881550a17cf7a94ce0e2818 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Tue, 13 Dec 2016 15:50:31 +0100
Subject: [PATCH] jacobians getter
---
src/processor_imu.h | 13 ++++++++++++-
1 file changed, 12 insertions(+), 1 deletion(-)
diff --git a/src/processor_imu.h b/src/processor_imu.h
index c20f141b1..11dbe7776 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -79,6 +79,8 @@ class ProcessorIMU : public ProcessorMotion{
virtual void remapPQV(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, Eigen::VectorXs& _delta_out);
virtual void remapDelta(Eigen::VectorXs& _delta_out);
virtual void remapData(const Eigen::VectorXs& _data);
+ void getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb);
+
public:
@@ -405,7 +407,16 @@ inline void ProcessorIMU::remapData(const Eigen::VectorXs& _data)
new (&gyro_measured_) Eigen::Map<const Eigen::Vector3s>(_data.data() + 3);
}
-/*void ProcessorIMU::getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb)
+inline void ProcessorIMU::getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb)
+{
+ _dDp_dab = dDp_dab_;
+ _dDv_dab = dDv_dab_;
+ _dDp_dwb = dDp_dwb_;
+ _dDv_dwb = dDv_dwb_;
+ _dDq_dwb = dDq_dwb_;
+}
+
+/*inline void ProcessorIMU::getJacobians(Eigen::Matrix9s& _dD_db)
{
_dDp_dab = dDp_dab_;
_dDv_dab = dDv_dab_;
--
GitLab