diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index a7ff2d7b063657ec39fff82552fceedf82999847..61a2daa8c981e210ce281b3f898e31bcaf42ed82 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -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,
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 07b289c0ac0e21451dbe682774d3aeddc79ffa7b..c4e5f5bb8df481d7d53205ffb83511fd565225d8 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -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
{
diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
index bb668f2b85062e100ad80dbd92ce29411dd19458..c753366d6ec44e2a6c2bf668acc5a493d109a0c6 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -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);
}