From 21a601e83d883ebbaffcaa1e9dfa61e9a73bf84d Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Thu, 8 Dec 2016 14:11:11 +0100
Subject: [PATCH] add test dDo_dO
---
src/test/gtest_processorIMU_jacobians.cpp | 20 ++++++++++++++++++++
1 file changed, 20 insertions(+)
diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp
index 309c7ca53..494de2579 100644
--- a/src/test/gtest_processorIMU_jacobians.cpp
+++ b/src/test/gtest_processorIMU_jacobians.cpp
@@ -374,6 +374,26 @@ TEST_F(ProcessorIMU_jacobians, dDv_dO)
"\ndDv_dO_a - dDv_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO << std::endl;
}
+//dDo_dP = dDo_dV = [0, 0, 0]
+
+
+TEST_F(ProcessorIMU_jacobians, dDo_dO)
+{
+ using namespace wolf;
+ Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
+ Eigen::Matrix3s dDo_dO;
+
+ //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
+ remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
+ for(int i=0;i<3;i++){
+ remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
+ dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
+ }
+
+ EXPECT_TRUE((dDo_dO - deltas_jac.jacobian_delta_preint_.block(3,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDo_dO : \n" << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) <<
+ "\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
+}
+
TEST_F(ProcessorIMU_jacobians, dDp_dp)
{
using namespace wolf;
--
GitLab