Calibration

Advance in the IMU code, and create a complete imu preintegration toolbox with imu_tools.h

src/test/gtest_processor_imu.cpp

@@ -215,24 +215,27 @@ TEST_F(ProcessorIMUt, interpolate)
 
     problem->getProcessorMotionPtr()->setOrigin(x0, t);
 
-    data << 2, 0, 0, 0, 0, 0; // only acc_x, but measure gravity!
+    data << 2, 0, 0, 0, 0, 0; // only acc_x
+    cap_imu_ptr->setData(data);
 
     // make one step to depart from origin
-    cap_imu_ptr->setData(data);
     cap_imu_ptr->setTimeStamp(0.05);
     sensor_ptr->process(cap_imu_ptr);
+    Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
 
     // make two steps, then pretend it's just one
-    Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
     cap_imu_ptr->setTimeStamp(0.10);
     sensor_ptr->process(cap_imu_ptr);
     Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
+
     cap_imu_ptr->setTimeStamp(0.15);
     sensor_ptr->process(cap_imu_ptr);
     Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
     mot_final.delta_ = mot_final.delta_integr_;
     Motion mot_sec = mot_final;
 
+    problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
+
 class P : wolf::ProcessorIMU
 {
     public:
@@ -246,6 +249,7 @@ Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
 
 ASSERT_MATRIX_APPROX(mot_int.data_,  mot_int_gt.data_, 1e-6);
 //ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
+ASSERT_MATRIX_APPROX(mot_final.delta_integr_,  mot_sec.delta_integr_, 1e-6);
 
 
 }