Skip to content
Snippets Groups Projects
Commit 75cfde3e authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Debugging using mocap measurements

parent 170e7fdf
No related branches found
No related tags found
2 merge requests!18Release after RAL,!17After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
demos/processor_ft_preint.yaml
+ 1
− 2
View file @ 75cfde3e
...@@ -9,5 +9,4 @@ keyframe_vote: ...@@ -9,5 +9,4 @@ keyframe_vote:
max_buff_length: 50000 # motion deltas max_buff_length: 50000 # motion deltas
dist_traveled: 20000.0 # meters dist_traveled: 20000.0 # meters
angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg) angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg)
voting_active: false voting_active: false
voting_aux_active: false \ No newline at end of file
\ No newline at end of file
demos/processor_imu_solo12.yaml
+ 9
− 10
View file @ 75cfde3e
type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
time_tolerance: 0.0005 # Time tolerance for joining KFs
unmeasured_perturbation_std: 0.000001
keyframe_vote: keyframe_vote:
max_time_span: 0.05 # seconds angle_turned: 1000
# max_time_span: 0.075 # slower walking dist_traveled: 20000.0
max_buff_length: 100000000000 # motion deltas max_buff_length: 100000000000
dist_traveled: 20000.0 # meters max_time_span: 0.19999
angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg) voting_active: true
voting_active: true name: Main imu
\ No newline at end of file time_tolerance: 0.0005
type: ProcessorImu
unmeasured_perturbation_std: 1.0e-06
demos/solo_real_estimation.yaml
+ 6
− 0
View file @ 75cfde3e
...@@ -40,6 +40,9 @@ std_bp_drift: 0.005 # small drift ...@@ -40,6 +40,9 @@ std_bp_drift: 0.005 # small drift
std_pose_p: 0.001 std_pose_p: 0.001
std_pose_o_deg: 1 std_pose_o_deg: 1
std_mocap_extr_o_deg: 0.1
std_mocap_extr_p: 10
unfix_extr_sensor_pose: true
bias_imu_prior: [0,0,0, 0,0,0] bias_imu_prior: [0,0,0, 0,0,0]
# bias_imu_prior: [0,0,-0.011,-0.000079251, 0.00439540765, -0.0002120267] # bias_imu_prior: [0,0,-0.011,-0.000079251, 0.00439540765, -0.0002120267]
...@@ -71,6 +74,9 @@ artificial_bias_p: [0.0, 0.0, 0.0] ...@@ -71,6 +74,9 @@ artificial_bias_p: [0.0, 0.0, 0.0]
# artificial_bias_p: [3, 6, 4] # artificial_bias_p: [3, 6, 4]
# Data files # Data files
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/solo12_mpi_stamping_2020-09-29.npz" # data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/solo12_mpi_stamping_2020-09-29.npz"
# data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_09_50_Walking_Novicon.npz" # data_file_path: "/home/mfourmy/Documents/Phd_LAAS/data/quadruped_experiments/data_2020_10_08_09_50_Walking_Novicon.npz"
......
demos/solo_real_pov_estimation.cpp
+ 215
− 385
View file @ 75cfde3e
...@@ -32,6 +32,9 @@ ...@@ -32,6 +32,9 @@
#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h" #include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
#include "core/yaml/parser_yaml.h" #include "core/yaml/parser_yaml.h"
#include <core/sensor/sensor_pose.h>
#include <core/processor/processor_pose.h>
// IMU // IMU
#include "imu/sensor/sensor_imu.h" #include "imu/sensor/sensor_imu.h"
#include "imu/capture/capture_imu.h" #include "imu/capture/capture_imu.h"
...@@ -107,6 +110,15 @@ int main (int argc, char **argv) { ...@@ -107,6 +110,15 @@ int main (int argc, char **argv) {
Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data()); Eigen::Map<Vector3d> l_p_lg(l_p_lg_vec.data());
double fz_thresh = config["fz_thresh"].as<double>(); 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>();
std::string data_file_path = config["data_file_path"].as<std::string>(); 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::string out_npz_file_path = config["out_npz_file_path"].as<std::string>();
...@@ -114,6 +126,8 @@ int main (int argc, char **argv) { ...@@ -114,6 +126,8 @@ int main (int argc, char **argv) {
unsigned int nbc = ee_names.size(); unsigned int nbc = ee_names.size();
// Base to IMU transform // Base to IMU transform
Quaterniond b_q_i(b_qvec_i); 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 b_T_i(b_q_i, b_p_bi);
SE3 i_T_b = b_T_i.inverse(); SE3 i_T_b = b_T_i.inverse();
Matrix3d b_R_i = b_T_i.rotation(); Matrix3d b_R_i = b_T_i.rotation();
...@@ -139,6 +153,9 @@ int main (int argc, char **argv) { ...@@ -139,6 +153,9 @@ int main (int argc, char **argv) {
// cnpy::NpyArray o_R_i_npyarr = arr_map["o_R_i"]; // 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_p_wm_npyarr = arr_map["w_p_wm"];
cnpy::NpyArray w_q_m_npyarr = arr_map["w_q_m"]; 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"]; // cnpy::NpyArray w_R_m_npyarr = arr_map["w_R_m"];
double* t_arr = t_npyarr.data<double>(); double* t_arr = t_npyarr.data<double>();
double* imu_acc_arr = imu_acc_npyarr.data<double>(); double* imu_acc_arr = imu_acc_npyarr.data<double>();
...@@ -155,6 +172,10 @@ int main (int argc, char **argv) { ...@@ -155,6 +172,10 @@ int main (int argc, char **argv) {
// double* o_R_i_arr = o_R_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_p_wm_arr = w_p_wm_npyarr.data<double>();
double* w_q_m_arr = w_q_m_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>(); // double* w_R_m_arr = w_R_m_npyarr.data<double>();
unsigned int N = t_npyarr.shape[0]; unsigned int N = t_npyarr.shape[0];
if (max_t > 0){ if (max_t > 0){
...@@ -184,7 +205,7 @@ int main (int argc, char **argv) { ...@@ -184,7 +205,7 @@ int main (int argc, char **argv) {
ProblemPtr problem = Problem::create("POV", 3); ProblemPtr problem = Problem::create("POV", 3);
// add a tree manager for sliding window optimization // add a tree manager for sliding window optimization
ParserYaml parser = ParserYaml("demos/params_tree_manager.yaml", bodydynamics_root_dir); ParserYaml parser = ParserYaml("demos/tree_manager.yaml", bodydynamics_root_dir);
ParamsServer server = ParamsServer(parser.getParams()); ParamsServer server = ParamsServer(parser.getParams());
auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server); auto tree_manager = TreeManagerSlidingWindow::create("tree_manager", server);
problem->setTreeManager(tree_manager); problem->setTreeManager(tree_manager);
...@@ -212,19 +233,41 @@ int main (int argc, char **argv) { ...@@ -212,19 +233,41 @@ int main (int argc, char **argv) {
//===================================================== INITIALIZATION //===================================================== INITIALIZATION
// SENSOR + PROCESSOR IMU // 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"); 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); 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"); 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); ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_ftp_base);
// SENSOR + PROCESSOR POINT FEET NOMOVE // // SENSOR + PROCESSOR POINT FEET NOMOVE
ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>(); // ParamsSensorPointFeetNomovePtr intr_pfn = std::make_shared<ParamsSensorPointFeetNomove>();
intr_pfn->std_foot_nomove_ = std_odom3d_est; // intr_pfn->std_foot_nomove_ = std_odom3d_est;
VectorXd extr; // default size for dynamic vector is 0-0 -> what we need // VectorXd extr; // default size for dynamic vector is 0-0 -> what we need
SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn); // SensorBasePtr sen_pfnm_base = problem->installSensor("SensorPointFeetNomove", "SenPfnm", extr, intr_pfn);
SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base); // SensorPointFeetNomovePtr sen_pfnm = std::static_pointer_cast<SensorPointFeetNomove>(sen_pfnm_base);
ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>(); // ParamsProcessorPointFeetNomovePtr params_pfnm = std::make_shared<ParamsProcessorPointFeetNomove>();
ProcessorBasePtr proc_pfnm_base = problem->installProcessor("ProcessorPointFeetNomove", "PPointFeetNomove", sen_pfnm, params_pfnm); // 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 = 0.005;
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();
}
...@@ -233,15 +276,14 @@ int main (int argc, char **argv) { ...@@ -233,15 +276,14 @@ int main (int argc, char **argv) {
// - Manually create the first KF and its pose and velocity priors // - Manually create the first KF and its pose and velocity priors
// - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later) // - call setOrigin on processors isMotion since the flag prior_is_set is not true when doing installProcessor (later)
// VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()}); VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
Vector4d q_identity; q_identity << 0,0,0,1;
VectorComposite x_origin("POV", {Vector3d::Zero(), q_identity, Vector3d::Zero()});
VectorComposite std_origin("POV", {std_prior_p*Vector3d::Ones(), std_prior_o*Vector3d::Ones(), std_prior_v*Vector3d::Ones()}); 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->setPriorFactor(x_origin, std_origin, t0, 0.0005);
FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, 0.0005); // if mocap used
proc_imu->setOrigin(KF1); proc_imu->setOrigin(KF1);
//////////////////////////////////////////// //////////////////////////////////////////
// INITIAL BIAS FACTORS // INITIAL BIAS FACTORS
// Add absolute factor on Imu biases to simulate a fix() // 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(); Vector6d std_abs_imu; std_abs_imu << std_abs_bias_acc*Vector3d::Ones(), std_abs_bias_gyro*Vector3d::Ones();
...@@ -249,8 +291,9 @@ int main (int argc, char **argv) { ...@@ -249,8 +291,9 @@ int main (int argc, char **argv) {
CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0); CaptureBasePtr capbi0 = CaptureBase::emplace<CaptureBase>(KF1, "bi0", t0);
FeatureBasePtr featbi0 = FeatureBase::emplace<FeatureBase>(capbi0, "bi0", bias_imu_prior, Q_bi); 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); FactorBasePtr facbi0 = FactorBase::emplace<FactorBlockAbsolute>(featbi0, featbi0, KF1->getCaptureOf(sen_imu)->getSensorIntrinsic(), nullptr, false);
sen_imu->getStateBlock('I')->setState(bias_imu_prior); sen_imu->getStateBlock('I')->setState(bias_imu_prior);
/////////////////////////////////////////// ///////////////////////////////////////////
// process measurements in sequential order // process measurements in sequential order
double ts_since_last_kf = 0; double ts_since_last_kf = 0;
...@@ -259,32 +302,27 @@ int main (int argc, char **argv) { ...@@ -259,32 +302,27 @@ int main (int argc, char **argv) {
std::vector<Vector4d> i_qvec_l_vec_prev; std::vector<Vector4d> i_qvec_l_vec_prev;
std::vector<bool> feet_in_contact_prev; std::vector<bool> feet_in_contact_prev;
// for point feet factor
std::vector<Vector3d> i_p_il_vec_origin;
FrameBasePtr KF_origin = KF1;
////////////////////////////////////////// //////////////////////////////////////////
// Vectors to store estimates at the time of data processing // Vectors to store estimates at the time of data processing
// fbk -> feedback: to check if the estimation is stable enough for feedback control // fbk -> feedback: to check if the estimation is stable enough for feedback control
std::vector<Vector3d> p_ob_fbk_v; std::vector<Vector3d> p_ob_fbk_v;
std::vector<Vector4d> q_ob_fbk_v; std::vector<Vector4d> q_ob_fbk_v;
std::vector<Vector3d> v_ob_fbk_v; std::vector<Vector3d> v_ob_fbk_v;
double o_p_ob_carr[3*N]; double o_p_ob_fbk_carr[3*N];
double o_q_b_carr[4*N]; double o_q_b_fbk_carr[4*N];
double o_v_ob_carr[3*N]; double o_v_ob_fbk_carr[3*N];
double imu_bias_carr[6*N]; double imu_bias_fbk_carr[6*N];
double extr_mocap_fbk_carr[7*N];
std::vector<Vector3d> i_omg_oi_v; std::vector<Vector3d> i_omg_oi_v;
////////////////////////////////////////// //////////////////////////////////////////
unsigned int nb_kf = 1; unsigned int nb_kf = 1;
for (unsigned int i=0; i < N; i++){ for (unsigned int i=0; i < N; i++){
TimeStamp ts(t_arr[i]); TimeStamp ts(t_arr[i]);
//////////////////////////////////// ////////////////////////////////////
// Retrieve current state // Retrieve current state
VectorComposite curr_state = problem->getState(); VectorComposite curr_state = problem->getState();
Vector4d o_qvec_i_curr = curr_state['O']; Vector4d o_qvec_i_curr = curr_state['O'];
Quaterniond o_q_i_curr(o_qvec_i_curr); Quaterniond o_q_i_curr(o_qvec_i_curr);
...@@ -299,51 +337,54 @@ int main (int argc, char **argv) { ...@@ -299,51 +337,54 @@ int main (int argc, char **argv) {
Eigen::Map<Matrix<double,12, 1>> tau(tau_arr+12*i); 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>> qa(qa_arr+12*i);
Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i); Eigen::Map<Matrix<double,12, 1>> dqa(dqa_arr+12*i);
// Eigen::Map<Matrix<double,4, 1>> contact_gtr(contact_arr+4*i); // int conversion does not work!
// Eigen::Map<Matrix<double,12, 1>> b_p_bl_mocap(b_p_bl_mocap_arr+12*i); // int conversion does not work!
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 // store i_omg_oi for later computation of o_v_ob from o_v_oi
i_omg_oi_v.push_back(i_omg_oi); i_omg_oi_v.push_back(i_omg_oi);
Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState(); Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
// std::cout << "bias_acc_gyro: " << bias_acc_gyro.transpose() << std::endl; Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
Vector3d i_omg_oi_unbiased = (i_omg_oi - bias_acc_gyro.tail<3>());
//////////////////////////////////// ////////////////////////////////////
// IMU // IMU
Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi; Vector6d acc_gyr_meas; acc_gyr_meas << imu_acc, i_omg_oi;
// Vector6d acc_gyr_meas; acc_gyr_meas << 0,0,9.806, 0,0,0;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov(); Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
//////////////////////////////////// ////////////////////////////////////
// Kinematics + forces // Kinematics + forces
SE3 o_T_i(o_q_i_curr, Vector3d::Zero()); // SE3 o_T_i(o_q_i_curr, Vector3d::Zero());
Matrix3d o_R_i = o_T_i.rotation(); // Matrix3d o_R_i = o_T_i.rotation();
Matrix3d i_R_o = o_R_i.transpose(); // Matrix3d i_R_o = o_R_i.transpose();
Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr; // Vector3d i_v_oi_curr = i_R_o * o_v_oi_curr;
Vector3d i_acc = imu_acc + i_R_o * gravity(); // Vector3d i_acc = imu_acc + i_R_o * gravity();
Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr); // Vector3d i_asp_i = i_acc - i_omg_oi_unbiased.cross(i_v_oi_curr);
VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa; // VectorXd q(19); q << 0,0,0, o_q_i_curr.coeffs(), qa;
VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa; // VectorXd dq(18); dq << i_v_oi_curr, i_omg_oi_unbiased, dqa;
VectorXd ddq(model.nv); ddq.setZero(); // VectorXd ddq(model.nv); ddq.setZero();
ddq.head<3>() = i_asp_i; // ddq.head<3>() = i_asp_i;
// TODO: add other terms to compute forces (ddqa...) // // TODO: add other terms to compute forces (ddqa...)
// update and retrieve kinematics // // update and retrieve kinematics
forwardKinematics(model, data, q); // forwardKinematics(model, data, q);
updateFramePlacements(model, data); // updateFramePlacements(model, data);
// compute all linear jacobians in feet frames (only for quadruped) // // compute all linear jacobians in feet frames (only for quadruped)
MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero(); // MatrixXd Jlinvel(12, model.nv); Jlinvel.setZero();
for (unsigned int i_ee=0; i_ee < nbc; i_ee++){ // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
MatrixXd Jee(6, model.nv); Jee.setZero(); // MatrixXd Jee(6, model.nv); Jee.setZero();
computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee); // computeFrameJacobian(model, data, q, ee_ids[i_ee], Jee);
Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>(); // Jlinvel.block(3*i_ee, 0, 3, model.nv) = Jee.topRows<3>();
} // }
// estimate forces from torques // // estimate forces from torques
VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer) // VectorXd tau_cf = rnea(model, data, q, dq, ddq); // compute total torques applied on the joints (and free flyer)
tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer) // tau_cf.tail(model.nv-6) -= tau; // remove measured joint torque (not on the free flyer)
// VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat // VectorXd forces = Jlinvel_reduced.transpose().lu().solve(tau_joints); // works only for exactly determined system, removing free flyer from the system -> 12x12 J mat
// Solve in Least square sense (3 ways): // Solve in Least square sense (3 ways):
...@@ -352,187 +393,86 @@ int main (int argc, char **argv) { ...@@ -352,187 +393,86 @@ int main (int argc, char **argv) {
// (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2) // (A.transpose() * A).ldlt().solve(A.transpose() * b) // solve the normal equation (twice less accurate than other 2)
// Or else, retrieve from preprocessed dataset // Or else, retrieve from preprocessed dataset
Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i); // Eigen::Map<Matrix<double,12, 1>> l_forces(l_forces_arr+12*i);
Vector3d o_f_tot = Vector3d::Zero(); // Vector3d o_f_tot = Vector3d::Zero();
std::vector<Vector3d> l_f_l_vec; // std::vector<Vector3d> l_f_l_vec;
std::vector<Vector3d> o_f_l_vec; // std::vector<Vector3d> o_f_l_vec;
std::vector<Vector3d> i_p_il_vec; // std::vector<Vector3d> i_p_il_vec;
std::vector<Vector4d> i_qvec_l_vec; // std::vector<Vector4d> i_qvec_l_vec;
// needing relative kinematics measurements // // needing relative kinematics measurements
VectorXd q_static = q; q_static.head<7>() << 0,0,0, 0,0,0,1; // 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; // VectorXd dq_static = dq; dq_static.head<6>() << 0,0,0, 0,0,0;
forwardKinematics(model, data, q_static, dq_static); // forwardKinematics(model, data, q_static, dq_static);
updateFramePlacements(model, data); // updateFramePlacements(model, data);
for (unsigned int i_ee=0; i_ee < nbc; i_ee++){ // // std::cout << "" << std::endl;
auto b_T_l = data.oMf[ee_ids[i_ee]]; // for (unsigned int i_ee=0; i_ee < nbc; i_ee++){
auto i_T_l = i_T_b * b_T_l; // // std::cout << i_ee << std::endl;
Vector3d i_p_il = i_T_l.translation(); // meas // auto b_T_l = data.oMf[ee_ids[i_ee]];
Matrix3d i_R_l = i_T_l.rotation();
Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas // // overides with value from mocap
i_p_il = i_p_il + i_R_l*l_p_lg; // // std::cout << b_T_l.translation().transpose() << std::endl;
// b_T_l.translation(b_p_bl_mocap.segment<3>(3*i_ee));
Vector3d l_f_l = l_forces.segment(3*i_ee, 3); // meas // // std::cout << b_T_l.translation().transpose() << std::endl;
Vector3d o_f_l = o_R_i*i_R_l * l_f_l; // for contact test
o_f_tot += o_f_l; // auto i_T_l = i_T_b * b_T_l;
// Vector3d i_p_il = i_T_l.translation(); // meas
l_f_l_vec.push_back(l_f_l); // Matrix3d i_R_l = i_T_l.rotation();
o_f_l_vec.push_back(o_f_l); // Vector4d i_qvec_l = Quaterniond(i_R_l).coeffs(); // meas
i_p_il_vec.push_back(i_p_il); // i_p_il = i_p_il + i_R_l*l_p_lg;
i_qvec_l_vec.push_back(i_qvec_l);
} // 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
// initialization for point feet factor
if (i == 0){ // l_f_l_vec.push_back(l_f_l);
i_p_il_vec_origin = i_p_il_vec; // // o_f_l_vec.push_back(o_f_l);
} // i_p_il_vec.push_back(i_p_il);
// std::cout << "o_f_tot: " << o_f_tot.transpose() << std::endl; // i_qvec_l_vec.push_back(i_qvec_l);
// std::cout << "o_f_tot + m*g: " << (o_f_tot + data.mass[0]*model.gravity.linear()).transpose() << std::endl; // }
CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov); CaptureImuPtr CImu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
CImu->process(); CImu->process();
////////////////////////////////////////
////////////////////////////////////////
////////////////////////////////////////
// Integrate leg odom in 4 separate factors
////////////////////////////////////////
// 2) fill the leg odometry data matrix, depending on the contact dimension
// forwardKinematics and updateFramePlacements have been called on b centered q and dq
// gyro
// // .) nothing
// data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0;
// ..) direct gyro
// data_legodom.rightCols<1>() << acc_gyr_meas.tail<3>(), 0,0,0,0, 0,0,0;
// ...) gyro compensated
// for (int i_ee=0; i_ee < 4; i_ee++)
// {
// Eigen::MatrixXd data_legodom(10,2);
// // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0;
// data_legodom.rightCols<1>() << i_omg_oi_unbiased, 0,0,0,0, 0,0,0;
// Vector3d l_v_bl = getFrameVelocity(model, data, ee_ids[i_ee]).linear();
// Vector3d l_v_il = l_v_bl;
// data_legodom.col(0) << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee], l_v_il;
// // data_legodom.col(0) << 0,0,0, 0,0,0,1, 0,0,0;
// // 3) process the data and its cov
// Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
// CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d_lst[i_ee], data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
// CLO->process();
// }
////////////////////////////////////////
////////////////////////////////////////
// 1) detect feet in contact // 1) detect feet in contact
std::vector<int> feet_in_contact; // int nb_feet_in_contact = 0;
int nb_feet_in_contact = 0; // std::unordered_map<int, Vector3d> kin_incontact;
std::unordered_map<int, Vector3d> kin_incontact; // for (unsigned int i_ee=0; i_ee<nbc; i_ee++){
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
// 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;
// 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
// nb_feet: just for debugging/testing kinematic factor
if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){ // // if ((o_f_l_vec[i_ee](2) > fz_thresh) && (nb_feet_in_contact < nb_feet)){
feet_in_contact.push_back(i_ee); // if (contact_gtr(i_ee)){
nb_feet_in_contact++; // nb_feet_in_contact++;
kin_incontact.insert({i_ee, i_p_il_vec[i_ee]}); // kin_incontact.insert({i_ee, i_p_il_vec[i_ee]});
}
}
CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
CPF->process();
// // 2) fill the leg odometry data matrix, depending on the contact dimension
// // forwardKinematics and updateFramePlacements have been called on b centered q and dq
// // Option a) Differential kinematics
// Eigen::MatrixXd data_legodom(10,feet_in_contact.size()+1);
// // gyro
// // // .) nothing
// // data_legodom.rightCols<1>() << 0,0,0, 0,0,0,0, 0,0,0;
// // ..) direct gyro
// data_legodom.rightCols<1>() << acc_gyr_meas.tail<3>(), 0,0,0,0, 0,0,0;
// // ...) gyro compensated
// // Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
// // data_legodom.rightCols<1>() << (acc_gyr_meas.tail<3>()-bias_acc_gyro.tail<3>()), 0,0,0,0, 0,0,0;
// unsigned int j = 0;
// for (int i_ee_c: feet_in_contact){
// Vector3d l_v_bl = getFrameVelocity(model, data, ee_ids[i_ee_c]).linear();
// // data_legodom.col(j) << i_p_il_vec[i_ee], i_qvec_l_vec[i_ee], l_v_bl;
// data_legodom.col(j) << 0,0,0, 0,0,0,1, 0,0,0;
// j++;
// }
// // 3) process the data and its cov
// Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity();
// CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_DIFFERENTIAL_KIN);
// CLO->process();
// // Option b) Relative kinematics
// if (i > 0){
// // pbl1_km, pbl1_k, pbl2_km, pbl2_k, ..., w_b
// Eigen::MatrixXd data_legodom(3,2*nb_feet_in_contact+1);
// // data_legodom.rightCols<1>() = acc_gyr_meas.tail<3>();
// // data_legodom.rightCols<1>() = Vector3d::Zero();
// // Compensate bias
// Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
// data_legodom.rightCols<1>() = (acc_gyr_meas.tail<3>() - bias_acc_gyro.tail<3>());
// for (unsigned int j=0; j < feet_in_contact.size(); j++){
// data_legodom.col(2*j ) << i_p_il_vec_prev[j];
// data_legodom.col(2*j+1) << i_p_il_vec[j];
// // data_legodom.col(2*j ) << 0,0,0;
// // data_legodom.col(2*j+1) << 0,0,0;
// } // }
// }
// // 3) process the data and its cov // CapturePointFeetNomovePtr CPF = std::make_shared<CapturePointFeetNomove>(ts, sen_pfnm, kin_incontact);
// Eigen::Matrix6d data_legodom3D_cov = pow(std_odom3d_est, 2) * Eigen::Matrix6d::Identity(); // CPF->process();
// CaptureLegOdomPtr CLO = std::make_shared<CaptureLegOdom>(ts, sen_odom3d, data_legodom, data_legodom3D_cov, dt, CaptureLegOdomType::POINT_FEET_RELATIVE_KIN);
// CLO->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, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
// ts_since_last_kf += dt;
// if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){
// // problem->print(4,true,true,true);
// auto kf = problem->emplaceFrame(ts);
// problem->keyFrameCallback(kf, nullptr, 0.0005);
// std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// std::cout << report << std::endl;
// ts_since_last_kf = 0;
// // std::cout << "Extr SPose: " << sensor_pose->getP()->getState().transpose() << " " << sensor_pose->getO()->getState().transpose() << std::endl;
// } // }
// i_p_il_vec_prev = i_p_il_vec;
// i_qvec_l_vec_prev = i_qvec_l_vec;
// add zero vel artificial factor
if (problem->getTrajectory()->getFrameMap().size() > nb_kf){
auto kf_pair = problem->getTrajectory()->getFrameMap().rbegin();
std::cout << "New KF " << kf_pair->first << std::endl;
auto kf = kf_pair->second;
// CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(kf, "Vel0", kf_pair->first);
// FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", Vector3d::Zero(), 0.00001*Matrix3d::Ones());
// FactorBasePtr facV0 = FactorBase::emplace<FactorBlockAbsolute>(featV0, featV0, kf->getV(), nullptr, false);
// point feet factor for each foot (short cut to avoid using a processor...)
for (int i_ee=0; i_ee < 4; i_ee++)
{
Vector6d meas; meas << i_p_il_vec_origin[i_ee], i_p_il_vec[i_ee];
Matrix3d cov = pow(std_odom3d_est, 2)*Matrix3d::Identity();
// CaptureBasePtr cap = CaptureBase::emplace<CaptureBase>(kf, "PointFeet", kf_pair->first);
// FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "PointFeet", meas, cov);
// FactorPointFeetNomovePtr fac = FactorBase::emplace<FactorPointFeetNomove>(feat, feat, KF_origin, nullptr, false);
}
i_p_il_vec_origin = i_p_il_vec;
nb_kf++;
KF_origin = kf;
}
ts_since_last_kf += dt; // solve every new KF
if ((solve_every_sec > 0) && (ts_since_last_kf >= solve_every_sec)){ if (problem->getTrajectory()->getFrameMap().size() > nb_kf ){
// problem->print(4,true,true,true);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl; std::cout << report << std::endl;
ts_since_last_kf = 0; nb_kf = problem->getTrajectory()->getFrameMap().size();
} }
// Store current state estimation // Store current state estimation
...@@ -542,18 +482,23 @@ int main (int argc, char **argv) { ...@@ -542,18 +482,23 @@ int main (int argc, char **argv) {
Vector3d o_v_oi = state_fbk['V']; Vector3d o_v_oi = state_fbk['V'];
// IMU to base frame // IMU to base frame
o_R_i = Quaterniond(o_qvec_i).toRotationMatrix(); 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; Matrix3d o_R_b = o_R_i * i_R_b;
Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs(); Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib; 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_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;
Vector6d imu_bias = sen_imu->getIntrinsic()->getState(); 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_carr+3*i, o_p_ob.data(), 3*sizeof(double)); mempcpy(o_p_ob_fbk_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_q_b_fbk_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(o_v_ob_fbk_carr+3*i, o_v_ob.data(), 3*sizeof(double));
mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*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); p_ob_fbk_v.push_back(o_p_ob);
q_ob_fbk_v.push_back(o_qvec_b); q_ob_fbk_v.push_back(o_qvec_b);
...@@ -565,159 +510,40 @@ int main (int argc, char **argv) { ...@@ -565,159 +510,40 @@ int main (int argc, char **argv) {
/////////////////////////////////////////// ///////////////////////////////////////////
//////////////// SOLVING ////////////////// //////////////// SOLVING //////////////////
/////////////////////////////////////////// ///////////////////////////////////////////
problem->print(4,true,true,true);
if (solve_end){ if (solve_end){
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF); std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
problem->print(4,true,true,true); problem->print(4,true,true,true);
std::cout << report << std::endl; std::cout << report << std::endl;
} }
////////////////////////////////////// double o_p_ob_carr[3*N];
////////////////////////////////////// double o_q_b_carr[4*N];
// STORE DATA OFFLINE // double o_v_ob_carr[3*N];
// NOT WORKING WITH SLIDING WINDOW // double imu_bias_carr[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];
// std::fstream file_est; Vector3d o_p_oi = state_est['P'];
// std::fstream file_fbk; Vector4d o_qvec_i = state_est['O'];
// std::fstream file_kfs; Vector3d o_v_oi = state_est['V'];
// std::fstream file_cov;
// file_est.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/est.csv", std::fstream::out); Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
// file_fbk.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/fbk.csv", std::fstream::out); std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
// file_kfs.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/kfs.csv", std::fstream::out); Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
// file_cov.open("/home/mfourmy/Documents/Phd_LAAS/phd_misc/centroidalkin/cov.csv", std::fstream::out);
// std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz\n"; Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
// file_est << header_est; Matrix3d o_R_b = o_R_i * i_R_b;
// file_fbk << header_est; Vector4d o_qvec_b = Quaterniond(o_R_b).coeffs();
// std::string header_kfs = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax,bay,baz,bwx,bwy,bwz\n"; Vector3d o_p_ob = o_p_oi + o_R_i * i_p_ib;
// file_kfs << header_kfs; Vector3d o_v_ob = o_v_oi + o_R_i * i_omg_oi_unbiased.cross(i_R_b*b_p_bi);
// std::string header_cov = "t,Qpx,Qpy,Qpz,Qqx,Qqy,Qqz,Qvx,Qvy,Qvz,Qbax,Qbay,Qbaz,Qbwx,Qbwy,Qbwz\n"; // Vector3d o_p_ob = o_p_oi;
// file_cov << header_cov; // Vector3d o_v_ob = o_v_oi;
// VectorComposite state_est; mempcpy(o_p_ob_carr+3*i, o_p_ob.data(), 3*sizeof(double));
// double o_p_ob_carr[3*N]; mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
// double o_q_b_carr[4*N]; mempcpy(o_v_ob_carr+3*i, o_v_ob.data(), 3*sizeof(double));
// double o_v_ob_carr[3*N]; mempcpy(imu_bias_carr+3*i, imu_bias.data(), 3*sizeof(double));
// for (unsigned int i=0; i < N; i++) { }
// 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'];
// Vector6d bias_acc_gyro = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
// Vector3d i_omg_oi_unbiased = (i_omg_oi - bias_acc_gyro.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);
// 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));
// file_est << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
// << t_arr[i] << ","
// << o_p_ob(0) << ","
// << o_p_ob(1) << ","
// << o_p_ob(2) << ","
// << o_qvec_b(0) << ","
// << o_qvec_b(1) << ","
// << o_qvec_b(2) << ","
// << o_qvec_b(3) << ","
// << o_v_oi(0) << ","
// << o_v_oi(1) << ","
// << o_v_oi(2)
// << "\n";
// }
// VectorComposite kf_state;
// CaptureBasePtr cap_imu;
// VectorComposite bi_bias_est;
// for (auto& elt: problem->getTrajectory()->getFrameMap()){
// auto kf = elt.second;
// if (kf->isKey()){
// kf_state = kf->getState();
// cap_imu = kf->getCaptureOf(sen_imu);
// bi_bias_est = cap_imu->getState();
// file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
// << kf->getTimeStamp().get() << ","
// << kf_state['P'](0) << ","
// << kf_state['P'](1) << ","
// << kf_state['P'](2) << ","
// << kf_state['O'](0) << ","
// << kf_state['O'](1) << ","
// << kf_state['O'](2) << ","
// << kf_state['O'](3) << ","
// << kf_state['V'](0) << ","
// << kf_state['V'](1) << ","
// << kf_state['V'](2) << ","
// << bi_bias_est['I'](0) << ","
// << bi_bias_est['I'](1) << ","
// << bi_bias_est['I'](2) << ","
// << bi_bias_est['I'](3) << ","
// << bi_bias_est['I'](4) << ","
// << bi_bias_est['I'](5)
// << "\n";
// // 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();
// 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);
// solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise
// problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi);
// file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
// << kf->getTimeStamp().get() << ","
// << Qp(0,0) << ","
// << Qp(1,1) << ","
// << Qp(2,2) << ","
// << Qo(0,0) << ","
// << Qo(1,1) << ","
// << Qo(2,2) << ","
// << Qv(0,0) << ","
// << Qv(1,1) << ","
// << Qv(2,2) << ","
// << Qbi(0,0) << ","
// << Qbi(1,1) << ","
// << Qbi(2,2) << ","
// << Qbi(3,3) << ","
// << Qbi(4,4) << ","
// << Qbi(5,5)
// << "\n";
// }
// }
// for (unsigned int i=0; i < N; i++) {
// file_fbk << std::setprecision(std::numeric_limits<long double>::digits10 + 1)
// << t_arr[i] << ","
// << p_ob_fbk_v[i](0) << ","
// << p_ob_fbk_v[i](1) << ","
// << p_ob_fbk_v[i](2) << ","
// << q_ob_fbk_v[i](0) << ","
// << q_ob_fbk_v[i](1) << ","
// << q_ob_fbk_v[i](2) << ","
// << q_ob_fbk_v[i](3) << ","
// << v_ob_fbk_v[i](0) << ","
// << v_ob_fbk_v[i](1) << ","
// << v_ob_fbk_v[i](2)
// << "\n";
// }
// file_est.close();
// file_kfs.close();
// file_cov.close();
// Save trajectories in npz file // Save trajectories in npz file
...@@ -731,12 +557,16 @@ int main (int argc, char **argv) { ...@@ -731,12 +557,16 @@ int main (int argc, char **argv) {
cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a"); cnpy::npz_save(out_npz_file_path, "qa", qa_arr, {N,12}, "a");
// save estimates // 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_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_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"); cnpy::npz_save(out_npz_file_path, "o_v_ob", o_v_ob_carr, {N,3}, "a");
// and biases // 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_biases", imu_bias_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");
} }
demos/params_tree_manager.yaml → demos/tree_manager.yaml
+ 2
− 2
View file @ 75cfde3e
config: config:
problem: problem:
tree_manager: tree_manager:
fix_first_frame: true n_fix_first_frames: 3
n_frames: 5 n_frames: 100000000000
type: TreeManagerSlidingWindow type: TreeManagerSlidingWindow
viral_remove_empty_parent: true viral_remove_empty_parent: true
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment