From eb823f7a746d93c1d1a66617c2c10ddc43e9d561 Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Wed, 14 Dec 2016 12:26:00 +0100
Subject: [PATCH] correct getJacobians method
---
src/processor_imu.h | 10 +++++-----
1 file changed, 5 insertions(+), 5 deletions(-)
diff --git a/src/processor_imu.h b/src/processor_imu.h
index 98619262c..d2531d495 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -424,12 +424,12 @@ inline void ProcessorIMU::getJacobians(Eigen::Matrix<wolf::Scalar,9,6>& _dD_db)
[ dDv_dab dDv_dwb ]
*/
- _dD_db.block(3,3,0,0) = dDp_dab_;
- _dD_db.block(3,3,3,0) = Eigen::Matrix3s::Zero();
- _dD_db.block(3,3,6,0) = dDv_dab_;
- _dD_db.block(3,3,0,3) = dDp_dwb_;
+ _dD_db.block(0,0,3,3) = dDp_dab_;
+ _dD_db.block(3,0,3,3) = Eigen::Matrix3s::Zero();
+ _dD_db.block(6,0,3,3) = dDv_dab_;
+ _dD_db.block(0,3,3,3) = dDp_dwb_;
_dD_db.block(3,3,3,3) = dDq_dwb_;
- _dD_db.block(3,3,6,3) = dDv_dwb_;
+ _dD_db.block(6,3,3,3) = dDv_dwb_;
}
} // namespace wolf
--
GitLab