Skip to content
Snippets Groups Projects
Commit 1c242954 authored by Idril-Tadzio Geer Cousté's avatar Idril-Tadzio Geer Cousté
Browse files

refactored test for readability

parent 484516be
No related branches found
No related tags found
4 merge requests!39release after RAL,!38After 2nd RAL submission,!33Imu2d static initialization,!31Imu2d static initialization
#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h> #include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h> #include <core/utils/utils_gtest.h>
#include "imu/internal/config.h" #include "imu/internal/config.h"
...@@ -71,6 +72,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test ...@@ -71,6 +72,12 @@ class ProcessorImu2dStaticInitTest : public testing::Test
}; };
/**
* SET TO TRUE TO PRODUCE CSV FILE
* SET TO FALSE TO STOP PRODUCING CSV FILE
*/
#define WRITE_CSV_FILE true
TEST_F(ProcessorImu2dStaticInitTest, static) TEST_F(ProcessorImu2dStaticInitTest, static)
{ {
// Set the origin // Set the origin
...@@ -92,6 +99,15 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -92,6 +99,15 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
second_frame = false; second_frame = false;
_first_frame = nullptr; _first_frame = nullptr;
#if 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
int i = 1; int i = 1;
int n_frames = 0; int n_frames = 0;
for(t = t0+dt; t <= t0 + 3.1 + dt/2; t+=dt){ for(t = t0+dt; t <= t0 + 3.1 + dt/2; t+=dt){
...@@ -110,6 +126,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -110,6 +126,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
n_frames++; n_frames++;
_last_frame = _processor_motion->getOrigin()->getFrame(); _last_frame = _processor_motion->getOrigin()->getFrame();
_first_frame = _last_frame; _first_frame = _last_frame;
// fix Position and orientation
_last_frame->getP()->fix();
_last_frame->getO()->fix();
// impose zero velocity // impose zero velocity
_last_frame->getV()->setZero(); _last_frame->getV()->setZero();
...@@ -129,6 +149,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -129,6 +149,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
n_frames++; n_frames++;
second_frame = true; second_frame = true;
_last_frame = _processor_motion->getOrigin()->getFrame(); _last_frame = _processor_motion->getOrigin()->getFrame();
// fix Position and orientation
_last_frame->getP()->fix();
_last_frame->getO()->fix();
// impose zero velocity // impose zero velocity
_last_frame->getV()->setZero(); _last_frame->getV()->setZero();
...@@ -174,37 +198,63 @@ TEST_F(ProcessorImu2dStaticInitTest, static) ...@@ -174,37 +198,63 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
} }
//WOLF_INFO("The current problem is:"); //WOLF_INFO("The current problem is:");
//_problem_ptr->print(4); //_problem_ptr->print(4);
WOLF_INFO("Solving...");
std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
auto state = _problem_ptr->getState(); auto state = _problem_ptr->getState();
auto bias_est = _sensor_ptr->getIntrinsic()->getState(); auto bias_est = _sensor_ptr->getIntrinsic()->getState();
auto bias_preint = _processor_motion->getLast()->getCalibrationPreint(); auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
WOLF_INFO("Solved"); #if WRITE_CSV_FILE
if(_first_frame) // pre-solve print to CSV
{ file_est << std::fixed << t << ";"
//WOLF_INFO("4 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState()); << state['P'](0) << ";"
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector()); << state['V'](0) << ";"
//WOLF_INFO_COND(second_frame, "The second frame has been created"); << bias_est(0) << ";"
//WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector()); << bias_preint(0) << "\n";
//WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState()); #endif
//WOLF_INFO("7 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
WOLF_INFO("Current frame is", _processor_motion->getLast()->id()); if(_problem_ptr->getTrajectory()->getFrameMap().size() > n_frames)
WOLF_INFO("The state is \n:", state); {
WOLF_INFO("The estimated bias is: \n", bias_est); WOLF_INFO("Solving...");
WOLF_INFO("The preintegrated bias is: \n", bias_preint); std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
}
if(second_frame) state = _problem_ptr->getState();
{ bias_est = _sensor_ptr->getIntrinsic()->getState();
ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9); bias_preint = _processor_motion->getLast()->getCalibrationPreint();
ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
} #if WRITE_CSV_FILE
if (n_frames > 2) // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
{ file_est << std::fixed << t+dt/2 << ";"
ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9); << state['P'](0) << ";"
} << state['V'](0) << ";"
WOLF_INFO("------------------------------------------------------------------------\n"); << bias_est(0) << ";"
<< bias_preint(0) << "\n";
#endif
WOLF_INFO("Solved");
if(_first_frame)
{
//WOLF_INFO("4 - The first frame is ", _first_frame->id(), " and it's currently estimated bias is: \n", _first_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("its state vector is: \n", _first_frame->getStateVector());
//WOLF_INFO_COND(second_frame, "The second frame has been created");
//WOLF_INFO("5 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
//WOLF_INFO("6 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
//WOLF_INFO("7 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
WOLF_INFO("Current frame is", _processor_motion->getLast()->id());
WOLF_INFO("The state is \n:", state);
WOLF_INFO("The estimated bias is: \n", bias_est);
WOLF_INFO("The preintegrated bias is: \n", bias_preint);
}
if(n_frames > 1)
{
ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
}
if (n_frames > 2)
{
ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
}
WOLF_INFO("------------------------------------------------------------------------\n");
}
} }
} }
......
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment