From 6abbde3b80d3faa873ace5e89ae5f2d10bc6ed38 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 4 Dec 2020 15:21:36 +0100
Subject: [PATCH] Make CSV file optional, disable by default
---
...est_processor_motion_intrinsics_update.cpp | 34 +++++++++++--------
1 file changed, 20 insertions(+), 14 deletions(-)
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 3dad0c832..4eb93433b 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -151,10 +151,11 @@ TEST_F(ProcessorImuTest, test1)
+//#define WRITE_CSV_FILE // COMMENT OUT THIS LINE TO AVOID PRODUCING CSV FILE
TEST_F(ProcessorImuTest, getState)
{
Matrix6d data_cov; data_cov. setIdentity();
- // Advances at constant speed on both wheels
+ // Advances at constant acceleration
double AX = 0;
problem_->print(4,true,true,true);
@@ -165,33 +166,33 @@ TEST_F(ProcessorImuTest, getState)
double dt(0.1);
int nb_kf = 1;
-
+#ifdef WRITE_CSV_FILE
std::fstream file_est;
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;
-
+#endif
while (t < 4){
auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front());
C->process();
- VectorComposite state = problem_->getState(t);
- VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
- VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
- std::cout << "calib size: " << calib_estim.size() << std::endl;
- std::cout << "calib_preint size: " << calib_preint.size() << std::endl;
+
std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts " << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
+#ifdef WRITE_CSV_FILE
// pre-solve print to CSV
+ VectorComposite state = problem_->getState(t);
+ VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
+ VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
file_est << std::fixed << t << ";"
<< state['P'](0) << ";"
<< state['V'](0) << ";"
<< calib_estim(0) << ";"
<< calib_preint(0) << "\n";
-
+#endif
if (problem_->getTrajectory()->getFrameMap().size() > nb_kf){
@@ -201,23 +202,28 @@ TEST_F(ProcessorImuTest, getState)
solver_->solve();
problem_->print(4, true, true, true);
- nb_kf++;
+ nb_kf = problem_->getTrajectory()->getFrameMap().size();
+
+#ifdef WRITE_CSV_FILE
// 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();
+ state = problem_->getState(t);
+ calib_estim = sensor_->getIntrinsic(t)->getState();
+ 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";
-
+#endif
}
t += dt;
}
+#ifdef WRITE_CSV_FILE
+ file_est.close();
+#endif
}
--
GitLab