Add post-solve info to CSV file

.gitignore

@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
 build_release/
 
 IMU.found
+est.csv

test/gtest_processor_motion_intrinsics_update.cpp

@@ -167,8 +167,9 @@ TEST_F(ProcessorImuTest, getState)
 
 
     std::fstream file_est; 
-    file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/est.csv", std::fstream::out); 
-    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bax_preint\n";
+    file_est.open("./est.csv", std::fstream::out);
+//    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
+    std::string header_est = "t;px;vx;bax_est;bax_preint\n";
     file_est << header_est;
 
 
@@ -176,22 +177,19 @@ TEST_F(ProcessorImuTest, getState)
         auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); 
         C->process();
         VectorComposite state = problem_->getState(t);
-        VectorXd calib = sensor_->getIntrinsic(t)->getState();
+        VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
         VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
-        std::cout << "calib size: " << calib.size() << std::endl;
+        std::cout << "calib size: " << calib_estim.size() << std::endl;
         std::cout << "calib_preint size: " << calib_preint.size() << std::endl;
-        file_est << t << ","
-            << state['P'](0) << ","
-            << state['P'](1) << ","
-            << state['P'](2) << ","
-            << state['O'](0) << ","
-            << state['O'](1) << ","
-            << state['O'](2) << ","
-            << state['O'](3) << ","
-            << state['V'](0) << ","
-            << state['V'](1) << ","
-            << state['V'](2) << ","
-            << calib(0) << ","
+
+        std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts " << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
+
+
+        // pre-solve print to CSV
+        file_est << std::fixed << t << ";"
+            << state['P'](0) << ";"
+            << state['V'](0) << ";"
+            << calib_estim(0) << ";"
             << calib_preint(0) << "\n";
 
 
@@ -205,6 +203,16 @@ TEST_F(ProcessorImuTest, getState)
             problem_->print(4, true, true, true);
             nb_kf++;
 
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            VectorComposite state = problem_->getState(t);
+            VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
+            VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
+            file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](0) << ";"
+                << state['V'](0) << ";"
+                << calib_estim(0) << ";"
+                << calib_preint(0) << "\n";
+
         }
 
         t += dt;