From 846e0553105b43c3a86596acf18a161efb562ea0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 3 Dec 2020 15:46:22 +0100
Subject: [PATCH] Add post-solve info to CSV file
---
.gitignore | 1 +
...est_processor_motion_intrinsics_update.cpp | 40 +++++++++++--------
2 files changed, 25 insertions(+), 16 deletions(-)
diff --git a/.gitignore b/.gitignore
index 3d1d237de..91c5366fd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
build_release/
IMU.found
+est.csv
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 2a50b49c3..3dad0c832 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/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;
--
GitLab