diff --git a/demos/solo_real_estimation.yaml b/demos/solo_real_estimation.yaml
index 62258179f3d786b1093ca314d2e9895bf765b7c3..c00ef31936d3db340ceefa227cf9d159f4c6e0d5 100644
--- a/demos/solo_real_estimation.yaml
+++ b/demos/solo_real_estimation.yaml
@@ -43,6 +43,8 @@ std_pose_o_deg: 1
std_mocap_extr_o_deg: 0.1
std_mocap_extr_p: 10
unfix_extr_sensor_pose: true
+time_shift_mocap: 0
+time_tolerance_mocap: 0.001
bias_imu_prior: [0,0,0, 0,0,0]
# bias_imu_prior: [0,0,-0.011,-0.000079251, 0.00439540765, -0.0002120267]
diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp
index f52ae06bb3f9ca1379b6171f42485e81c53f6b69..83cc8497bad7a63d861d97dd5540f17bb0a073d6 100644
--- a/demos/solo_real_pov_estimation.cpp
+++ b/demos/solo_real_pov_estimation.cpp
@@ -26,6 +26,7 @@
#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>
@@ -39,6 +40,7 @@
#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"
@@ -115,9 +117,10 @@ int main (int argc, char **argv) {
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>();
@@ -255,7 +258,7 @@ int main (int argc, char **argv) {
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;
+ 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();
@@ -308,11 +311,12 @@ int main (int argc, char **argv) {
std::vector<Vector3d> p_ob_fbk_v;
std::vector<Vector4d> q_ob_fbk_v;
std::vector<Vector3d> v_ob_fbk_v;
- double o_p_ob_fbk_carr[3*N];
- double o_q_b_fbk_carr[4*N];
- double o_v_ob_fbk_carr[3*N];
- double imu_bias_fbk_carr[6*N];
- double extr_mocap_fbk_carr[7*N];
+ // 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;
//////////////////////////////////////////
@@ -453,7 +457,7 @@ int main (int argc, char **argv) {
// 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());
+ CapturePosePtr CP = std::make_shared<CapturePose>(ts+time_shift_mocap, sensor_pose, pose_meas, sensor_pose->getNoiseCov());
CP->process();
// ts_since_last_kf += dt;
@@ -515,11 +519,11 @@ int main (int argc, char **argv) {
problem->print(4,true,true,true);
std::cout << report << std::endl;
}
-
- double o_p_ob_carr[3*N];
- double o_q_b_carr[4*N];
- double o_v_ob_carr[3*N];
- double imu_bias_carr[6*N];
+
+ 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];
@@ -547,6 +551,97 @@ int main (int argc, char **argv) {
}
+ // 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
@@ -554,13 +649,14 @@ int main (int argc, char **argv) {
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");
+ 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");
@@ -570,4 +666,14 @@ int main (int argc, char **argv) {
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");
+
}