all fixed

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