New file for testing mocap+imu only

demos/CMakeLists.txt

@@ -11,28 +11,28 @@ include_directories(
 
 add_library(mcapi_utils mcapi_utils.cpp)
 
-add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
-target_link_libraries(mcapi_povcdl_estimation
-    mcapi_utils
-    ${wolfcore_LIBRARIES}
-    ${wolfimu_LIBRARIES}
-    ${PLUGIN_NAME}
-    ${multicontact-api_LIBRARIES}
-    ${pinocchio_LIBRARIES}
-)
-target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+# add_executable(mcapi_povcdl_estimation mcapi_povcdl_estimation.cpp)
+# target_link_libraries(mcapi_povcdl_estimation
+#     mcapi_utils
+#     ${wolfcore_LIBRARIES}
+#     ${wolfimu_LIBRARIES}
+#     ${PLUGIN_NAME}
+#     ${multicontact-api_LIBRARIES}
+#     ${pinocchio_LIBRARIES}
+# )
+# target_compile_definitions(mcapi_povcdl_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
 
 
-add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
-target_link_libraries(mcapi_pov_estimation
-    mcapi_utils
-    ${wolfcore_LIBRARIES}
-    ${wolfimu_LIBRARIES}
-    ${PLUGIN_NAME}
-    ${multicontact-api_LIBRARIES}
-    ${pinocchio_LIBRARIES}
-)
-target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
+# add_executable(mcapi_pov_estimation mcapi_pov_estimation.cpp)
+# target_link_libraries(mcapi_pov_estimation
+#     mcapi_utils
+#     ${wolfcore_LIBRARIES}
+#     ${wolfimu_LIBRARIES}
+#     ${PLUGIN_NAME}
+#     ${multicontact-api_LIBRARIES}
+#     ${pinocchio_LIBRARIES}
+# )
+# target_compile_definitions(mcapi_pov_estimation PRIVATE ${PINOCCHIO_CFLAGS_OTHER})
 
 add_executable(solo_real_povcdl_estimation solo_real_povcdl_estimation.cpp)
 target_link_libraries(solo_real_povcdl_estimation 
@@ -56,6 +56,15 @@ target_link_libraries(solo_real_pov_estimation
     z
 )
 
+add_executable(solo_mocap_imu solo_mocap_imu.cpp)
+target_link_libraries(solo_mocap_imu 
+    ${wolfcore_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${pinocchio_LIBRARIES}    
+    /usr/local/lib/libcnpy.so
+    z
+)
 
 # add_executable(test_cnpy test_cnpy.cpp)
 # target_link_libraries(test_cnpy

demos/solo_mocap_imu.cpp

+#include <iostream>
+#include <fstream>
+
+// #include <random>
+#include <yaml-cpp/yaml.h>
+#include <Eigen/Dense>
+#include <ctime>
+#include "eigenmvn.h"
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+
+// CNPY
+#include <cnpy.h>
+
+// WOLF
+#include <core/problem/problem.h>
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/math/rotations.h>
+#include <core/capture/capture_base.h>
+#include <core/capture/capture_pose.h>
+#include <core/feature/feature_base.h>
+#include <core/factor/factor_pose_3d_with_extrinsics.h>
+#include <core/factor/factor_block_absolute.h>
+#include <core/processor/processor_odom_3d.h>
+#include <core/sensor/sensor_odom_3d.h>
+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/yaml/parser_yaml.h"
+
+#include <core/sensor/sensor_pose.h>
+#include <core/processor/processor_pose.h>
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+
+// BODYDYNAMICS
+#include "bodydynamics/internal/config.h"
+
+
+// UTILS
+#include "mcapi_utils.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace pinocchio;
+
+typedef pinocchio::SE3Tpl<double> SE3;
+typedef pinocchio::ForceTpl<double> Force;
+
+
+int main (int argc, char **argv) {
+    // retrieve parameters from yaml file
+    YAML::Node config = YAML::LoadFile("/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml");
+    std::string robot_urdf = config["robot_urdf"].as<std::string>();
+    int dimc = config["dimc"].as<int>();
+    int nb_feet = config["nb_feet"].as<int>();
+    double dt = config["dt"].as<double>();
+    double min_t = config["min_t"].as<double>();
+    double max_t = config["max_t"].as<double>();
+    double solve_every_sec = config["solve_every_sec"].as<double>();
+    bool solve_end = config["solve_end"].as<bool>();
+
+    // Ceres setup
+    int max_num_iterations = config["max_num_iterations"].as<int>();
+    
+    // estimator sensor noises
+    double std_pbc_est = config["std_pbc_est"].as<double>();
+    double std_vbc_est = config["std_vbc_est"].as<double>();
+    double std_f_est = config["std_f_est"].as<double>();
+    double std_tau_est = config["std_tau_est"].as<double>();
+    double std_odom3d_est = config["std_odom3d_est"].as<double>();
+    
+    // priors
+    double std_prior_p = config["std_prior_p"].as<double>();
+    double std_prior_o = config["std_prior_o"].as<double>();
+    double std_prior_v = config["std_prior_v"].as<double>();
+    double std_abs_bias_pbc = config["std_abs_bias_pbc"].as<double>();
+    double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+    double std_abs_bias_gyro = config["std_abs_bias_gyro"].as<double>();    
+    double std_bp_drift = config["std_bp_drift"].as<double>();
+    std::vector<double> bias_imu_prior_vec = config["bias_imu_prior"].as<std::vector<double>>();
+    Eigen::Map<Vector6d> bias_imu_prior(bias_imu_prior_vec.data());
+    std::vector<double> b_p_bi_vec = config["b_p_bi"].as<std::vector<double>>();
+    std::vector<double> b_q_i_vec = config["b_q_i"].as<std::vector<double>>();
+    std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+    Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
+    Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+    Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
+    double fz_thresh = config["fz_thresh"].as<double>();
+
+    // MOCAP
+    double std_pose_p = config["std_pose_p"].as<double>();
+    double std_pose_o_deg = config["std_pose_o_deg"].as<double>();
+    double std_mocap_extr_p = config["std_mocap_extr_p"].as<double>();
+    double std_mocap_extr_o_deg = config["std_mocap_extr_o_deg"].as<double>();
+    bool unfix_extr_sensor_pose = config["unfix_extr_sensor_pose"].as<bool>();
+    bool time_tolerance_mocap = config["time_tolerance_mocap"].as<double>();
+    double time_shift_mocap = config["time_shift_mocap"].as<double>();
+    
+
+    std::string data_file_path = config["data_file_path"].as<std::string>();
+    std::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
+
+    // Base to IMU transform
+    Quaterniond b_q_i(b_qvec_i);
+    assert(b_q_i.normalized().isApprox(b_q_i));
+    Quaterniond i_q_b = b_q_i.conjugate();
+    SE3 b_T_i(b_q_i, b_p_bi);
+    SE3 i_T_b = b_T_i.inverse();
+    Matrix3d b_R_i =  b_T_i.rotation();
+    Vector3d i_p_ib = i_T_b.translation();
+    Matrix3d i_R_b =  i_T_b.rotation();
+
+    // initialize some vectors and fill them with dicretized data
+    cnpy::npz_t arr_map = cnpy::npz_load(data_file_path);
+
+    //load it into a new array
+    cnpy::NpyArray t_npyarr = arr_map["t"];
+    cnpy::NpyArray imu_acc_npyarr = arr_map["imu_acc"];
+    // cnpy::NpyArray o_a_oi_npyarr = arr_map["o_a_oi"];
+    cnpy::NpyArray o_q_i_npyarr = arr_map["o_q_i"];
+    cnpy::NpyArray i_omg_oi_npyarr = arr_map["i_omg_oi"];
+    cnpy::NpyArray qa_npyarr = arr_map["qa"];
+    cnpy::NpyArray dqa_npyarr = arr_map["dqa"];
+    cnpy::NpyArray tau_npyarr = arr_map["tau"];
+    cnpy::NpyArray l_forces_npyarr = arr_map["l_forces"];
+    // cnpy::NpyArray w_pose_wm_npyarr = arr_map["w_pose_wm"];
+    cnpy::NpyArray m_v_wm_npyarr = arr_map["m_v_wm"];
+    cnpy::NpyArray w_v_wm_npyarr = arr_map["w_v_wm"];
+    // cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"];
+    cnpy::NpyArray w_p_wm_npyarr = arr_map["w_p_wm"];
+    cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"];
+    // cnpy::NpyArray contact_npyarr = arr_map["contact"];
+    // cnpy::NpyArray b_p_bl_mocap_npyarr = arr_map["b_p_bl_mocap"];
+
+    // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
+    double* t_arr = t_npyarr.data<double>();
+    double* imu_acc_arr = imu_acc_npyarr.data<double>();
+    // double* o_a_oi_arr = o_a_oi_npyarr.data<double>();
+    double* i_omg_oi_arr = i_omg_oi_npyarr.data<double>();
+    double* qa_arr = qa_npyarr.data<double>();
+    double* dqa_arr = dqa_npyarr.data<double>();
+    double* tau_arr = tau_npyarr.data<double>();
+    double* l_forces_arr = l_forces_npyarr.data<double>();
+    // double* w_pose_wm_arr = w_pose_wm_npyarr.data<double>();
+    double* m_v_wm_arr = m_v_wm_npyarr.data<double>();
+    double* w_v_wm_arr = w_v_wm_npyarr.data<double>();
+    double* o_q_i_arr = o_q_i_npyarr.data<double>();
+    // double* o_R_i_arr = o_R_i_npyarr.data<double>();
+    double* w_p_wm_arr = w_p_wm_npyarr.data<double>();
+    double* w_q_m_arr = w_q_m_npyarr.data<double>();
+    
+    // double* contact_arr = contact_npyarr.data<double>();
+    // double* b_p_bl_mocap_arr = b_p_bl_mocap_npyarr.data<double>();
+
+    // double* w_R_m_arr = w_R_m_npyarr.data<double>();
+    unsigned int N = t_npyarr.shape[0];
+    if (max_t > 0){
+        N = N < int(max_t/dt) ? N : int(max_t/dt);
+    }
+
+    /////////////////////////
+    // WOLF enters the scene
+    // SETUP PROBLEM/SENSORS/PROCESSORS
+    std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+    ProblemPtr problem = Problem::create("POV", 3);
+
+    // add a tree manager for sliding window optimization 
+    ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
+    ParamsServer server = ParamsServer(parser.getParams());
+    auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
+    problem->setTreeManager(tree_manager);
+
+    //////////////////////
+    // COMPUTE INITIAL STATE
+    TimeStamp t0(t_arr[0]);
+    Eigen::Map<Vector4d> o_q_i(o_q_i_arr);  // initialize with IMU orientation
+    Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr);  // Hyp: b=i (not the case)
+    Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr);
+    Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr);
+    w_qvec_wm.normalize();  // not close enough to wolf eps sometimes
+
+    //////////////////////
+
+    // CERES WRAPPER
+    SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+    solver->getSolverOptions().max_num_iterations = max_num_iterations;
+
+    //===================================================== INITIALIZATION
+    // SENSOR + PROCESSOR IMU
+    SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", (Vector7d() << 0,0,0, 0,0,0,1).finished(), bodydynamics_root_dir + "/demos/sensor_imu_solo12.yaml");
+    SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
+    ProcessorBasePtr proc_ftp_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", bodydynamics_root_dir + "/demos/processor_imu_solo12.yaml");
+    ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
+
+    // SENSOR + PROCESSOR POSE (for mocap)
+    // pose sensor and proc (to get extrinsics in the prb)
+    auto intr_sp = std::make_shared<ParamsSensorPose>();
+    intr_sp->std_p = std_pose_p;
+    intr_sp->std_o = toRad(std_pose_o_deg);
+    Vector7d extr_pose; extr_pose << i_p_ib, i_q_b.coeffs();
+    auto sensor_pose = problem->installSensor("SensorPose", "pose", extr_pose, intr_sp);
+    auto params_proc = std::make_shared<ParamsProcessorPose>();
+    params_proc->time_tolerance = time_tolerance_mocap;
+    auto proc_pose = problem->installProcessor("ProcessorPose", "pose", sensor_pose, params_proc);
+    // somehow by default the sensor extrinsics is fixed
+    Matrix3d cov_sp_p = pow(std_mocap_extr_p, 2)*Matrix3d::Identity();
+    Matrix3d cov_sp_o = pow(std_mocap_extr_o_deg/60, 2)*Matrix3d::Identity();
+    sensor_pose->addPriorParameter('P', extr_pose.head<3>(), cov_sp_p);
+    sensor_pose->addPriorParameter('O', extr_pose.tail<4>(), cov_sp_o);
+    if (unfix_extr_sensor_pose){
+        sensor_pose->unfixExtrinsics();
+    }
+    else{
+        sensor_pose->fixExtrinsics();
+    }
+
+    VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
+    VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()});
+    // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+    FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005);  // if mocap used
+    
+    proc_imu->setOrigin(KF1);
+
+    //////////////////////////////////////////
+    // INITIAL BIAS FACTORS
+    // Add absolute factor on Imu biases to simulate a fix()
+    Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
+    Matrix6d Q_bi = std_abs_imu.array().square().matrix().asDiagonal();    
+    CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
+    FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
+    FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);   
+    KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
+    sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+    // sen_imu->fixIntrinsics();
+
+
+    //////////////////////////////////////////
+    // Vectors to store estimates at the time of data processing 
+    // fbk -> feedback: to check if the estimation is stable enough for feedback control
+    std::vector<Vector3d> p_ob_fbk_v; 
+    std::vector<Vector4d> q_ob_fbk_v; 
+    std::vector<Vector3d> v_ob_fbk_v; 
+    // Allocate on the heap to prevent stack overflow for large datasets
+    double* o_p_ob_fbk_carr = new double[3*N];
+    double* o_q_b_fbk_carr = new double[4*N];
+    double* o_v_ob_fbk_carr = new double[3*N];
+    double* imu_bias_fbk_carr = new double[6*N];
+    double* extr_mocap_fbk_carr = new double[7*N];
+
+    std::vector<Vector3d> i_omg_oi_v; 
+    //////////////////////////////////////////
+
+    unsigned int nb_kf = 1;
+    for (unsigned int i=0; i < N; i++){
+        TimeStamp ts(t_arr[i]);
+
+        ////////////////////////////////////
+        // Retrieve current state
+        VectorComposite curr_state = problem->getState();
+        Vector4d o_qvec_i_curr = curr_state['O'];
+        Quaterniond o_q_i_curr(o_qvec_i_curr); 
+        Vector3d o_v_oi_curr = curr_state['V']; 
+
+        ////////////////////////////////////
+        ////////////////////////////////////
+        // Get measurements
+        // retrieve traj data
+        Eigen::Map<Vector3d> imu_acc(imu_acc_arr+3*i);
+        Eigen::Map<Vector3d> i_omg_oi(i_omg_oi_arr+3*i);  // Hyp: b=i (not the case)
+        Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i);
+        Eigen::Map<Matrix<double,12, 1>> qa(qa_arr+12*i);
+        Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
+        Eigen::Map<Vector3d> w_p_wm(w_p_wm_arr+3*i);
+        Eigen::Map<Vector4d> w_qvec_wm(w_q_m_arr+4*i);
+        w_qvec_wm.normalize();  // not close enough to wolf eps sometimes
+
+        // store i_omg_oi for later computation of o_v_ob from o_v_oi
+        i_omg_oi_v.push_back(i_omg_oi);
+
+        Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
+        Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+
+        ////////////////////////////////////
+        // IMU
+        Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
+        Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
+
+        CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+        CImu->process();
+ 
+        // Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+        Vector7d pose_meas; pose_meas << w_p_wm, w_qvec_wm;
+        CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
+        CP->process();
+
+        // solve every new KF
+        if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
+            std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+            std::cout << ts << "  ";
+            std::cout << report << std::endl;
+            nb_kf = problem->getTrajectory()->getFrameMap().size();
+        }
+
+        // Store current state estimation
+        VectorComposite state_fbk = problem->getState(ts);
+        Vector3d o_p_oi = state_fbk['P'];
+        Vector4d o_qvec_i = state_fbk['O'];
+        Vector3d o_v_oi = state_fbk['V'];
+
+        // IMU to base frame 
+        Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+        // o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+        Matrix3d o_R_b = o_R_i * i_R_b;
+        Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+        Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+        Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(o_R_i*b_p_bi);
+        // Vector3d o_p_ob = o_p_oi;
+        // Vector3d o_v_ob = o_v_oi;
+
+        imu_bias = sen_imu->getIntrinsic()->getState();
+        Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
+
+        mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(),             3*sizeof(double));
+        mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(),           4*sizeof(double));
+        mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(),             3*sizeof(double));
+        mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(),         6*sizeof(double));
+        mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double));
+
+        p_ob_fbk_v.push_back(o_p_ob);
+        q_ob_fbk_v.push_back(o_qvec_b);
+        v_ob_fbk_v.push_back(o_v_ob);
+    }
+    
+
+
+    ///////////////////////////////////////////
+    //////////////// SOLVING //////////////////
+    ///////////////////////////////////////////
+    if (solve_end){
+        std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+        problem->print(4,true,true,true);
+        std::cout << report << std::endl;
+    }
+
+    double* o_p_ob_carr = new double[3*N];
+    double* o_q_b_carr = new double[4*N];
+    double* imu_bias_carr = new double[6*N];
+    double* o_v_ob_carr = new double[6*N];
+    for (unsigned int i=0; i < N; i++) { 
+        VectorComposite state_est = problem->getState(t_arr[i]);
+        Vector3d i_omg_oi = i_omg_oi_v[i];
+        Vector3d o_p_oi = state_est['P'];
+        Vector4d o_qvec_i = state_est['O'];
+        Vector3d o_v_oi = state_est['V'];
+
+        auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+        Vector6d imu_bias = sb->getState();
+        // std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
+        Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
+
+        Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
+        Matrix3d o_R_b = o_R_i * i_R_b;
+        Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
+        Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
+        Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
+        // Vector3d o_p_ob = o_p_oi;
+        // Vector3d o_v_ob = o_v_oi;
+
+        mempcpy(o_p_ob_carr+3*i, o_p_ob.data(),     3*sizeof(double));
+        mempcpy(o_q_b_carr +4*i, o_qvec_b.data(),   4*sizeof(double));
+        mempcpy(o_v_ob_carr+3*i, o_v_ob.data(),     3*sizeof(double));
+        mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
+    }
+
+
+    // Compute Covariances
+    unsigned int Nkf = problem->getTrajectory()->getFrameMap().size();
+    double* tkf_carr  = new double[1*Nkf];
+    double* Qp_carr  = new double[3*Nkf];
+    double* Qo_carr  = new double[3*Nkf];
+    double* Qv_carr  = new double[3*Nkf];
+    double* Qbi_carr = new double[6*Nkf];
+    double* Qm_carr  = new double[6*Nkf];
+    // factor errors
+    double* fac_imu_err_carr = new double[9*Nkf];
+    double* fac_pose_err_carr  = new double[6*Nkf];
+    int i = 0;
+    for (auto& elt: problem->getTrajectory()->getFrameMap()){
+        std::cout << "Traj " << i << "/" << problem->getTrajectory()->getFrameMap().size() << std::endl;
+        tkf_carr[i] = elt.first.get();
+        auto kf = elt.second;
+        VectorComposite kf_state = kf->getState();
+        
+        // compute covariances of KF and capture stateblocks
+        Eigen::MatrixXd Qp =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qo =  Eigen::Matrix3d::Identity();  // global or local?
+        Eigen::MatrixXd Qv =  Eigen::Matrix3d::Identity();
+        Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity();
+        Eigen::MatrixXd Qmp =  Eigen::Matrix3d::Identity();  // extr mocap
+        Eigen::MatrixXd Qmo =  Eigen::Matrix3d::Identity();  // extr mocap
+        
+        solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL);  // should not be computed every time !! see below
+        problem->getCovarianceBlock(kf->getStateBlock('P'), Qp);
+        problem->getCovarianceBlock(kf->getStateBlock('O'), Qo);
+        problem->getCovarianceBlock(kf->getStateBlock('V'), Qv);
+
+        CaptureBasePtr cap_imu = kf->getCaptureOf(sen_imu); 
+
+        std::vector<StateBlockPtr> extr_sb_vec = {sensor_pose->getP(), sensor_pose->getO()};
+        solver->computeCovariances(extr_sb_vec);  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(sensor_pose->getP(), Qmp);
+        problem->getCovarianceBlock(sensor_pose->getO(), Qmo);
+        // std::cout << "Qbp\n" << Qbp << std::endl;
+        solver->computeCovariances(cap_imu->getStateBlockVec());  // not computed by ALL and destroys other computed covs -> issue to raise
+        problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
+
+        // diagonal components
+        Vector3d Qp_diag = Qp.diagonal(); 
+        Vector3d Qo_diag = Qo.diagonal(); 
+        Vector3d Qv_diag = Qv.diagonal(); 
+        Vector6d Qbi_diag = Qbi.diagonal(); 
+        Vector6d Qm_diag; Qm_diag << Qmp.diagonal(), Qmo.diagonal(); 
+        
+        memcpy(Qp_carr  + 3*i, Qp_diag.data(),  3*sizeof(double)); 
+        memcpy(Qo_carr  + 3*i, Qo_diag.data(),  3*sizeof(double)); 
+        memcpy(Qv_carr  + 3*i, Qv_diag.data(),  3*sizeof(double)); 
+        memcpy(Qbi_carr + 6*i, Qbi_diag.data(), 6*sizeof(double)); 
+        memcpy(Qm_carr  + 6*i, Qm_diag.data(),  6*sizeof(double)); 
+
+        // Factor errors
+        // std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
+        FactorBasePtrList fac_lst; 
+        kf->getFactorList(fac_lst);
+        Vector9d imu_err = Vector9d::Zero();
+        Vector6d pose_err = Vector6d::Zero();
+        for (auto fac: fac_lst){
+            if (fac->getProcessor() == proc_imu){
+                auto f = std::dynamic_pointer_cast<FactorImu>(fac);
+                // Matrix6d R_bias_drift = f->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
+                // Matrix6d R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper().bottomRightCorner<6,6>();
+                // std::cout << R_bias_drift << std::endl;
+                // MatrixXd R_bias_drift = f->getFeature()->getMeasurementSquareRootInformationUpper();
+                // MatrixXd Cov_bias_drift = f->getFeature()->getMeasurementCovariance();
+                // std::cout << "R_bias_drift" << std::endl;
+                // std::cout << R_bias_drift << std::endl;
+                // std::cout << "Cov_bias_drift" << std::endl;
+                // std::cout << Cov_bias_drift << std::endl;
+
+                if (f){
+                    imu_err = f->error();
+                }
+            }
+            if (fac->getProcessor() == proc_pose){
+                auto f = std::dynamic_pointer_cast<FactorPose3dWithExtrinsics>(fac);
+                if (f){
+                    pose_err = f->error();
+                }
+            }
+        }
+        memcpy(fac_imu_err_carr + 9*i, imu_err.data(), 9*sizeof(double)); 
+        memcpy(fac_pose_err_carr + 6*i, pose_err.data(),  6*sizeof(double)); 
+
+
+        i++;
+    }
+
+    // Save trajectories in npz file
+    // save ground truth    
+    cnpy::npz_save(out_npz_file_path, "t", t_arr, {N}, "w");             // "w" overwrites any file with same name
+    cnpy::npz_save(out_npz_file_path, "w_p_wm", w_p_wm_arr, {N,3}, "a"); // "a" appends to the file we created above
+    cnpy::npz_save(out_npz_file_path, "w_q_m",  w_q_m_arr,  {N,4}, "a");
+    cnpy::npz_save(out_npz_file_path, "w_v_wm", w_v_wm_arr, {N,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "m_v_wm", m_v_wm_arr, {N,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_q_i",  o_q_i_arr, {N,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "qa",     qa_arr, {N,12}, "a");
+
+    // save estimates
+    cnpy::npz_save(out_npz_file_path, "o_p_ob_fbk", o_p_ob_fbk_carr, {N,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_q_b_fbk",  o_q_b_fbk_carr,  {N,4}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_v_ob_fbk", o_v_ob_fbk_carr, {N,3}, "a");
+    
+    cnpy::npz_save(out_npz_file_path, "o_p_ob", o_p_ob_carr, {N,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_q_b",  o_q_b_carr,  {N,4}, "a");
+    cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
+    
+    // and biases/extrinsics
+    cnpy::npz_save(out_npz_file_path, "imu_bias_fbk", imu_bias_fbk_carr, {N,6}, "a");
+    cnpy::npz_save(out_npz_file_path, "imu_bias", imu_bias_carr, {N,6}, "a");
+    cnpy::npz_save(out_npz_file_path, "extr_mocap_fbk", extr_mocap_fbk_carr, {N,7}, "a");
+
+    // covariances
+    cnpy::npz_save(out_npz_file_path, "tkf", tkf_carr, {Nkf}, "a");
+    cnpy::npz_save(out_npz_file_path, "Qp", Qp_carr,   {Nkf,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "Qo", Qo_carr,   {Nkf,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "Qv", Qv_carr,   {Nkf,3}, "a");
+    cnpy::npz_save(out_npz_file_path, "Qbi", Qbi_carr, {Nkf,6}, "a");
+    cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr,   {Nkf,6}, "a");
+    cnpy::npz_save(out_npz_file_path, "fac_imu_err", fac_imu_err_carr, {Nkf,9}, "a");
+    cnpy::npz_save(out_npz_file_path, "fac_pose_err", fac_pose_err_carr, {Nkf,6}, "a");
+
+}

demos/solo_real_estimation.yaml

@@ -65,9 +65,9 @@ l_p_lg: [0.0, 0, 0]
 # l_p_lg: [0, 0, -0.031]  # for sin traj, point to which position estimation on z starts to be less good 
 
 # base to IMU transformation
-b_p_bi: [0.0, 0.0, 0.0]
+# b_p_bi: [0.0, 0.0, 0.0]
 # b_p_bi: [-0.2, 0.0, 0.0]
-# b_p_bi: [0.1163, 0.0, 0.02]
+b_p_bi: [0.1163, 0.0, 0.02]
 b_q_i: [0.0, 0.0, 0.0, 1.0]
 # b_q_i: [0.00000592745,  -0.03255761280,  -0.00025745595,  0.706732091]
 

demos/solo_real_pov_estimation.cpp

@@ -294,7 +294,9 @@ int main (int argc, char **argv) {
     CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
     FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi);
     FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);   
+    KF1->getCaptureOf(sen_imu)->getStateBlock('I')->setState(bias_imu_prior);
     sen_imu->getStateBlock('I')->setState(bias_imu_prior);
+    // sen_imu->fixIntrinsics();
 
 
     ///////////////////////////////////////////