[WIP] gtest_processorIMU_jacobians - got bias_jac

src/test/gtest_processorIMU_jacobians.cpp

@@ -35,6 +35,7 @@ class ProcessorIMU_jacobians : public testing::Test
     public:
     TimeStamp t;
     Eigen::Vector6s data_;
+    struct IMU_jac_bias bias_jac;
 
     virtual void SetUp()
     {
@@ -47,6 +48,38 @@ class ProcessorIMU_jacobians : public testing::Test
         IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
         //SensorBase* sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, nullptr);
         //wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+
+        // Set the origin
+        t.set(0.0001); // clock in 0,1 ms ticks
+        Eigen::VectorXs x0(16);
+        x0 << 0,1,0,  0,0,0,1,  1,0,0,  0,0,.000,  0,0,.000; // P Q V B B
+
+        //wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+        //CaptureIMU* imu_ptr;
+
+        ProcessorIMU_UnitTester processor_imu;
+        //processor_imu.setOrigin(x0, t);
+        wolf::Scalar ddelta_bias = 0.00000001;
+        wolf::Scalar 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.segment<3>(3) = Delta0.segment<3>(3)*10;
+        Eigen::Vector3s ang0, ang;
+        ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad; 
+        //Delta0 << 0,0,0, 1,0,0,0 ,0,0,0; 
+        Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+6);
+        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;
+
+        struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
+        bias_jac = bias_jac_c;
     }
 
     virtual void TearDown(){}