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) { ...@@ -95,7 +95,7 @@ int main (int argc, char **argv) {
double std_vbc_sim = config["std_vbc_sim"].as<double>(); double std_vbc_sim = config["std_vbc_sim"].as<double>();
double std_f_sim = config["std_f_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_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 // priors
double std_prior_p = config["std_prior_p"].as<double>(); double std_prior_p = config["std_prior_p"].as<double>();
...@@ -448,7 +448,7 @@ int main (int argc, char **argv) { ...@@ -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>(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>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity();
P_origin.block<3,3>(6,6) = pow(std_prior_v, 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 // Prior pose factor
CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6)); CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6));
pose_prior_capture->emplaceFeatureAndFactor(); pose_prior_capture->emplaceFeatureAndFactor();
......
...@@ -45,13 +45,13 @@ bias_pbc_prior: [0,0,0] ...@@ -45,13 +45,13 @@ bias_pbc_prior: [0,0,0]
# model disturbance # model disturbance
scale_dist: 0.01 # disturbance of link inertia levers scale_dist: 0.00 # disturbance of link inertia levers
base_dist_only: True base_dist_only: True
# which measurement/parameter is disturbed? # which measurement/parameter is disturbed?
vbc_is_dist: False vbc_is_dist: False
Iw_is_dist: True Iw_is_dist: False
Lgest_is_dist: True Lgest_is_dist: False
# Robot model # 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