diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 25c6f4dfdb9923e6f9bcec1d0f620e4906c1ea3a..e8896eccf78f6f4aef3138b1c6914646c788617c 100644
--- a/demos/CMakeLists.txt
+++ b/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
diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_mocap_imu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da0270444b930aa6dec61620599d4107bf894665
--- /dev/null
+++ b/demos/solo_mocap_imu.cpp
@@ -0,0 +1,514 @@
+#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");
+
+}
diff --git a/demos/solo_real_estimation.yaml b/demos/solo_real_estimation.yaml
index c00ef31936d3db340ceefa227cf9d159f4c6e0d5..1867ec7a227dd3a3d623fc6db430a9cbb36d823b 100644
--- a/demos/solo_real_estimation.yaml
+++ b/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]
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp
index 83cc8497bad7a63d861d97dd5540f17bb0a073d6..efd455589bd1cbb5889630802a58b0099e8e11f0 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/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();
///////////////////////////////////////////