Skip to content
Snippets Groups Projects
Commit 99f8e483 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Test that was not passing now passes with new Eigen.

parent c9bb8c6a
No related branches found
No related tags found
No related merge requests found
...@@ -288,13 +288,19 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_) ...@@ -288,13 +288,19 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
Eigen::Matrix3s dDq_dwb; Eigen::Matrix3s dDq_dwb;
new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3); new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
for(int i=0;i<3;i++){ for(int i=0;i<3;i++){
new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3); new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias; dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
} }
ASSERT_FALSE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001)) << "dDq_dwb : \n" << dDq_dwb << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_ << ASSERT_TRUE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001))
"\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl; << "dDq_dwb : \n" << dDq_dwb
<< "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_
<< "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb
<< "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix()
<< std::endl;
} }
TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_) TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
...@@ -547,4 +553,4 @@ int main(int argc, char **argv) ...@@ -547,4 +553,4 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment