diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
index 510110e67a2b530c30758c3147c7391fafc1c178..989fc10f16d2e2cb7fa1799825f7b40a3d65cf86 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -5,6 +5,8 @@
* \author: mfourmy
*/
+#include <fstream>
+
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_base.h"
@@ -26,11 +28,9 @@
using namespace Eigen;
using namespace wolf;
-using std::cout;
-using std::endl;
const Vector3d zero3 = Vector3d::Zero();
-const double dt = 0.0001;
+const double dt = 0.00001;
class ImuMocapFusion_Test : public testing::Test
@@ -47,6 +47,7 @@ class ImuMocapFusion_Test : public testing::Test
Quaterniond b_q_m_;
Vector3d ba_;
Vector3d bw_;
+ double t_max;
void SetUp() override
{
@@ -55,36 +56,49 @@ class ImuMocapFusion_Test : public testing::Test
//////////////////////
// simulate trajectory
//////////////////////
-
- Vector3d freq_lin; freq_lin << 0.5, 1, 1.5;
- Vector3d freq_ang; freq_ang << 0.4, 0.7, 0.6;
-
- // P = sin(freq*t)
- // V = freq*cos(freq*t)
+ // 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();
- b_p_bm_ = Vector3d::Zero();
- b_q_m_ = Quaterniond::Identity();
+ // ba_ = Vector3d::Zero();
+ // bw_ = Vector3d::Zero();
+ ba_ << 0.001, 0.001, 0.001;
+ bw_ << 0.001, 0.001, 0.001;
+ // 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 = freq_lin.array(); // *(1,1,1)
+ 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 << b_p_bm_, b_q_m_.coeffs();
+ 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>();
auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
@@ -107,9 +121,12 @@ class ImuMocapFusion_Test : public testing::Test
prc_imu_params->dist_traveled = 10000000000;
prc_imu_params->angle_turned = 1000000000;
prc_imu_params->voting_active = true;
- auto processor_ptr = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_imu_, prc_imu_params);
+ 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_->fixIntrinsics();
+ sensor_imu_->unfixIntrinsics();
// Store necessary values in vectors
// std::vector<Vector3d> w_p_wb_vec;
@@ -121,22 +138,49 @@ class ImuMocapFusion_Test : public testing::Test
// 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);
-
- int traj_size = 10001;
- for (int i=0; i < traj_size; i++){
- double t = i*dt;
- Vector3d w_p_wb_sin = Eigen::sin((freq_lin*t).array()); // just for comparison with the integration
- Vector3d w_omg_b = freq_ang.array()* Eigen::cos((freq_ang*t).array());
- Vector3d w_a_wb = -freq_lin.array().square()*Eigen::sin((freq_lin*t).array());
- // integrate simulated traj
- w_p_wb = w_p_wb + w_v_wb*dt + 0.5*w_a_wb*dt*dt;
- w_v_wb = w_v_wb + w_a_wb*dt;
- w_q_b = wolf::exp_q(w_omg_b*dt) * w_q_b;
+
+ 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
- Vector3d acc_meas = w_q_b.conjugate()*(w_a_wb - wolf::gravity()) + ba_;
- Vector3d omg_meas = w_q_b.conjugate()*w_omg_b + bw_;
+ 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_;
@@ -145,18 +189,17 @@ class ImuMocapFusion_Test : public testing::Test
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_->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();
- if ((i%1000) == 0){
- std::cout << t << std::endl;
- std::cout << "w_p_wb: " << w_p_wb.transpose() << std::endl;
- std::cout << "w_p_wb_sin: " << w_p_wb_sin.transpose() << std::endl;
- }
+ t+=dt;
}
+ file_gtr.close();
+
}
void TearDown() override{};
@@ -172,12 +215,60 @@ TEST_F(ImuMocapFusion_Test, check_tree)
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::BRIEF);
+ 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();
SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);