diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
index 989fc10f16d2e2cb7fa1799825f7b40a3d65cf86..d69a49f223f42b336a898046cec513535e35666d 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -30,7 +30,7 @@ using namespace Eigen;
using namespace wolf;
const Vector3d zero3 = Vector3d::Zero();
-const double dt = 0.00001;
+const double dt = 0.001;
class ImuMocapFusion_Test : public testing::Test
@@ -74,8 +74,8 @@ class ImuMocapFusion_Test : public testing::Test
// biases and extrinsics
// ba_ = Vector3d::Zero();
// bw_ = Vector3d::Zero();
- ba_ << 0.001, 0.001, 0.001;
- bw_ << 0.001, 0.001, 0.001;
+ 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;
@@ -140,10 +140,10 @@ class ImuMocapFusion_Test : public testing::Test
// 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;
+ // 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;
@@ -154,8 +154,7 @@ class ImuMocapFusion_Test : public testing::Test
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
+ // imu measurements (BETTER HERE)
Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
Vector3d omg_meas = b_omg_b + bw_;
@@ -164,19 +163,19 @@ class ImuMocapFusion_Test : public testing::Test
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";
+ // 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_;
@@ -198,7 +197,7 @@ class ImuMocapFusion_Test : public testing::Test
t+=dt;
}
- file_gtr.close();
+ // file_gtr.close();
}
@@ -216,63 +215,63 @@ 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);
+ // 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();
-
- SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
- ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ // 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)