Skip to content
Snippets Groups Projects
Commit 5e9eb26c authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Add res and norm to imu factor

parent 310b9ffb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/imu/factor/factor_imu.h
+ 14
− 0
View file @ 5e9eb26c
......@@ -46,6 +46,8 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
*
*/
Eigen::Vector9d error();
Eigen::Vector9d res();
double cost();
/** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
......@@ -252,6 +254,18 @@ Eigen::Vector9d FactorImu::error()
return err;
}
Eigen::Vector9d FactorImu::res(){
Eigen::Vector9d err = error();
return getMeasurementSquareRootInformationUpper()*err;
}
double FactorImu::cost(){
return res().squaredNorm();
}
template<typename D1, typename D2, typename D3>
inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
const Eigen::QuaternionBase<D2> & _q1,
......
include/imu/sensor/sensor_imu.h
+ 6
− 6
View file @ 5e9eb26c
......@@ -33,12 +33,12 @@ struct ParamsSensorImu : public ParamsSensorBase
ParamsSensorImu(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
w_noise = _server.getParam<double>(prefix + _unique_name + "/w_noise");
a_noise = _server.getParam<double>(prefix + _unique_name + "/a_noise");
ab_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_initial_stdev");
wb_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev");
ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
w_noise = _server.getParam<double>(prefix + _unique_name + "/motion_variances/w_noise");
a_noise = _server.getParam<double>(prefix + _unique_name + "/motion_variances/a_noise");
ab_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/motion_variances/ab_initial_stdev");
wb_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/motion_variances/wb_initial_stdev");
ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/motion_variances/ab_rate_stdev");
wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/motion_variances/wb_rate_stdev");
}
std::string print() const override
{
......
test/gtest_imu_mocap_fusion.cpp
+ 173
− 114
View file @ 5e9eb26c
......@@ -20,6 +20,7 @@
#include "imu/capture/capture_imu.h"
#include "imu/sensor/sensor_imu.h"
#include "imu/processor/processor_imu.h"
#include "imu/factor/factor_imu.h"
#include "Eigen/Dense"
#include <Eigen/SparseQR>
......@@ -32,6 +33,24 @@ using namespace wolf;
const Vector3d zero3 = Vector3d::Zero();
const double dt = 0.001;
const bool WRITE_CSV = false;
void printingFactorErrors(ProblemPtr pbe){
std::cout << "\n\nPrinting factor errors: " << std::endl;
for (auto KF: pbe->getTrajectory()->getFrameMap()){
for (auto fac: KF.second->getFactorList()){
auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
if (f){
std::cout << f->getType() << " " << f->error().transpose() << std::endl;
}
else{
auto fi = std::dynamic_pointer_cast<FactorImu>(fac);
std::cout << fi->getType() << " " << fi->error().transpose() << std::endl;
}
}
}
}
class ImuMocapFusion_Test : public testing::Test
{
......@@ -47,7 +66,7 @@ class ImuMocapFusion_Test : public testing::Test
Quaterniond b_q_m_;
Vector3d ba_;
Vector3d bw_;
double t_max;
double t_max_;
void SetUp() override
{
......@@ -61,30 +80,27 @@ class ImuMocapFusion_Test : public testing::Test
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_p; amp_p << 0.1, 0.2, 0.3;
Vector3d amp_a = amp_p.array() * freq_lin.array().square();
Vector3d amp_o; amp_o << 1, 1, 1;
Vector3d amp_o; amp_o << 0.5, 0.6, 0.7; // rad
// 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();
ba_ << 0.003, 0.002, 0.002;
bw_ << 0.004, 0.003, 0.002;
ba_ << 0.01, 0.02, 0.03;
bw_ << 0.04, 0.03, 0.02;
// 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);
Vector4d b_q_m_vec; b_q_m_vec << 0.5,0.5,0.5,0.5;
b_q_m_ = Quaterniond(b_q_m_vec);
// initialize state
Vector3d w_p_wb = Vector3d::Zero();
Quaterniond w_q_b = Quaterniond::Identity();
Vector3d w_v_wb = Vector3d::Zero(); // * cos(0)
Vector3d w_v_wb = Vector3d::Zero();
// Problem and solver_
problem_ = Problem::create("POV", 3);
......@@ -92,20 +108,21 @@ class ImuMocapFusion_Test : public testing::Test
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
FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0, 0.0005);
// 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 << 0,0,0, 0,0,0,1;
sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
// Vector7d pose_extr; pose_extr << 0,0,0, 0,0,0,1;
Vector7d pose_extr; pose_extr << b_p_bm_, b_q_m_vec;
sensor_pose_ = problem_->installSensor("SensorPose", "pose", pose_extr, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = 0.0005;
params_proc->time_tolerance = dt/2;
auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
// somehow by default the sensor extrinsics is fixed
sensor_pose_->unfixExtrinsics();
// sensor_pose_->fixExtrinsics();
// sensor_pose_->unfixExtrinsics();
sensor_pose_->fixExtrinsics();
Matrix6d cov_pose = sensor_pose_->getNoiseCov();
// IMU sensor
......@@ -113,8 +130,8 @@ class ImuMocapFusion_Test : public testing::Test
ParamsSensorImuPtr sen_imu_params = std::make_shared<ParamsSensorImu>();
sen_imu_params->a_noise = 0.01;
sen_imu_params->w_noise = 0.01;
sen_imu_params->ab_rate_stdev = 0.00001;
sen_imu_params->wb_rate_stdev = 0.00001;
sen_imu_params->ab_rate_stdev = 1e-12;
sen_imu_params->wb_rate_stdev = 1e-12;
sensor_imu_ = problem_->installSensor("SensorImu", "Main Imu", imu_extr, sen_imu_params);
ParamsProcessorImuPtr prc_imu_params = std::make_shared<ParamsProcessorImu>();
prc_imu_params->max_time_span = 0.199999;
......@@ -122,40 +139,57 @@ class ImuMocapFusion_Test : public testing::Test
prc_imu_params->dist_traveled = 10000000000;
prc_imu_params->angle_turned = 1000000000;
prc_imu_params->voting_active = true;
prc_imu_params->time_tolerance = dt/2;
auto proc_base = problem_->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_imu_, prc_imu_params);
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_base);
Vector6d imu_bias; imu_bias << ba_, bw_;
sensor_imu_->getStateBlock('I')->setState(imu_bias);
sensor_imu_->getStateBlock('I')->perturb(0.01);
proc_imu->setOrigin(KF1);
Matrix6d cov_imu = sensor_imu_->getNoiseCov();
// sensor_imu_->fixIntrinsics();
sensor_imu_->unfixIntrinsics();
// Store necessary values in vectors
// std::vector<Vector3d> w_p_wb_vec;
// std::vector<Vector3d> w_p_wb_sin_vec;
// std::vector<Quaterniond> w_q_b_vec;
// std::vector<Vector3d> w_v_wb_vec;
// w_p_wb_vec.push_back(w_p_wb);
// 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);
// 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
#ifdef WRITE_CSV
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_bias;
file_bias.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/PhdTests/gtest_bias.csv", std::fstream::out);
std::string header_bias = "t,bax,bay,baz,bwx,bwy,bwz\n";
file_bias << header_bias;
file_gtr << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< 0.0 << ","
<< 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";
#endif
double t = dt; // skip the zero so that data correspond to current state
t_max_ = 2;
int nb_kf = 1;
while (t <= t_max_){
// P = cos(freq*t)
// 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());
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 omg_meas = b_omg_b + bw_;
......@@ -164,41 +198,61 @@ 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";
// // 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_;
#ifdef WRITE_CSV
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";
#endif
// process data
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_->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();
// solve every new KF
if (problem_->getTrajectory()->getFrameMap().size() > nb_kf ){
std::string report = solver_->solve(SolverManager::ReportVerbosity::BRIEF);
nb_kf = problem_->getTrajectory()->getFrameMap().size();
Vector6d imu_bias = sensor_imu_->getIntrinsic()->getState();
#ifdef WRITE_CSV
file_bias << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
<< t << ","
<< imu_bias(0) << ","
<< imu_bias(1) << ","
<< imu_bias(2) << ","
<< imu_bias(3) << ","
<< imu_bias(4) << ","
<< imu_bias(5)
<< "\n";
#endif
}
t+=dt;
}
// file_gtr.close();
#ifdef WRITE_CSV
file_gtr.close();
file_bias.close();
#endif WRITE_CSV
}
......@@ -216,61 +270,66 @@ 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;
}
printingFactorErrors(problem_);
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();
ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 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();
printingFactorErrors(problem_);
ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-5);
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);
}
......
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