Newest args

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]

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");
+
 }