Commit 7adde78f authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Fixed mcapi core changes

parent bede62de
......@@ -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();
......
......@@ -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
......
Supports Markdown
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