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"
...@@ -30,7 +30,7 @@ using namespace Eigen; ...@@ -30,7 +30,7 @@ using namespace Eigen;
using namespace wolf; using namespace wolf;
const Vector3d zero3 = Vector3d::Zero(); const Vector3d zero3 = Vector3d::Zero();
const double dt = 0.00001; const double dt = 0.001;
class ImuMocapFusion_Test : public testing::Test class ImuMocapFusion_Test : public testing::Test
...@@ -74,8 +74,8 @@ class ImuMocapFusion_Test : public testing::Test ...@@ -74,8 +74,8 @@ class ImuMocapFusion_Test : public testing::Test
// biases and extrinsics // biases and extrinsics
// ba_ = Vector3d::Zero(); // ba_ = Vector3d::Zero();
// bw_ = Vector3d::Zero(); // bw_ = Vector3d::Zero();
ba_ << 0.001, 0.001, 0.001; ba_ << 0.003, 0.002, 0.002;
bw_ << 0.001, 0.001, 0.001; bw_ << 0.004, 0.003, 0.002;
// b_p_bm_ = Vector3d::Zero(); // b_p_bm_ = Vector3d::Zero();
// b_q_m_ = Quaterniond::Identity(); // b_q_m_ = Quaterniond::Identity();
b_p_bm_ << 0.1, 0.2, 0.3; b_p_bm_ << 0.1, 0.2, 0.3;
...@@ -140,10 +140,10 @@ class ImuMocapFusion_Test : public testing::Test ...@@ -140,10 +140,10 @@ class ImuMocapFusion_Test : public testing::Test
// w_v_wb_vec.push_back(w_v_wb); // w_v_wb_vec.push_back(w_v_wb);
std::fstream file_gtr; // std::fstream file_gtr;
file_gtr.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_gtr.csv", std::fstream::out); // 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"; // std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
file_gtr << header; // file_gtr << header;
double t = 0; double t = 0;
t_max = 5.0; t_max = 5.0;
...@@ -154,8 +154,7 @@ class ImuMocapFusion_Test : public testing::Test ...@@ -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_omg_b = -amp_w.array()*Eigen::sin((freq_ang*t).array());
Vector3d b_a_wb = -amp_a.array()*Eigen::cos((freq_lin*t).array()); Vector3d b_a_wb = -amp_a.array()*Eigen::cos((freq_lin*t).array());
// imu measurements (BETTER HERE)
// imu measurements
Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_; Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
Vector3d omg_meas = b_omg_b + bw_; Vector3d omg_meas = b_omg_b + bw_;
...@@ -164,19 +163,19 @@ class ImuMocapFusion_Test : public testing::Test ...@@ -164,19 +163,19 @@ class ImuMocapFusion_Test : public testing::Test
w_v_wb = w_v_wb + w_q_b*b_a_wb*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); w_q_b = w_q_b*wolf::exp_q(b_omg_b*dt);
file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1) // file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< t << "," // << t << ","
<< w_p_wb(0) << "," // << w_p_wb(0) << ","
<< w_p_wb(1) << "," // << w_p_wb(1) << ","
<< w_p_wb(2) << "," // << w_p_wb(2) << ","
<< w_q_b.coeffs()(0) << "," // << w_q_b.coeffs()(0) << ","
<< w_q_b.coeffs()(1) << "," // << w_q_b.coeffs()(1) << ","
<< w_q_b.coeffs()(2) << "," // << w_q_b.coeffs()(2) << ","
<< w_q_b.coeffs()(3) << "," // << w_q_b.coeffs()(3) << ","
<< w_v_wb(0) << "," // << w_v_wb(0) << ","
<< w_v_wb(1) << "," // << w_v_wb(1) << ","
<< w_v_wb(2) // << w_v_wb(2)
<< "\n"; // << "\n";
// // imu measurements // // imu measurements
// Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_; // Vector3d acc_meas = b_a_wb - w_q_b.conjugate() * wolf::gravity() + ba_;
...@@ -198,7 +197,7 @@ class ImuMocapFusion_Test : public testing::Test ...@@ -198,7 +197,7 @@ class ImuMocapFusion_Test : public testing::Test
t+=dt; t+=dt;
} }
file_gtr.close(); // file_gtr.close();
} }
...@@ -216,63 +215,63 @@ TEST_F(ImuMocapFusion_Test, check_tree) ...@@ -216,63 +215,63 @@ TEST_F(ImuMocapFusion_Test, check_tree)
TEST_F(ImuMocapFusion_Test, solve) TEST_F(ImuMocapFusion_Test, solve)
{ {
std::fstream file_int; // std::fstream file_int;
std::fstream file_est; // std::fstream file_est;
file_int.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_int.csv", std::fstream::out); // 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); // 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"; // std::string header = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n";
file_int << header; // file_int << header;
file_est << header; // file_est << header;
double t = 0; // double t = 0;
while (t <= t_max){ // while (t <= t_max){
auto s = problem_->getState(t); // auto s = problem_->getState(t);
file_int << std::setprecision(std::numeric_limits<long double>::digits10 + 1) // file_int << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< t << "," // << t << ","
<< s['P'](0) << "," // << s['P'](0) << ","
<< s['P'](1) << "," // << s['P'](1) << ","
<< s['P'](2) << "," // << s['P'](2) << ","
<< s['O'](0) << "," // << s['O'](0) << ","
<< s['O'](1) << "," // << s['O'](1) << ","
<< s['O'](2) << "," // << s['O'](2) << ","
<< s['O'](3) << "," // << s['O'](3) << ","
<< s['V'](0) << "," // << s['V'](0) << ","
<< s['V'](1) << "," // << s['V'](1) << ","
<< s['V'](2) // << s['V'](2)
<< "\n"; // << "\n";
t+=dt; // t+=dt;
} // }
problem_->print(4, 1, 1, 1); // problem_->print(4, 1, 1, 1);
problem_->perturb(); problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
std::cout << report << std::endl; // std::cout << report << std::endl;
problem_->print(4, 1, 1, 1); // problem_->print(4, 1, 1, 1);
t = 0; // t = 0;
while (t <= t_max){ // while (t <= t_max){
auto s = problem_->getState(t); // auto s = problem_->getState(t);
file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1) // file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< t << "," // << t << ","
<< s['P'](0) << "," // << s['P'](0) << ","
<< s['P'](1) << "," // << s['P'](1) << ","
<< s['P'](2) << "," // << s['P'](2) << ","
<< s['O'](0) << "," // << s['O'](0) << ","
<< s['O'](1) << "," // << s['O'](1) << ","
<< s['O'](2) << "," // << s['O'](2) << ","
<< s['O'](3) << "," // << s['O'](3) << ","
<< s['V'](0) << "," // << s['V'](0) << ","
<< s['V'](1) << "," // << s['V'](1) << ","
<< s['V'](2) // << s['V'](2)
<< "\n"; // << "\n";
t+=dt; // t+=dt;
} // }
file_int.close(); // file_int.close();
file_est.close(); // file_est.close();
SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_); ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6); ASSERT_MATRIX_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
ASSERT_MATRIX_APPROX(sp->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) int main(int argc, char **argv)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment