diff --git a/.gitignore b/.gitignore index 3d1d237de284515d210782928bd15eb347decf99..91c5366fd823c89dcb9024788ac8c419f5337c52 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 2a50b49c3c0b58169ce836a3260c89b7229ade19..3dad0c8324bffeeb2c57855b431537ae842fe249 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;