Fixed mcapi core changes

demos/mcapi_povcdl_estimation.cpp

@@ -95,7 +95,7 @@ int main (int argc, char **argv) {
     double std_vbc_sim = config["std_vbc_sim"].as<double>();
     double std_f_sim = config["std_f_sim"].as<double>();
     double std_tau_sim = config["std_tau_sim"].as<double>();
-    double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
+    // double std_odom3d_sim = config["std_odom3d_sim"].as<double>();
     
     // priors
     double std_prior_p = config["std_prior_p"].as<double>();
@@ -448,7 +448,7 @@ int main (int argc, char **argv) {
     P_origin.block<3,3>(0,0)   = pow(std_prior_p, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(3,3)   = pow(std_prior_o, 2) * Matrix3d::Identity();
     P_origin.block<3,3>(6,6)   = pow(std_prior_v, 2) * Matrix3d::Identity();
-    FrameBasePtr KF1 = problem->emplaceKeyFrame(KEY, t0, x_origin);
+    FrameBasePtr KF1 = problem->emplaceKeyFrame(t0, x_origin);
     // Prior pose factor
     CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
     pose_prior_capture->emplaceFeatureAndFactor();

demos/mcapi_povcdl_estimation.yaml

@@ -45,13 +45,13 @@ bias_pbc_prior: [0,0,0]
 
 
 # model disturbance
-scale_dist: 0.01  # disturbance of link inertia levers 
+scale_dist: 0.00  # disturbance of link inertia levers 
 base_dist_only: True
 
 # which measurement/parameter is disturbed?
 vbc_is_dist: False
-Iw_is_dist: True
-Lgest_is_dist: True
+Iw_is_dist: False
+Lgest_is_dist: False
 
 
 # Robot model