diff --git a/demos/solo_mocap_imu.cpp b/demos/solo_mocap_imu.cpp
index da0270444b930aa6dec61620599d4107bf894665..a8eff287c62c87bdd8be1caf188efb53989ec4a8 100644
--- a/demos/solo_mocap_imu.cpp
+++ b/demos/solo_mocap_imu.cpp
@@ -61,42 +61,25 @@ 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>();
@@ -201,8 +184,8 @@ int main (int argc, char **argv) {
// 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);
+ ProcessorBasePtr proc_imu_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_imu_base);
// SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb)
@@ -226,10 +209,13 @@ int main (int argc, char **argv) {
sensor_pose->fixExtrinsics();
}
- VectorComposite x_origin("POV", {w_p_wm, w_qvec_wm, Vector3d::Zero()});
+ 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("POV", {w_p_wi_init, 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
+ FrameBasePtr KF1 = problem->setPriorInitialGuess(x_origin, t0, time_tolerance_mocap); // if mocap used
proc_imu->setOrigin(KF1);
@@ -259,6 +245,10 @@ int main (int argc, char **argv) {
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<Vector6d> Qbi_fbk_v; Qbi_fbk_v.push_back(Vector6d::Zero());
+
+ //
std::vector<Vector3d> i_omg_oi_v;
//////////////////////////////////////////
@@ -266,22 +256,12 @@ int main (int argc, char **argv) {
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
@@ -311,11 +291,30 @@ int main (int argc, char **argv) {
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
std::cout << ts << " ";
std::cout << report << std::endl;
- nb_kf = problem->getTrajectory()->getFrameMap().size();
+
+ // recover covariances at this point
+ auto kf_last = problem->getTrajectory()->getFrameMap().rbegin()->second;
+
+ CaptureBasePtr cap_imu = kf_last->getCaptureOf(sen_imu);
+
+ Eigen::MatrixXd Qbi_fbk = Eigen::Matrix6d::Identity();
+ 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();
+
+
+ Qbi_fbk_v.push_back(Qbi_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(ts);
+ 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'];
@@ -326,11 +325,12 @@ int main (int argc, char **argv) {
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_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;
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));
@@ -394,6 +394,9 @@ int main (int argc, char **argv) {
double* Qv_carr = new double[3*Nkf];
double* Qbi_carr = new double[6*Nkf];
double* Qm_carr = new double[6*Nkf];
+ // feedback covariances
+ double* Qbi_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];
@@ -440,6 +443,10 @@ int main (int argc, char **argv) {
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(Qbi_fbk_carr + 6*i, Qbi_fbk_v[i].data(), 6*sizeof(double));
+
+
// Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
FactorBasePtrList fac_lst;
@@ -507,6 +514,7 @@ 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, "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");