Skip to content
Snippets Groups Projects
Commit 1b182ee3 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Works with 10-3 (change imu meas definition order)

parent 06021d96
No related branches found
No related tags found
3 merge requests!39release after RAL,!38After 2nd RAL submission,!29Resolve "gtest imu and mocap"
Hide whitespace changes
Inline Side-by-side
test/gtest_imu_mocap_fusion.cpp
+ 77
− 78
View file @ 1b182ee3
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment