diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index b5324fbc9f89b395470479f5249b34ae0d9403ab..863147d289cf33dfe4dc272df02d65a9f3dd70b3 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -1,3 +1,10 @@
+/*
+ * gtest_imu2d_static_init.cpp
+ *
+ * Created on: Sept 2021
+ * Author: igeer
+ */
+
#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
@@ -507,252 +514,18 @@ TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random)
TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
}
-//class ProcessorImu2dStaticInitTest : public testing::Test
-//{
-//
-// public: //These can be accessed in fixtures
-// wolf::ProblemPtr _problem_ptr;
-// wolf::SensorBasePtr _sensor_ptr;
-// wolf::ProcessorMotionPtr _processor_motion;
-// wolf::TimeStamp t;
-// wolf::TimeStamp t0;
-// double mti_clock, tmp;
-// double dt;
-// bool second_frame;
-// bool third_frame;
-// Vector6d data;
-// Matrix6d data_cov;
-// VectorComposite x0c; // initial state composite
-// VectorComposite s0c; // initial sigmas composite
-// std::shared_ptr<wolf::CaptureImu> C;
-// FrameBasePtr _first_frame;
-// FrameBasePtr _last_frame;
-// SolverCeresPtr _solver;
-//
-// //a new of this will be created for each new test
-// void SetUp() override
-// {
-// using namespace Eigen;
-// using std::shared_ptr;
-// using std::make_shared;
-// using std::static_pointer_cast;
-// using namespace wolf::Constants;
-//
-// std::string wolf_root = _WOLF_IMU_ROOT_DIR;
-//
-// // Wolf problem
-// _problem_ptr = Problem::create("POV", 2);
-// Vector3d extrinsics = (Vector3d() << 0,0, 0).finished();
-// _sensor_ptr = _problem_ptr->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d.yaml");
-// ProcessorBasePtr processor_ptr = _problem_ptr->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu2d_static_init.yaml");
-// _processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
-//
-// // Time and data variables
-// second_frame = false;
-// third_frame = false;
-// dt = 0.01;
-// t0.set(0);
-// t = t0;
-// data = Vector6d::Random();
-// data_cov = Matrix6d::Identity();
-// _last_frame = nullptr;
-//
-// // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
-// C = make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
-//
-// // Create the solver
-// _solver = make_shared<SolverCeres>(_problem_ptr);
-// }
-//
-//};
-//
-//
-///**
-// * SET TO TRUE TO PRODUCE CSV FILE
-// * SET TO FALSE TO STOP PRODUCING CSV FILE
-// */
-//#define WRITE_CSV_FILE true
-//
-//TEST_F(ProcessorImu2dStaticInitTest, static)
-//{
-// // Set the origin
-// x0c['P'] = Vector2d(0, 0);
-// x0c['O'] = Vector1d(0);
-// x0c['V'] = Vector2d(0, 0);
-//
-// data_cov *= 1e-3;
-// data << 1, 2, 3, 4, 5, 6;
-// Vector3d bias_groundtruth;
-// bias_groundtruth << data(0), data(1), data(5);
-// //dt = 0.0001;
-// auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
-// KF0->fix();
-// _processor_motion->setOrigin(KF0);
-//
-// WOLF_INFO("Data is: \n", data);
-// int size = 3;
-// second_frame = false;
-// _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 n_frames = 0;
-// for(t = t0+dt; t <= t0 + 3.1 + dt/2; t+=dt){
-// WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
-// ++i;
-// C = std::make_shared<CaptureImu>(t, _sensor_ptr, data, data_cov, Vector3d::Zero());
-// WOLF_INFO("Created new Capture");
-// if(second_frame){
-// //ASSERT_MATRIX_APPROX(_processor_motion->getState().vector("POV"), Vector5d::Zero(), 1e-9);
-// }
-// WOLF_INFO("Processing capture");
-// C->process();
-// WOLF_INFO("Doing the static initialization stuff");
-// if (not _last_frame)
-// {
-// n_frames++;
-// _last_frame = _processor_motion->getOrigin()->getFrame();
-// _first_frame = _last_frame;
-//
-// // fix Position and orientation
-// _last_frame->getP()->fix();
-// _last_frame->getO()->fix();
-//
-// // impose zero velocity
-// _last_frame->getV()->setZero();
-// _last_frame->getV()->fix();
-//
-// // impose zero odometry
-// _processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure()));
-// }
-// else
-// {
-// assert(_processor_motion->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
-// assert(_processor_motion->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
-//
-// // new frame
-// if (_last_frame != _processor_motion->getOrigin()->getFrame())
-// {
-// n_frames++;
-// second_frame = true;
-// _last_frame = _processor_motion->getOrigin()->getFrame();
-//
-// // fix Position and orientation
-// _last_frame->getP()->fix();
-// _last_frame->getO()->fix();
-//
-// // impose zero velocity
-// _last_frame->getV()->setZero();
-// _last_frame->getV()->fix();
-//
-// // impose zero odometry
-// _processor_motion->setOdometry(_sensor_ptr->getProblem()->stateZero(_processor_motion->getStateStructure()));
-//
-// // add zero displacement and rotation capture & feature & factor with all previous frames
-// assert(_sensor_ptr->getProblem());
-// Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
-// for (auto frm_pair = _sensor_ptr->getProblem()->getTrajectory()->begin();
-// frm_pair != _sensor_ptr->getProblem()->getTrajectory()->end();
-// frm_pair++)
-// {
-// if (frm_pair->second == _last_frame)
-// break;
-// auto capture_zero = CaptureBase::emplace<CaptureVoid>(_last_frame, _last_frame->getTimeStamp(), nullptr);
-// auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
-// "FeatureZeroOdom",
-// zero_motion,
-// Eigen::MatrixXd::Identity(size,size) * 0.01);
-//
-// FactorBase::emplace<FactorRelativePose2d>(feature_zero,
-// feature_zero,
-// frm_pair->second,
-// nullptr,
-// false,
-// TOP_MOTION);
-//
-// }
-// }
-// }
-// WOLF_INFO("Static initialization done");
-// if(_first_frame)
-// {
-// //WOLF_INFO("0 - 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("1 - The last (current) frame is: ", _last_frame->id(), " and it's state vector is: \n", _last_frame->getStateVector());
-// //WOLF_INFO("2 - The current frame's estimated bias is: \n", _last_frame->getCaptureOf(_sensor_ptr)->getStateBlock('I')->getState());
-// //WOLF_INFO("3 - The current state is (from _processor_motion): \n", _processor_motion->getState().vector("POV"));
-// }
-// //WOLF_INFO("The current problem is:");
-// //_problem_ptr->print(4);
-// auto state = _problem_ptr->getState();
-// auto bias_est = _sensor_ptr->getIntrinsic()->getState();
-// auto bias_preint = _processor_motion->getLast()->getCalibrationPreint();
-//
-// #if WRITE_CSV_FILE
-// // pre-solve print to CSV
-// file_est << std::fixed << t << ";"
-// << state['P'](0) << ";"
-// << state['V'](0) << ";"
-// << bias_est(0) << ";"
-// << bias_preint(0) << "\n";
-// #endif
-//
-// if(_problem_ptr->getTrajectory()->getFrameMap().size() > n_frames)
-// {
-// WOLF_INFO("Solving...");
-// std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
-//
-// state = _problem_ptr->getState();
-// bias_est = _sensor_ptr->getIntrinsic()->getState();
-// bias_preint = _processor_motion->getLast()->getCalibrationPreint();
-//
-// #if WRITE_CSV_FILE
-// // 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 << ";"
-// << state['P'](0) << ";"
-// << state['V'](0) << ";"
-// << 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");
-// }
-// }
-//}
-//
+
+TEST_F(ProcessorImu2dStaticInitTest, realistic_test)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << -0.529550648247,
+ 0.278316717683,
+ -0.00122491355676).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index 9c2bdbe07c0f8fef4c302d6ad47ab3d5ad13b523..7ad48b186079c12af08b6db42cc014207bc7bc38 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -1,3 +1,9 @@
+/*
+ * gtest_imu_static_init.cpp
+ *
+ * Created on: Sept 2021
+ * Author: igeer
+ */
#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h>
@@ -519,152 +525,6 @@ TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random)
TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false);
}
-//TEST_F(ProcessorImuStaticInitTest, static)
-//{
-// WOLF_INFO("Starting Test");
-//
-// data_cov *= 1e-3;
-// data << 1, 2, 3, 4, 5, 6;
-// data *= 0.01;
-// data.head(3) -= wolf::gravity();
-// sensor_ptr_->getIntrinsic(t0)->setState(data);
-// data << -wolf::gravity(), 0,0,0;
-// //Vector6d bias_groundtruth = data;
-// //TODO: Fix this, it works because initial position and extrinsics are 0, but gravity has to be preceded by the quaternion that takes it to the IMU reference
-// //WOLF_INFO("The Bias ground_truth is: \n", bias_groundtruth);
-//
-// //dt = 0.3;
-//
-// WOLF_INFO("Data is: \n", data);
-// //int size = 7;
-//
-// int i = 1;
-// int n_frames = 0;
-// for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
-// WOLF_INFO("Starting iteration ", i, " with timestamp ", t);
-// ++i;
-// auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov);
-// //WOLF_INFO("Created new Capture");
-// //WOLF_INFO("Processing capture");
-// C->process();
-// //WOLF_INFO("Doing the static initialization stuff");
-// if (not last_frame_)
-// {
-// n_frames++;
-// last_frame_ = processor_motion_->getOrigin()->getFrame();
-// first_frame_ = last_frame_;
-//
-// // fix Position and orientation
-// last_frame_->getP()->fix();
-// last_frame_->getO()->fix();
-//
-// // impose zero velocity
-// last_frame_->getV()->setZero();
-// last_frame_->getV()->fix();
-//
-// // impose zero odometry
-// processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
-// }
-// else
-// {
-// assert(processor_motion_->getOrigin() && "SubscriberImuEnableable::callback: nullptr origin");
-// assert(processor_motion_->getOrigin()->getFrame() && "SubscriberImuEnableable::callback: nullptr origin frame");
-//
-// // new frame
-// if (last_frame_ != processor_motion_->getOrigin()->getFrame())
-// {
-// n_frames++;
-// last_frame_ = processor_motion_->getOrigin()->getFrame();
-//
-// // TODO: We have to impose that the position is the same as for the first frame and fix it, and do the same for the orientation
-// // impose same Position
-// last_frame_->getP()->setState(first_frame_->getP()->getState());
-// last_frame_->getP()->fix();
-// //
-// // impose same Orientqation
-// last_frame_->getO()->setState(first_frame_->getO()->getState());
-// last_frame_->getO()->fix();
-//
-// // impose zero Velocity
-// last_frame_->getV()->setZero();
-// last_frame_->getV()->fix();
-//
-// // impose zero odometry
-// processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure()));
-//
-// // add zero displacement and rotation capture & feature & factor with all previous frames
-// //assert(sensor_ptr_->getProblem());
-// //Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
-// //zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
-//
-// //for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin();
-// // frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
-// // frm_pair++)
-// //{
-// // if (frm_pair->second == last_frame_)
-// // break;
-// // auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
-// // auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
-// // "FeatureZeroOdom",
-// // zero_motion,
-// // Eigen::MatrixXd::Identity(6,6) * 0.01);
-//
-// // FactorBase::emplace<FactorRelativePose3d>(feature_zero,
-// // feature_zero,
-// // frm_pair->second,
-// // nullptr,
-// // false,
-// // TOP_MOTION);
-// //
-// //}
-// }
-// }
-// //WOLF_INFO("Static initialization done");
-// if(first_frame_)
-// {
-// //WOLF_INFO("0 - 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("1 - The last (current) frame is: ", last_frame_->id(), " and it's state vector is: \n", last_frame_->getStateVector());
-// //WOLF_INFO("2 - The current frame's estimated bias is: \n", last_frame_->getCaptureOf(sensor_ptr_)->getStateBlock('I')->getState());
-// //WOLF_INFO("3 - The current state is (from processor_motion_): \n", processor_motion_->getState().vector("POV"));
-// }
-// WOLF_INFO("The current problem is:");
-// problem_ptr_->print(4);
-// //WOLF_INFO("Solving...");
-// std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
-// WOLF_INFO("Solver Report: ", report);
-//
-// auto state = problem_ptr_->getState();
-// auto bias_est = sensor_ptr_->getIntrinsic()->getState();
-// auto bias_preint = processor_motion_->getLast()->getCalibrationPreint();
-//
-// //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("Number of frames is ", n_frames, " with ID ", processor_motion_->getLast()->id());
-// WOLF_INFO("The state is: ", state);
-// WOLF_INFO("The estimated bias is: ", bias_est.transpose());
-// WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
-// }
-// if(n_frames > 1)
-// {
-// //EXPECT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-9);
-// //EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-9);
-// }
-// if (n_frames > 2)
-// {
-// //EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-9);
-// }
-// WOLF_INFO("------------------------------------------------------------------------\n");
-// }
-//}
int main(int argc, char **argv)
{