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

Fusing gtest works fine (extrinsics + imu bias rightly estimated) but only for dt=10-5

parent 1a2022f4
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
+ 129
− 38
View file @ 06021d96
......@@ -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);
......
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