diff --git a/CMakeLists.txt b/CMakeLists.txt
index d15dc6a79549afa2c0ca3b3f8d29ab7c9e161f40..c71a9885f3622a706f27d03cc06424378af29c7e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -79,19 +79,15 @@ ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-# option(BUILD_EXAMPLES "Build examples" OFF)
set(BUILD_TESTS true)
-set(BUILD_EXAMPLES false)
# Does this has any other interest
# but for the examples ?
# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
+IF(BUILD_DEMOS OR BUILD_TESTS)
string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-message("UPPER_NAME ${UPPER_NAME}")
+ENDIF(BUILD_DEMOS OR BUILD_TESTS)
#find dependencies.
@@ -212,7 +208,6 @@ TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfimu_INCLUDE_DIRS)
IF(BUILD_DEMOS)
- #Build examples
MESSAGE("Building demos.")
ADD_SUBDIRECTORY(demos)
ENDIF(BUILD_DEMOS)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index e8896eccf78f6f4aef3138b1c6914646c788617c..e070121147a47b1e73fcd72e714ac3afa01e8f42 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -45,8 +45,8 @@ target_link_libraries(solo_real_povcdl_estimation
z
)
-add_executable(solo_real_pov_estimation solo_real_pov_estimation.cpp)
-target_link_libraries(solo_real_pov_estimation
+add_executable(solo_imu_kine solo_imu_kine.cpp)
+target_link_libraries(solo_imu_kine
mcapi_utils
${wolfcore_LIBRARIES}
${wolfimu_LIBRARIES}
@@ -56,8 +56,19 @@ target_link_libraries(solo_real_pov_estimation
z
)
-add_executable(solo_mocap_imu solo_mocap_imu.cpp)
-target_link_libraries(solo_mocap_imu
+add_executable(solo_kine_mocap solo_kine_mocap.cpp)
+target_link_libraries(solo_kine_mocap
+ mcapi_utils
+ ${wolfcore_LIBRARIES}
+ ${wolfimu_LIBRARIES}
+ ${PLUGIN_NAME}
+ ${pinocchio_LIBRARIES}
+ /usr/local/lib/libcnpy.so
+ z
+)
+
+add_executable(solo_imu_mocap solo_imu_mocap.cpp)
+target_link_libraries(solo_imu_mocap
${wolfcore_LIBRARIES}
${wolfimu_LIBRARIES}
${PLUGIN_NAME}
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_imu_kine.cpp
similarity index 98%
rename from demos/solo_real_pov_estimation.cpp
rename to demos/solo_imu_kine.cpp
index efd455589bd1cbb5889630802a58b0099e8e11f0..2c1870e02af803af36326cd4f81280113d59d36a 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/demos/solo_imu_kine.cpp
@@ -75,15 +75,17 @@ 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>();
+ // robot
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
+
// Ceres setup
int max_num_iterations = config["max_num_iterations"].as<int>();
@@ -106,12 +108,16 @@ int main (int argc, char **argv) {
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());
+
+
+ // contacts
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
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>();
@@ -125,8 +131,6 @@ int main (int argc, char **argv) {
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>();
- std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
- unsigned int nbc = ee_names.size();
// Base to IMU transform
Quaterniond b_q_i(b_qvec_i);
assert(b_q_i.normalized().isApprox(b_q_i));
@@ -185,12 +189,13 @@ int main (int argc, char **argv) {
N = N < int(max_t/dt) ? N : int(max_t/dt);
}
- // Creating measurements from simulated trajectory
// Load the urdf model
Model model;
pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
std::cout << "model name: " << model.name << std::endl;
Data data(model);
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
// Recover end effector frame ids
std::vector<int> ee_ids;
@@ -216,15 +221,15 @@ int main (int argc, char **argv) {
//////////////////////
// COMPUTE INITIAL STATE
TimeStamp t0(t_arr[0]);
- Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
- Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
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
- // initial state
+ // initial configuration
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
@@ -299,14 +304,6 @@ int main (int argc, char **argv) {
// sen_imu->fixIntrinsics();
- ///////////////////////////////////////////
- // process measurements in sequential order
- double ts_since_last_kf = 0;
-
- std::vector<Vector3d> i_p_il_vec_prev;
- std::vector<Vector4d> i_qvec_l_vec_prev;
- std::vector<bool> feet_in_contact_prev;
-
//////////////////////////////////////////
// Vectors to store estimates at the time of data processing
// fbk -> feedback: to check if the estimation is stable enough for feedback control
diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_imu_mocap.cpp
similarity index 88%
rename from demos/solo_mocap_imu.cpp
rename to demos/solo_imu_mocap.cpp
index a8eff287c62c87bdd8be1caf188efb53989ec4a8..e8989d6356d6b85a466c754fce59f22586c2f215 100644
--- a/demos/solo_mocap_imu.cpp
+++ b/demos/solo_imu_mocap.cpp
@@ -246,7 +246,11 @@ int main (int argc, char **argv) {
double* extr_mocap_fbk_carr = new double[7*N];
// covariances computed on the fly
+ std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
//
std::vector<Vector3d> i_omg_oi_v;
@@ -256,35 +260,44 @@ int main (int argc, char **argv) {
for (unsigned int i=0; i < N; i++){
TimeStamp ts(t_arr[i]);
- ////////////////////////////////////
- ////////////////////////////////////
+ ////////////////
+ // PROCESS MOCAP
+ ////////////////
// 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<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
+ // 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();
+ ////////////////
+ ////////////////
+
+ //////////////
+ // PROCESS IMU
+ //////////////
+ // 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)
+
// 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 ){
@@ -294,16 +307,43 @@ int main (int argc, char **argv) {
// recover covariances at this point
auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
-
- CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
+
+ 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_fbk);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
+
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi_fbk);
- Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ // Retrieve diagonals
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
+ Qp_fbk_v.push_back(Qp_fbk_diag);
+ Qo_fbk_v.push_back(Qo_fbk_diag);
+ Qv_fbk_v.push_back(Qv_fbk_diag);
Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ Qm_fbk_v.push_back(Qm_fbk_diag);
// std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
// std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
@@ -395,7 +435,11 @@ int main (int argc, char **argv) {
double* Qbi_carr = new double[6*Nkf];
double* Qm_carr = new double[6*Nkf];
// feedback covariances
+ double* Qp_fbk_carr = new double[3*Nkf];
+ double* Qo_fbk_carr = new double[3*Nkf];
+ double* Qv_fbk_carr = new double[3*Nkf];
double* Qbi_fbk_carr = new double[6*Nkf];
+ double* Qm_fbk_carr = new double[6*Nkf];
// factor errors
double* fac_imu_err_carr = new double[9*Nkf];
@@ -444,7 +488,11 @@ int main (int argc, char **argv) {
memcpy(Qm_carr + 6*i, Qm_diag.data(), 6*sizeof(double));
// Recover feedback covariances
+ memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].data(), 6*sizeof(double));
// Factor errors
@@ -514,8 +562,12 @@ int main (int argc, char **argv) {
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, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
cnpy::npz_save(out_npz_file_path, "Qm", Qm_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_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_kine_mocap.cpp b/demos/solo_kine_mocap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b44013c1db341c94451977b287c8a0b71a3ffea1
--- /dev/null
+++ b/demos/solo_kine_mocap.cpp
@@ -0,0 +1,607 @@
+#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"
+#include "bodydynamics/sensor/sensor_point_feet_nomove.h"
+#include "bodydynamics/processor/processor_point_feet_nomove.h"
+#include "bodydynamics/capture/capture_point_feet_nomove.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");
+ 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>();
+
+ // robot
+ std::string robot_urdf = config["robot_urdf"].as<std::string>();
+ int dimc = config["dimc"].as<int>();
+ int nb_feet = config["nb_feet"].as<int>();
+
+ // 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>();
+
+ // IMU
+ double std_abs_bias_acc = config["std_abs_bias_acc"].as<double>();
+ double std_abs_bias_gyro = config["std_abs_bias_gyro"].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());
+
+ // base extrinsics
+ 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>>();
+ Eigen::Map<Vector3d> b_p_bi(b_p_bi_vec.data());
+ Eigen::Map<Vector4d> b_qvec_i(b_q_i_vec.data());
+
+ // contacts
+ std::vector<double> l_p_lg_vec = config["l_p_lg"].as<std::vector<double>>();
+ 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);
+ }
+
+ // Load the urdf model
+ Model model;
+ pinocchio::urdf::buildModel(robot_urdf, JointModelFreeFlyer(), model);
+ std::cout << "model name: " << model.name << std::endl;
+ Data data(model);
+ std::vector<std::string> ee_names = {"FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"};
+ unsigned int nbc = ee_names.size();
+
+ // Recover end effector frame ids
+ std::vector<int> ee_ids;
+ std::cout << "Frame ids" << std::endl;
+ for (auto ee_name: ee_names){
+ ee_ids.push_back(model.getFrameId(ee_name));
+ std::cout << ee_name << std::endl;
+ }
+
+ /////////////////////////
+ // WOLF enters the scene
+ // SETUP PROBLEM/SENSORS/PROCESSORS
+ std::string bodydynamics_root_dir = _WOLF_BODYDYNAMICS_ROOT_DIR;
+
+ ProblemPtr problem = Problem::create("PO", 3);
+ // 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
+
+ // initial configuration
+ Eigen::Map<Matrix<double,12, 1>> qa(qa_arr);
+ Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr);
+ VectorXd q(19); q << 0,0,0, o_q_i, qa; // TODO: put current base orientation estimate? YES
+ VectorXd dq(18); dq << 0,0,0, i_omg_oi, dqa; // TODO: put current base velocity estimate? YES
+
+ //////////////////////
+
+ // CERES WRAPPER
+ SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+ solver->getSolverOptions().max_num_iterations = max_num_iterations;
+
+ //===================================================== INITIALIZATION
+ // SENSOR + PROCESSOR POINT FEET NOMOVE
+ ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
+ intr_pfn->std_foot_nomove_ = std_odom3d_est;
+ VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
+ SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
+ SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
+ ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
+ ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm);
+
+
+ // 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();
+ }
+
+ Matrix3d w_R_m_init = q2R(w_qvec_wm);
+ Vector3d w_p_wi_init = w_p_wm + w_R_m_init*b_p_bi;
+
+ VectorComposite x_origin("PO", {w_p_wi_init, w_qvec_wm});
+ VectorComposite std_origin("PO", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones()});
+ // FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, time_tolerance_mocap); // if mocap used
+
+
+ //////////////////////////////////////////
+ // 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];
+
+ // covariances computed on the fly
+ std::vector<Vector3d> Qp_fbk_v; Qp_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qo_fbk_v; Qo_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector3d> Qv_fbk_v; Qv_fbk_v.push_back(Vector3d::Zero());
+ std::vector<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+ std::vector<Vector6d> Qm_fbk_v; Qm_fbk_v.push_back(Vector6d::Zero());
+
+ //
+ 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]);
+ std::cout << ts << std::endl;
+
+ ////////////////
+ // PROCESS MOCAP
+ ////////////////
+ // Get measurements
+ // retrieve traj data
+ 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
+
+ // 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();
+ ////////////////
+ ////////////////
+
+ /////////////////////
+ // PROCESS KINEMATICS
+ /////////////////////
+ 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);
+
+
+ // Or else, retrieve from preprocessed dataset
+ Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
+
+ Vector3d o_f_tot = Vector3d::Zero();
+ std::vector<Vector3d> l_f_l_vec;
+ // std::vector<Vector3d> o_f_l_vec;
+ std::vector<Vector3d> i_p_il_vec;
+ std::vector<Vector4d> i_qvec_l_vec;
+ // needing relative kinematics measurements
+ VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1;
+ VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
+ forwardKinematics(model, data, q_static, dq_static);
+ updateFramePlacements(model, data);
+
+ for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
+ auto b_T_l = data.oMf[ee_ids[i_ee]];
+
+
+ auto i_T_l = i_T_b * b_T_l;
+ Vector3d i_p_il = i_T_l.translation(); // meas
+ Matrix3d i_R_l = i_T_l.rotation();
+ Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas
+ i_p_il = i_p_il + i_R_l*l_p_lg;
+
+ Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas
+ // Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
+
+ l_f_l_vec.push_back(l_f_l);
+ // o_f_l_vec.push_back(o_f_l);
+ i_p_il_vec.push_back(i_p_il);
+ i_qvec_l_vec.push_back(i_qvec_l);
+ }
+
+ // 1) detect feet in contact
+ int nb_feet_in_contact = 0;
+ std::unordered_map<int, Vector3d> kin_incontact;
+ for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
+ // basic contact detection based on normal foot vel, things break if no foot is in contact
+ // std::cout << "F" << ee_names[i_ee] << " norm: " << wrench_meas_v[i_ee][i].norm() << std::endl;
+ // nb_feet: just for debugging/testing kinematic factor
+
+ if ((true)){ // !!! ASSUME ALWAYS IN CONTACT
+ // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
+ // if (contact_gtr(i_ee)){
+ nb_feet_in_contact++;
+ kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
+ }
+ }
+
+ CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
+ CPF->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;
+
+ // recover covariances at this point
+ auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+
+
+ // compute covariances of KF and capture stateblocks
+ Eigen::MatrixXd Qp_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qo_fbk = Eigen::Matrix3d::Identity(); // global or local?
+ Eigen::MatrixXd Qv_fbk = Eigen::Matrix3d::Identity();
+ Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ Eigen::MatrixXd Qmp_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+ Eigen::MatrixXd Qmo_fbk = Eigen::Matrix3d::Identity(); // extr mocap
+
+ solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below
+ problem->getCovarianceBlock(kf_last->getStateBlock('P'), Qp_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('O'), Qo_fbk);
+ problem->getCovarianceBlock(kf_last->getStateBlock('V'), Qv_fbk);
+
+ 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_fbk);
+ problem->getCovarianceBlock(sensor_pose->getO(), Qmo_fbk);
+
+
+ // Retrieve diagonals
+ Vector3d Qp_fbk_diag = Qp_fbk.diagonal();
+ Vector3d Qo_fbk_diag = Qo_fbk.diagonal();
+ Vector3d Qv_fbk_diag = Qv_fbk.diagonal();
+ Vector6d Qbi_fbk_diag = Qbi_fbk.diagonal();
+ Vector6d Qm_fbk_diag; Qm_fbk_diag << Qmp_fbk.diagonal(), Qmo_fbk.diagonal();
+ Vector3d Qmo_fbk_diag = Qmo_fbk.diagonal();
+
+ Qp_fbk_v.push_back(Qp_fbk_diag);
+ Qo_fbk_v.push_back(Qo_fbk_diag);
+ Qv_fbk_v.push_back(Qv_fbk_diag);
+ Qbi_fbk_v.push_back(Qbi_fbk_diag);
+ Qm_fbk_v.push_back(Qm_fbk_diag);
+
+ // std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
+ // std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
+
+
+ nb_kf++;
+ }
+
+ // Store current state estimation
+ VectorComposite state_fbk = problem->getState();
+ // 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'];
+ Vector3d o_v_oi = Vector3d::Zero();
+
+ // 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*i_R_b*b_p_bi);
+ // Vector3d o_p_ob = o_p_oi;
+ Vector3d o_v_ob = o_v_oi;
+
+ Vector3d imu_bias = Vector3d::Zero();
+ // imu_bias = sen_imu->getIntrinsic()->getState();
+ // imu_bias = sen_imu->getIntrinsic(ts)->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 = Vector3d::Zero();
+
+ 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];
+ // feedback covariances
+ double* Qp_fbk_carr = new double[3*Nkf];
+ double* Qo_fbk_carr = new double[3*Nkf];
+ double* Qv_fbk_carr = new double[3*Nkf];
+ double* Qbi_fbk_carr = new double[6*Nkf];
+ double* Qm_fbk_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);
+
+ 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);
+
+ // 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));
+
+ // Recover feedback covariances
+ memcpy(Qp_fbk_carr + 3*i, Qp_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qo_fbk_carr + 3*i, Qo_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qv_fbk_carr + 3*i, Qv_fbk_v[i].data(), 3*sizeof(double));
+ memcpy(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+ memcpy(Qm_fbk_carr + 6*i, Qm_fbk_v[i].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, "Qp_fbk", Qp_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qo_fbk", Qo_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qv_fbk", Qv_fbk_carr, {Nkf,3}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qbi_fbk", Qbi_fbk_carr, {Nkf,6}, "a");
+ cnpy::npz_save(out_npz_file_path, "Qm_fbk", Qm_fbk_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");
+
+}