diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index a7ff2d7b063657ec39fff82552fceedf82999847..0d52b68b7a005597b958d90a334a620819f581fe 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -4,6 +4,7 @@
//Wolf includes
#include "imu/feature/feature_imu.h"
#include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu.h"
#include "core/factor/factor_autodiff.h"
#include "core/math/rotations.h"
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d16b4a3857c88316a48fa5cc79f77a6bc4f7b40d..715b11aa464d8db46a596d80b36e312cc4f51c46 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -37,6 +37,12 @@ target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp)
+target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
+wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
+target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b5324fbc9f89b395470479f5249b34ae0d9403ab
--- /dev/null
+++ b/test/gtest_imu2d_static_init.cpp
@@ -0,0 +1,761 @@
+#include <fstream>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+#include "imu/internal/config.h"
+
+#include "imu/factor/factor_imu2d.h"
+#include "imu/math/imu2d_tools.h"
+#include "imu/sensor/sensor_imu2d.h"
+#include "core/capture/capture_void.h"
+#include <core/factor/factor_relative_pose_2d.h>
+
+using namespace Eigen;
+using namespace wolf;
+
+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 dt;
+ Vector6d data;
+ Matrix6d data_cov;
+ FrameBasePtr KF0_;
+ FrameBasePtr first_frame_;
+ FrameBasePtr last_frame_;
+ SolverCeresPtr solver_;
+
+ //a new of this will be created for each new test
+ void SetUp() override
+ {
+ WOLF_INFO("Doing setup...");
+ 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 + "/test/yaml/sensor_imu2d_static_init.yaml");
+ ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu2d_static_init.yaml");
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+ // Time and data variables
+ dt = 0.1;
+ t0.set(0);
+ t = t0;
+ data = Vector6d::Random();
+ data_cov = Matrix6d::Identity();
+ last_frame_ = nullptr;
+ first_frame_ = nullptr;
+
+ // Create the solver
+ solver_ = make_shared<SolverCeres>(problem_ptr_);
+
+ // Set the origin
+ VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()});
+ VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()});
+ //KF0_ = problem_ptr_->setPriorFix(x_origin, t0, dt/2);
+ KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+ }
+
+ void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data.head(2) = body_magnitudes.head(2);
+ data(5) = body_magnitudes(2);
+ data.head(2) += bias_groundtruth.head(2);
+ data(5) += bias_groundtruth(2);
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./est2d-"+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;o;bax_est;bg_est;bax_preint;bg_preint\n";
+ if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
+ //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+ file_est << header_est;
+#endif
+
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ 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
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\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
+ if(n_frames > 0)
+ {
+ 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();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+ 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(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
+
+ void TestStaticZeroOdom(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data.head(2) = body_magnitudes.head(2);
+ data(5) = body_magnitudes(2);
+ data.head(2) += bias_groundtruth.head(2);
+ data(5) += bias_groundtruth(2);
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./est2dzeroodom-"+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;o;bax_est;bg_est;bax_preint;bg_preint\n";
+ if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n";
+ //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n";
+ file_est << header_est;
+#endif
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ 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
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+
+ // new frame
+ if (last_frame_ != processor_motion_->getOrigin()->getFrame())
+ {
+ n_frames++;
+ last_frame_ = processor_motion_->getOrigin()->getFrame();
+
+ // 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());
+ 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",
+ Vector3d::Zero(),
+ Eigen::MatrixXd::Identity(3,3) * 0.01);
+
+ FactorBase::emplace<FactorRelativePose2d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+
+ }
+
+ // impose static
+ last_frame_->getV()->setZero();
+
+ //Fix frame
+ last_frame_->getV()->fix();
+
+ //Solve
+ if(n_frames > 0)
+ {
+ 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();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'] << ";"
+ << bias_est.head(2).norm() << ";"
+ << bias_est(2) << ";"
+ << bias_preint.head(2).norm() << ";"
+ << bias_preint(2) << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'] << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(2) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(2) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+
+ 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(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
+};
+
+
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished();
+ Vector3d bias_initial_guess = Vector3d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = Vector3d::Random()*100;
+ bias_initial_guess(2) = 0;
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
+}
+
+TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random)
+{
+ Vector3d body_magnitudes = Vector3d::Zero();
+ Vector3d bias_groundtruth = Vector3d::Zero();
+ Vector3d bias_initial_guess = Vector3d::Random()*0.01;
+
+ 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");
+// }
+// }
+//}
+//
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c2bdbe07c0f8fef4c302d6ad47ab3d5ad13b523
--- /dev/null
+++ b/test/gtest_imu_static_init.cpp
@@ -0,0 +1,673 @@
+#include <fstream>
+
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+#include "imu/internal/config.h"
+
+#include "imu/factor/factor_imu.h"
+#include "imu/math/imu_tools.h"
+#include "imu/sensor/sensor_imu.h"
+#include "core/capture/capture_void.h"
+#include <core/factor/factor_relative_pose_3d.h>
+
+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
+{
+
+ 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 dt;
+ Vector6d data;
+ Matrix6d data_cov;
+ FrameBasePtr KF0_;
+ FrameBasePtr first_frame_;
+ FrameBasePtr last_frame_;
+ SolverCeresPtr solver_;
+
+ //a new of this will be created for each new test
+ void SetUp() override
+ {
+ WOLF_INFO("Doing setup...");
+ 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", 3);
+ Vector7d extrinsics = (Vector7d() << 0,0,0,0,0,0,1).finished();
+ sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/test/yaml/sensor_imu_static_init.yaml");
+ ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu_static_init.yaml");
+ processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+ // Time and data variables
+ dt = 0.1;
+ t0.set(0);
+ t = t0;
+ data = Vector6d::Random();
+ data_cov = Matrix6d::Identity();
+ last_frame_ = nullptr;
+ first_frame_ = nullptr;
+
+ // Create the solver
+ solver_ = make_shared<SolverCeres>(problem_ptr_);
+
+ // Set the origin
+ 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);
+
+ Vector4d q_init; q_init << 0,0,0,1;
+ VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
+ VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
+ KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+ }
+
+ void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data = body_magnitudes;
+ data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
+ data += bias_groundtruth;
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#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";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+ file_est << header_est;
+#endif
+
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ 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
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var) << ";"
+ << 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
+ if(n_frames > 0)
+ {
+ 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();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(testing_var+3) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+
+ 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(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
+
+ void TestStaticZeroOdom(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol)
+ {
+ //Data
+ data = body_magnitudes;
+ data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
+ data += bias_groundtruth;
+
+ //Set bias initial guess
+ sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
+ processor_motion_->setOrigin(KF0_);
+
+#if WRITE_CSV_FILE
+ std::fstream file_est;
+ file_est.open("./estzeroodom-"+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";
+ if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n";
+ file_est << header_est;
+#endif
+
+
+ int n_frames = 0;
+ for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){
+ WOLF_INFO("\n------------------------------------------------------------------------");
+
+ //Create and process capture
+ auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
+ 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
+ if(testing_var == 3){
+ file_est << std::fixed << t << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var) << ";"
+ << 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 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());
+ 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",
+ Vector7d::Zero(),
+ Eigen::MatrixXd::Identity(6,6) * 0.01);
+
+ FactorBase::emplace<FactorRelativePose3d>(feature_zero,
+ feature_zero,
+ frm_pair->second,
+ nullptr,
+ false,
+ TOP_MOTION);
+
+ }
+
+ // impose static
+ last_frame_->getV()->setZero();
+
+ //Fix frame
+ last_frame_->getV()->fix();
+
+ //Solve
+ if(n_frames > 0)
+ {
+ 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();
+
+ WOLF_INFO("The current problem is:");
+ problem_ptr_->print(4);
+
+#if WRITE_CSV_FILE
+ // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result
+ if(testing_var == 3){
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'].norm() << ";"
+ << state['V'].norm() << ";"
+ << state['O'].norm() << ";"
+ << bias_est.head(3).norm() << ";"
+ << bias_est.tail(3).norm() << ";"
+ << bias_preint.head(3).norm() << ";"
+ << bias_preint.tail(3).norm() << "\n";
+ }
+ else
+ {
+ file_est << std::fixed << t+dt/2 << ";"
+ << state['P'](testing_var) << ";"
+ << state['V'](testing_var) << ";"
+ << state['O'](testing_var) << ";"
+ << bias_est(testing_var) << ";"
+ << bias_est(testing_var+3) << ";"
+ << bias_preint(testing_var) << ";"
+ << bias_preint(testing_var+3) << "\n";
+
+ }
+#endif
+ }
+
+ }
+
+
+
+ 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(small_tol)
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+ }
+ }
+ else
+ {
+ if(n_frames == 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ }
+ else if (n_frames > 2)
+ {
+ EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3);
+ EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3);
+ EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3);
+ }
+ }
+ }
+
+#if WRITE_CSV_FILE
+ file_est.close();
+#endif
+
+ }
+};
+
+
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0.42, 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, true);
+}
+
+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.42, 0, 0, 0, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+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.01, 0, 0).finished();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+ Vector6d bias_initial_guess = Vector6d::Zero();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished();
+
+ TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false);
+}
+
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_a_random)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = Vector6d::Random()*100;
+ bias_initial_guess.tail(3) = Vector3d::Zero();
+
+ TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true);
+}
+
+TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random)
+{
+ Vector6d body_magnitudes = Vector6d::Zero();
+ Vector6d bias_groundtruth = Vector6d::Zero();
+ Vector6d bias_initial_guess = Vector6d::Random()*0.01;
+
+ 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)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp
index 4dbac962133c7d8da9f76e152a09e0749c1f2e71..a013072abf3064c5a2ac26302a1fcfe145f1a0b5 100644
--- a/test/gtest_processor_motion_intrinsics_update.cpp
+++ b/test/gtest_processor_motion_intrinsics_update.cpp
@@ -155,7 +155,7 @@ TEST_F(ProcessorImuTest, test1)
* SET TO TRUE TO PRODUCE CSV FILE
* SET TO FALSE TO STOP PRODUCING CSV FILE
*/
-#define WRITE_CSV_FILE false
+#define WRITE_CSV_FILE true
TEST_F(ProcessorImuTest, getState)
{
diff --git a/test/yaml/imu2d_static_init.yaml b/test/yaml/imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4978c8e2863908df72a336da1225667411d3022a
--- /dev/null
+++ b/test/yaml/imu2d_static_init.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 0.9999 # seconds
+ max_buff_length: 1000000000 # motion deltas
+ dist_traveled: 100000000000 # meters
+ angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/imu_static_init.yaml b/test/yaml/imu_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..46393f30b8f24bf645bfb1272764e6c8915e5dc2
--- /dev/null
+++ b/test/yaml/imu_static_init.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.0001
+
+keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 0.9999 # seconds
+ max_buff_length: 1000000000 # motion deltas
+ dist_traveled: 100000000000 # meters
+ angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3c78a00d35c785fe73381d8f6ce99a705d27ce77
--- /dev/null
+++ b/test/yaml/sensor_imu.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances:
+ a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.800 # m/s2 - initial bias
+ wb_initial_stdev: 0.350 # rad/sec - initial bias
+ ab_rate_stdev: 0.1 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
diff --git a/test/yaml/sensor_imu2d_static_init.yaml b/test/yaml/sensor_imu2d_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936
--- /dev/null
+++ b/test/yaml/sensor_imu2d_static_init.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances:
+ a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.800 # m/s2 - initial bias
+ wb_initial_stdev: 0.350 # rad/sec - initial bias
+ ab_rate_stdev: 0.1 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
diff --git a/test/yaml/sensor_imu_static_init.yaml b/test/yaml/sensor_imu_static_init.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..700b8d6762b91b38e96391f6032b2c7c4221eabc
--- /dev/null
+++ b/test/yaml/sensor_imu_static_init.yaml
@@ -0,0 +1,9 @@
+type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances:
+ a_noise: 0.001 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.001 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.001 # m/s2 - initial bias
+ wb_initial_stdev: 0.001 # rad/sec - initial bias
+ ab_rate_stdev: 0.001 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.001 # rad/s/sqrt(s)