diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp
index 834d93b86cbdfe508f2e0b798d3abf0ed962af0c..21f46597a778b2faeda72c3aa12dd53e268bcadb 100644
--- a/src/test/gtest_processorIMU_jacobians.cpp
+++ b/src/test/gtest_processorIMU_jacobians.cpp
@@ -98,6 +98,80 @@ class ProcessorIMU_jacobians : public testing::Test
deltas_jac = deltas_jac_c;
}
+ virtual void TearDown()
+ {
+ // code here will be called just after the test completes
+ // ok to through exceptions from here if need be
+ /*
+ You can do deallocation of resources in TearDown or the destructor routine.
+ However, if you want exception handling you must do it only in the TearDown code because throwing an exception
+ from the destructor results in undefined behavior.
+ The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
+ Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
+ */
+ }
+};
+
+
+class ProcessorIMU_jacobians_Dq : public testing::Test
+{
+ public:
+ TimeStamp t;
+ Eigen::Vector6s data_;
+ Eigen::Matrix<wolf::Scalar,10,1> Delta0;
+ wolf::Scalar ddelta_bias2;
+ wolf::Scalar dt;
+ struct IMU_jac_bias bias_jac2;
+
+ void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
+
+ new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
+ new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
+ }
+
+ void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
+
+ assert(place < _jac_delta.Delta_noisy_vect_.size());
+ new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
+ new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
+ }
+
+ virtual void SetUp()
+ {
+ //SetUp for jacobians
+ wolf::Scalar deg_to_rad = M_PI/180.0;
+ data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
+
+ // Wolf problem
+ ProblemPtr wolf_problem_ptr_ = Problem::create(FRM_PQVBB_3D);
+ Eigen::VectorXs IMU_extrinsics(7);
+ IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
+
+ ProcessorIMU_UnitTester processor_imu;
+ ddelta_bias2 = 0.0001;
+ dt = 0.001;
+
+ //defining a random Delta to begin with (not to use Origin point)
+ Eigen::Matrix<wolf::Scalar,10,1> Delta0;
+ Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
+ Delta0.head<3>() = Delta0.head<3>()*100;
+ Delta0.tail<3>() = Delta0.tail<3>()*10;
+ Eigen::Vector3s ang0, ang;
+ ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
+
+ Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
+ Delta0_quat = v2q(ang0);
+ Delta0_quat.normalize();
+ ang = q2v(Delta0_quat);
+
+ //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
+ //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
+
+ //get data to compute jacobians
+ struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
+ bias_jac2.copyfrom(bias_jac_c);
+ }
+
virtual void TearDown()
{
// code here will be called just after the test completes
@@ -207,7 +281,7 @@ TEST_F(ProcessorIMU_jacobians, dDq_dab)
EXPECT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
}
-TEST_F(ProcessorIMU_jacobians, dDq_dwb)
+TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
{
using namespace wolf;
Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
@@ -219,8 +293,24 @@ TEST_F(ProcessorIMU_jacobians, dDq_dwb)
dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
}
- EXPECT_TRUE((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_ <<
- "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb << std::endl;
+ EXPECT_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_ <<
+ "\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_)
+{
+ using namespace wolf;
+ Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
+ Eigen::Matrix3s dDq_dwb;
+
+ new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac2.Delta0_.data() + 3);
+ for(int i=0;i<3;i++){
+ new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac2.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_bias2;
+ }
+
+ EXPECT_TRUE((dDq_dwb - bias_jac2.dDq_dwb_).isMuchSmallerThan(1,0.001)) << "dDq_dwb : \n" << dDq_dwb << "\n bias_jac2.dDq_dwb_ :\n" << bias_jac2.dDq_dwb_ <<
+ "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac2.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
}
///Perturbation TESTS