From 9e84f299c771c5072d7d3b8f514dceb89123759f Mon Sep 17 00:00:00 2001
From: Dinesh Atchuthan <datchuth@laas.fr>
Date: Tue, 29 Nov 2016 12:57:36 +0100
Subject: [PATCH] [WIP] gtest_processorIMU_jacobians - got bias_jac
---
src/test/gtest_processorIMU_jacobians.cpp | 33 +++++++++++++++++++++++
1 file changed, 33 insertions(+)
diff --git a/src/test/gtest_processorIMU_jacobians.cpp b/src/test/gtest_processorIMU_jacobians.cpp
index 6762c5964..c6c0352b3 100644
--- a/src/test/gtest_processorIMU_jacobians.cpp
+++ b/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(){}
--
GitLab