diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c9da956b62e1aa79c1f41be4c82bb44031755aa9..42bce1a6d1364c4ea6af8943a55efcbc88345aad 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -28,6 +28,9 @@ target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME} ${wolf_LIBRAR
wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
target_link_libraries(gtest_feature_imu ${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})
+
wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp)
target_link_libraries(gtest_factor_compass_3d ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bb668f2b85062e100ad80dbd92ce29411dd19458
--- /dev/null
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -0,0 +1,282 @@
+/**
+ * \file gtest_imu_mocap_fusion.cpp
+ *
+ * Created on: Feb 25, 2020
+ * \author: mfourmy
+ */
+
+#include <fstream>
+
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_base.h"
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/processor/processor_pose.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+#include "imu/internal/config.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/sensor/sensor_imu.h"
+#include "imu/processor/processor_imu.h"
+
+#include "Eigen/Dense"
+#include <Eigen/SparseQR>
+#include <Eigen/OrderingMethods>
+
+
+using namespace Eigen;
+using namespace wolf;
+
+const Vector3d zero3 = Vector3d::Zero();
+const double dt = 0.001;
+
+
+class ImuMocapFusion_Test : public testing::Test
+{
+
+ public:
+
+ ProblemPtr problem_;
+ SolverCeresPtr solver_;
+ SensorBasePtr sensor_pose_;
+ SensorBasePtr sensor_imu_;
+
+ Vector3d b_p_bm_;
+ Quaterniond b_q_m_;
+ Vector3d ba_;
+ Vector3d bw_;
+ double t_max;
+
+ void SetUp() override
+ {
+ std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+
+ //////////////////////
+ // simulate trajectory
+ //////////////////////
+ // Amplitudes
+ Vector3d freq_lin; freq_lin << 2, 2.5, 3; // in seconds
+ Vector3d freq_ang; freq_ang << 3.5, 3, 2.5; // in seconds
+ freq_lin *= 2*M_PI; // pass in radians
+ freq_ang *= 2*M_PI;
+ Vector3d amp_p; amp_p << 1.2, 1, 0.8;
+ Vector3d amp_a = amp_p.array() * freq_lin.array().square();
+ Vector3d amp_o; amp_o << 1, 1, 1;
+ // Vector3d amp_o; amp_o << 0,0,0;
+ Vector3d amp_w = amp_o.array() * freq_ang.array();
+
+ // P = cos(freq*t) + cst
+ // V = -freq*sin(freq*t)
+ // A = -freq^2*cos(freq*t)
+
+ // biases and extrinsics
+ // ba_ = Vector3d::Zero();
+ // bw_ = Vector3d::Zero();
+ ba_ << 0.003, 0.002, 0.002;
+ bw_ << 0.004, 0.003, 0.002;
+ // b_p_bm_ = Vector3d::Zero();
+ // b_q_m_ = Quaterniond::Identity();
+ b_p_bm_ << 0.1, 0.2, 0.3;
+ b_q_m_ = Quaterniond(0,0,0,1);
+
+ // initialize state
+ Vector3d w_p_wb = Vector3d::Zero();
+ Quaterniond w_q_b = Quaterniond::Identity();
+ Vector3d w_v_wb = Vector3d::Zero(); // * cos(0)
+
+ // Problem and solver_
+ problem_ = Problem::create("POV", 3);
+ solver_ = std::make_shared<SolverCeres>(problem_);
+ solver_->getSolverOptions().max_num_iterations = 500;
+ // initial guess
+ VectorComposite x_origin("POV", {w_p_wb, w_q_b.coeffs(), w_v_wb});
+ FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0, 0.0005); // if mocap used
+
+ // pose sensor and proc (to get extrinsics in the prb)
+ auto intr_sp = std::make_shared<ParamsSensorPose>();
+ intr_sp->std_p = 0.001;
+ intr_sp->std_o = 0.001;
+ Vector7d extr; extr << 0,0,0, 0,0,0,1;
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+ auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = 0.0005;
+ auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
+ // somehow by default the sensor extrinsics is fixed
+ sensor_pose_->unfixExtrinsics();
+ // sensor_pose_->fixExtrinsics();
+ Matrix6d cov_pose = sensor_pose_->getNoiseCov();
+
+ // IMU sensor
+ Vector7d imu_extr = (Vector7d() << 0,0,0, 0,0,0,1).finished();
+ ParamsSensorImuPtr sen_imu_params = std::make_shared<ParamsSensorImu>();
+ sen_imu_params->a_noise = 0.01;
+ sen_imu_params->w_noise = 0.01;
+ sen_imu_params->ab_rate_stdev = 0.00001;
+ sen_imu_params->wb_rate_stdev = 0.00001;
+ sensor_imu_ = problem_->installSensor("SensorImu", "Main Imu", imu_extr, sen_imu_params);
+ ParamsProcessorImuPtr prc_imu_params = std::make_shared<ParamsProcessorImu>();
+ prc_imu_params->max_time_span = 0.199999;
+ prc_imu_params->max_buff_length = 1000000000;
+ prc_imu_params->dist_traveled = 10000000000;
+ prc_imu_params->angle_turned = 1000000000;
+ prc_imu_params->voting_active = true;
+ auto proc_base = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_imu_, prc_imu_params);
+ ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_base);
+ proc_imu->setOrigin(KF1);
+ Matrix6d cov_imu = sensor_imu_->getNoiseCov();
+ // sensor_imu_->fixIntrinsics();
+ sensor_imu_->unfixIntrinsics();
+
+ // Store necessary values in vectors
+ // std::vector<Vector3d> w_p_wb_vec;
+ // std::vector<Vector3d> w_p_wb_sin_vec;
+ // std::vector<Quaterniond> w_q_b_vec;
+ // std::vector<Vector3d> w_v_wb_vec;
+
+ // w_p_wb_vec.push_back(w_p_wb);
+ // w_p_wb_sin_vec.push_back(w_p_wb);
+ // w_q_b_vec.push_back(w_q_b);
+ // w_v_wb_vec.push_back(w_v_wb);
+
+
+ // std::fstream file_gtr;
+ // file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_gtr.csv", std::fstream::out);
+ // std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
+ // file_gtr << header;
+
+ double t = 0;
+ t_max = 5.0;
+ while (t <= t_max){
+ // P = cos(freq*t) + cst
+ // V = -freq*sin(freq*t)
+ // A = -freq^2*cos(freq*t)
+ Vector3d b_omg_b = -amp_w.array()*Eigen::sin((freq_ang*t).array());
+ Vector3d b_a_wb = -amp_a.array()*Eigen::cos((freq_lin*t).array());
+
+ // imu measurements (BETTER HERE)
+ Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
+ Vector3d omg_meas = b_omg_b + bw_;
+
+ // integrate simulated traj
+ w_p_wb = w_p_wb + w_v_wb*dt + 0.5*(w_q_b*b_a_wb)*dt*dt;
+ w_v_wb = w_v_wb + w_q_b*b_a_wb*dt;
+ w_q_b = w_q_b*wolf::exp_q(b_omg_b*dt);
+
+ // file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ // << t << ","
+ // << w_p_wb(0) << ","
+ // << w_p_wb(1) << ","
+ // << w_p_wb(2) << ","
+ // << w_q_b.coeffs()(0) << ","
+ // << w_q_b.coeffs()(1) << ","
+ // << w_q_b.coeffs()(2) << ","
+ // << w_q_b.coeffs()(3) << ","
+ // << w_v_wb(0) << ","
+ // << w_v_wb(1) << ","
+ // << w_v_wb(2)
+ // << "\n";
+
+ // // imu measurements
+ // Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
+ // Vector3d omg_meas = b_omg_b + bw_;
+ // mocap measurements
+ Vector3d w_p_wm = w_p_wb + w_q_b*b_p_bm_;
+ Quaterniond w_q_m = w_q_b * b_q_m_;
+
+ // process data
+ Vector6d imu_data; imu_data << acc_meas, omg_meas;
+ CaptureBasePtr CIMU = std::make_shared<CaptureImu>(t, sensor_imu_, imu_data, cov_imu);
+ CIMU->process();
+ // sensor_imu_->fixIntrinsics();
+ sensor_imu_->unfixIntrinsics();
+ Vector7d pose_data; pose_data << w_p_wm, w_q_m.coeffs();
+ CaptureBasePtr CP = std::make_shared<CapturePose>(t, sensor_pose_, pose_data, cov_pose);
+ CP->process();
+
+ t+=dt;
+ }
+
+ // file_gtr.close();
+
+ }
+
+ void TearDown() override{};
+};
+
+
+
+
+TEST_F(ImuMocapFusion_Test, check_tree)
+{
+ ASSERT_TRUE(problem_->check(0));
+}
+
+TEST_F(ImuMocapFusion_Test, solve)
+{
+
+ // std::fstream file_int;
+ // std::fstream file_est;
+ // file_int.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_int.csv", std::fstream::out);
+ // file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_est.csv", std::fstream::out);
+ // std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
+ // file_int << header;
+ // file_est << header;
+
+ // double t = 0;
+ // while (t <= t_max){
+ // auto s = problem_->getState(t);
+ // file_int << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ // << t << ","
+ // << s['P'](0) << ","
+ // << s['P'](1) << ","
+ // << s['P'](2) << ","
+ // << s['O'](0) << ","
+ // << s['O'](1) << ","
+ // << s['O'](2) << ","
+ // << s['O'](3) << ","
+ // << s['V'](0) << ","
+ // << s['V'](1) << ","
+ // << s['V'](2)
+ // << "\n";
+ // t+=dt;
+ // }
+ // problem_->print(4, 1, 1, 1);
+ problem_->perturb();
+ std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+ // std::cout << report << std::endl;
+ // problem_->print(4, 1, 1, 1);
+
+ // t = 0;
+ // while (t <= t_max){
+ // auto s = problem_->getState(t);
+ // file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
+ // << t << ","
+ // << s['P'](0) << ","
+ // << s['P'](1) << ","
+ // << s['P'](2) << ","
+ // << s['O'](0) << ","
+ // << s['O'](1) << ","
+ // << s['O'](2) << ","
+ // << s['O'](3) << ","
+ // << s['V'](0) << ","
+ // << s['V'](1) << ","
+ // << s['V'](2)
+ // << "\n";
+ // t+=dt;
+ // }
+
+ // file_int.close();
+ // file_est.close();
+
+ ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_imu_->getIntrinsic()->getState(), (Vector6d() << ba_, bw_).finished(), 1e-4);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file