diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index 6ee9f27219f9019c0533135539099b245d204bc9..fc472d915512f54e76e7839d01065bc5ea9fb1a1 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -1,3 +1,4 @@
+#include <fstream>
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "imu/internal/config.h"
@@ -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)
{
// Set the origin
@@ -92,6 +99,15 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
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){
@@ -110,6 +126,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
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();
@@ -129,6 +149,10 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
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();
@@ -174,37 +198,63 @@ TEST_F(ProcessorImu2dStaticInitTest, static)
}
//WOLF_INFO("The current problem is:");
//_problem_ptr->print(4);
- WOLF_INFO("Solving...");
- std::string report = _solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
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("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(second_frame)
- {
- 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");
+ #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");
+ }
}
}
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index cfb7a446d358ed9668b5433d27625306da4c083a..65605c232f296fea9deaf75db4f9cceef38f49e2 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -1,3 +1,5 @@
+#include <fstream>
+
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "imu/internal/config.h"
@@ -11,6 +13,12 @@
using namespace Eigen;
using namespace wolf;
+/**
+ * SET TO TRUE TO PRODUCE CSV FILE
+ * SET TO FALSE TO STOP PRODUCING CSV FILE
+ */
+#define WRITE_CSV_FILE true
+
class ProcessorImuStaticInitTest : public testing::Test
{
@@ -23,7 +31,6 @@ class ProcessorImuStaticInitTest : public testing::Test
double dt;
Vector6d data;
Matrix6d data_cov;
- std::shared_ptr<wolf::CaptureImu> C;
FrameBasePtr KF0;
FrameBasePtr first_frame_;
FrameBasePtr last_frame_;
@@ -49,17 +56,14 @@ class ProcessorImuStaticInitTest : public testing::Test
processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
// Time and data variables
- dt = 0.01;
+ dt = 0.1;
t0.set(0);
t = t0;
data = Vector6d::Random();
- data_cov = Matrix6d::Identity();
+ data_cov = Matrix6d::Identity() * 1e-3;
last_frame_ = nullptr;
first_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_);
@@ -67,145 +71,303 @@ class ProcessorImuStaticInitTest : public testing::Test
VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()});
WOLF_INFO("x0c is: \n", x0c);
KF0 = problem_ptr_->setPriorFix(x0c, t0, dt/2);
- KF0->fix();
processor_motion_->setOrigin(KF0);
}
+ void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var)
+ {
+ //Data
+ data = body_magnitudes + bias_groundtruth;
+ data.head(3) -= Quaterniond(Vector4d(KF0->getO()->getState())).conjugate() * wolf::gravity();
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./est-"+test_name+".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;
+ if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
+ if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
+ if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ file_est << header_est;
+#endif
+
+
+ int n_frames = 0;
+ for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov);
+ C->process();
+
+ 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'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var+3) << "\n";
+#endif
+
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+ // impose static
+ last_frame_->getP()->setState(KF0->getP()->getState());
+ last_frame_->getO()->setState(KF0->getO()->getState());
+ last_frame_->getV()->setZero();
+
+ //Fix frame
+ last_frame_->fix();
+
+ //Solve
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO("Solver Report: ", report);
+
+ 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'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var+3) << "\n";
+#endif
+
+ }
+
+ //WOLF_INFO("The current problem is:");
+ //problem_ptr_->print(4);
+
+
+ WOLF_INFO("Number of frames is ", n_frames);
+ 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 == 2)
+ {
+ //ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-4);
+ //ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-6);
+ ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
};
-TEST_F(ProcessorImuStaticInitTest, static)
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
{
- WOLF_INFO("Starting Test");
-
- data_cov *= 1e-3;
- data << 1, 2, 3, 4, 5, 6;
- Vector6d bias_groundtruth;
- //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
- bias_groundtruth.head(3) = data.head(3) -wolf::gravity();
- bias_groundtruth.tail(3) = data.tail(3);
-
- //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;
- C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, Vector3d::Zero());
- 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_;
-
- // 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 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::BRIEF);
-
- 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("Current frame is ", n_frames, " with ID ", 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");
- }
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0);
}
+TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.1, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.1, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0);
+}
+
+//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)
{
testing::InitGoogleTest(&argc, argv);