From 8275cc3b2af1dbf9e4907aef45d4c4c95f33a37e Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Wed, 14 Mar 2018 11:20:18 +0100
Subject: [PATCH] Fix IMU_tools commit 458e75b3 that went too far and broke two
functions
---
src/IMU_tools.h | 9 ++++-----
1 file changed, 4 insertions(+), 5 deletions(-)
diff --git a/src/IMU_tools.h b/src/IMU_tools.h
index 003c6f7ab..ebdf64df1 100644
--- a/src/IMU_tools.h
+++ b/src/IMU_tools.h
@@ -439,10 +439,10 @@ inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const
MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
{
+ diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
+
J_do_dq1 = - jac_SO3_left_inv(diff_o);
J_do_dq2 = jac_SO3_right_inv(diff_o);
-
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
}
@@ -483,6 +483,8 @@ inline void diff(const MatrixBase<D1>& d1,
Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+ diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+
/* d = diff(d1, d2) is
* dp = dp2 - dp1
* do = Log(dq1.conj * dq2)
@@ -498,9 +500,6 @@ inline void diff(const MatrixBase<D1>& d1,
J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
-
-
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
}
template<typename D1, typename D2>
--
GitLab