diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 54bbb3aec498cacc736321f77abc127a24414106..be22913c2594f8c912efd7e69d4ddbb529a5f222 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -120,9 +120,7 @@ ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _uniqu const SensorBasePtr _sensor_ptr) { SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr); - ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(); - params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false"); - params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.01"); + ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server); // construct processor ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 161818df65a56f5b2df02d9a66327a71e80a63a0..2c33482365adf14e1b5ec468cc8cc4ca51c65154 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -26,8 +26,8 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase IntrinsicsRangeBearing(std::string _unique_name, const ParamsServer& _server): IntrinsicsBase(_unique_name, _server) { - noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise range metres std", "0.05"); - noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise bearing degrees std", "0.5"); + noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std"); + noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std"); } std::string print() { diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 00722d20b22fba998db5c0d85fe33391be9c8b94..744e03fe6cb4b47fa8f8d17723ded75f2f78921c 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -179,9 +179,9 @@ struct ProcessorParamsBase : public ParamsBase ProcessorParamsBase(std::string _unique_name, const ParamsServer& _server): ParamsBase(_unique_name, _server) { - voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false"); - voting_aux_active = _server.getParam<bool>(_unique_name + "/voting_aux_active", "false"); - time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0"); + voting_active = _server.getParam<bool>(_unique_name + "/voting_active"); + voting_aux_active = _server.getParam<bool>(_unique_name + "/voting_aux_active"); + time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance"); } virtual ~ProcessorParamsBase() = default; diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index 831fec13696365facbee70749bc197d5d7c3f3cd..36b6d5aa0885d9815135ab6221f7b0f029395602 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -23,7 +23,7 @@ struct ProcessorParamsDiffDrive : public ProcessorParamsMotion ProcessorParamsDiffDrive(std::string _unique_name, const ParamsServer& _server): ProcessorParamsMotion(_unique_name, _server) { - unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001"); + unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std"); } std::string print() { diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 8f68501f60feb22588032b60291b0a3ffcbfab2a..a5ff4c53382b3d6a1b6ace1fce41a28dbbc9bfab 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -33,15 +33,16 @@ struct ProcessorParamsMotion : public ProcessorParamsBase ProcessorParamsMotion(std::string _unique_name, const ParamsServer& _server): ProcessorParamsBase(_unique_name, _server) { - max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span", "0.5"); - max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length", "10"); - dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled", "5"); - angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5"); - unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4"); + max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span"); + max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length"); + dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled"); + angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned"); + unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std"); } std::string print() { - return "\n" + ProcessorParamsBase::print() + "max_time_span: " + std::to_string(max_time_span) + "\n" + return "\n" + ProcessorParamsBase::print() + + "max_time_span: " + std::to_string(max_time_span) + "\n" + "max_buff_length: " + std::to_string(max_buff_length) + "\n" + "dist_traveled: " + std::to_string(dist_traveled) + "\n" + "angle_turned: " +std::to_string(angle_turned) + "\n" diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h index 7c7c1cd0a431d4c4c0053598fa9567854cc09742..7b1acb5b2a22d8cdd7f8cc013ce14bfd0bad3321 100644 --- a/include/core/processor/processor_odom_2D.h +++ b/include/core/processor/processor_odom_2D.h @@ -26,11 +26,12 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server) : ProcessorParamsMotion(_unique_name, _server) { - cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0"); + cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det"); } std::string print() { - return "\n" + ProcessorParamsMotion::print() + "cov_det: " + std::to_string(cov_det) + "\n"; + return "\n" + ProcessorParamsMotion::print() + + "cov_det: " + std::to_string(cov_det) + "\n"; } }; diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index 3915ba33f8b9899deb99b6436b66a5b5c9ef9f4f..1c9277989d11fee7e0ea568e8cbccdb1bec58b0e 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -24,8 +24,8 @@ struct ProcessorParamsTracker : public ProcessorParamsBase ProcessorParamsTracker(std::string _unique_name, const wolf::ParamsServer & _server): ProcessorParamsBase(_unique_name, _server) { - min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1"); - max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1"); + min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe"); + max_new_features = _server.getParam<int>(_unique_name + "/max_new_features"); } std::string print() { diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index e6737438050dba3f7bff3b51130937572cc5ef23..2988669c965c55d398938b49ae27167ca3dfd5fa 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -52,13 +52,13 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase else if(model_str.compare("Five_Factor_Model")) model_ = DiffDriveModel::Five_Factor_Model; else throw std::runtime_error("Failed to fetch a valid value for the enumerate DiffDriveModel. Value provided: " + model_str); - factors_ = _server.getParam<Eigen::VectorXs>(_unique_name + "/factors", "[1,1,1]"); + factors_ = _server.getParam<Eigen::VectorXs>(_unique_name + "/factors"); left_resolution_ = _server.getParam<Scalar>(_unique_name + "/left_resolution_"); right_resolution_ = _server.getParam<Scalar>(_unique_name + "/right_resolution_"); - left_gain_ = _server.getParam<Scalar>(_unique_name + "/left_gain", "0.01"); - right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01"); + left_gain_ = _server.getParam<Scalar>(_unique_name + "/left_gain"); + right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain"); } virtual ~IntrinsicsDiffDrive() = default; std::string print() diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 1528c15e708d61f13ed8a5d1084207287722ba6b..b64e86ab9008296a57e01c7ed8a3a8061fa88b2b 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -43,7 +43,7 @@ CeresManager::~CeresManager() SolverManagerPtr CeresManager::create(const ProblemPtr &wolf_problem, const ParamsServer &_server) { ceres::Solver::Options opt; - opt.max_num_iterations = _server.getParam<int>("max_num_iterations","1000"); + opt.max_num_iterations = _server.getParam<int>("max_num_iterations"); // CeresManager m = CeresManager(wolf_problem, opt); return std::make_shared<CeresManager>(wolf_problem, opt); } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index eab55cc02cf4d12045fddf7813c4d7cdf87f35b4..a9044802917df11f6c7e7609b06add76a2c01021 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -77,8 +77,8 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) ProblemPtr Problem::autoSetup(ParamsServer &_server) { // Problem structure and dimension - std::string frame_structure = _server.getParam<std::string> ("problem/frame structure", "PO"); - int dim = _server.getParam<int> ("problem/dimension", "2"); + std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure"); + int dim = _server.getParam<int> ("problem/dimension"); auto problem = Problem::create(frame_structure, dim); // cout << "PRINTING SERVER MAP" << endl; // _server.print(); diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index 48a2747789e943fc76a171c894c5802635bc0093..1d61bd14b4e167bbfcdc259a1c4407c1a2030123 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -195,20 +195,13 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const return prc_ptr; } + ProcessorBasePtr ProcessorOdom2D::createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr) { - ProcessorOdom2DPtr prc_ptr; std::shared_ptr<ProcessorParamsOdom2D> params; - params = std::make_shared<ProcessorParamsOdom2D>(); - params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "true"); - params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.1"); - params->max_time_span = _server.getParam<double>(_unique_name + "/max_time_span", "999"); - params->dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled", "0.95"); // Will make KFs automatically every 1m displacement - params->angle_turned = _server.getParam<double>(_unique_name + "/angle_turned", "999"); - params->cov_det = _server.getParam<double>(_unique_name + "/cov_det", "999"); - params->unmeasured_perturbation_std = _server.getParam<double>(_unique_name + "/unmeasured_perturbation_std", "0.0001"); + params = std::make_shared<ProcessorParamsOdom2D>(_unique_name, _server); prc_ptr = std::make_shared<ProcessorOdom2D>(params); prc_ptr->setName(_unique_name); diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp index fd4427abec8211ddebf95b2172409692f807e72d..c96059e58b2c40358f75e0b520f9166944bd08ee 100644 --- a/src/sensor/sensor_odom_2D.cpp +++ b/src/sensor/sensor_odom_2D.cpp @@ -61,14 +61,15 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen: SensorBasePtr SensorOdom2D::createAutoConf(const std::string& _unique_name, const ParamsServer& _server) { // Eigen::VectorXs _extrinsics_po = Eigen::Vector3s(0,0,0); - Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos", "[0,0,0]"); + Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pose"); + assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); - SensorOdom2DPtr odo; - IntrinsicsOdom2D params; - params.k_disp_to_disp = _server.getParam<double>(_unique_name + "/intrinsic/k_disp_to_disp", "1"); - params.k_rot_to_rot = _server.getParam<double>(_unique_name + "/intrinsic/k_rot_to_rot", "1"); - odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); + + IntrinsicsOdom2DPtr params = std::make_shared<IntrinsicsOdom2D>(_unique_name, _server); + + SensorOdom2DPtr odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); odo->setName(_unique_name); + return odo; } diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h index 217196b7e26542f39d60fbfb1f53f01214e20ff9..d4f6dfbcde8fda17e75806ae9b4353afd27497bd 100644 --- a/test/dummy/processor_tracker_feature_dummy.h +++ b/test/dummy/processor_tracker_feature_dummy.h @@ -25,7 +25,7 @@ struct ProcessorParamsTrackerFeatureDummy : public ProcessorParamsTrackerFeature ProcessorParamsTrackerFeatureDummy(std::string _unique_name, const wolf::ParamsServer & _server): ProcessorParamsTrackerFeature(_unique_name, _server) { - n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost", "1"); + n_tracks_lost = _server.getParam<unsigned int>(_unique_name + "/n_tracks_lost"); } }; diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h index f79b2da401c1f925d7f8629f1e4b9f1cc4c14b3b..9977abc480ab2c1cff695b732286cd7857f5276d 100644 --- a/test/dummy/processor_tracker_landmark_dummy.h +++ b/test/dummy/processor_tracker_landmark_dummy.h @@ -23,7 +23,7 @@ struct ProcessorParamsTrackerLandmarkDummy : public ProcessorParamsTrackerLandma ProcessorParamsTrackerLandmarkDummy(std::string _unique_name, const wolf::ParamsServer & _server): ProcessorParamsTrackerLandmark(_unique_name, _server) { - n_landmarks_lost = _server.getParam<unsigned int>(_unique_name + "/n_landmarks_lost", "1"); + n_landmarks_lost = _server.getParam<unsigned int>(_unique_name + "/n_landmarks_lost"); } };