diff --git a/src/yaml/processor_odom_3D_yaml.cpp b/src/yaml/processor_odom_3D_yaml.cpp index 211bd865243dc7126dc062b392583bfd95a8066e..c8e597c01d7f69d9f06c3bbd62f6ba85dfb5fbb6 100644 --- a/src/yaml/processor_odom_3D_yaml.cpp +++ b/src/yaml/processor_odom_3D_yaml.cpp @@ -26,7 +26,6 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f if (config["type"].as<std::string>() == "ODOM 3D") { - ProcessorParamsOdom3DPtr params = std::make_shared<ProcessorParamsOdom3D>(); params->time_tolerance = config["time_tolerance"] .as<Scalar>(); @@ -34,6 +33,7 @@ static ProcessorParamsBasePtr createProcessorOdom3DParams(const std::string & _f params->max_buff_length = config["max_buff_length"] .as<SizeEigen >(); params->dist_traveled = config["dist_traveled"] .as<Scalar>(); params->angle_turned = config["angle_turned"] .as<Scalar>(); + params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<Scalar>(); return params; } diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp index 803963c71565d23c45619c414aa93881a7bcd737..51ceb2f6344d440fec93bf3417654a9f47bf3333 100644 --- a/src/yaml/sensor_odom_3D_yaml.cpp +++ b/src/yaml/sensor_odom_3D_yaml.cpp @@ -26,8 +26,6 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do if (config["type"].as<std::string>() == "ODOM 3D") { -// YAML::Node variances = config["motion_variances"]; - IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>(); params->k_disp_to_disp = config["k_disp_to_disp"] .as<Scalar>(); diff --git a/test/yaml/processor_odom_3D.yaml b/test/yaml/processor_odom_3D.yaml index cdaf5367400bba2c140de09016d024ad29f3d61f..6d8c58b1ae502ab29bbc6cf07cee5a3b1822d6a3 100644 --- a/test/yaml/processor_odom_3D.yaml +++ b/test/yaml/processor_odom_3D.yaml @@ -5,4 +5,5 @@ time_tolerance: 0.01 # seconds max_time_span: 0.2 # seconds max_buff_length: 10 # motion deltas dist_traveled: 0.5 # meters -angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) \ No newline at end of file +angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) +unmeasured_perturbation_std: 0.001 \ No newline at end of file