Add res and norm to imu factor

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,

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
     {

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);
 }