From d2170b01fa8f5593d3ad4fbc52dca3ecfdaddff7 Mon Sep 17 00:00:00 2001 From: jvallve <jvallve@iri.upc.edu> Date: Thu, 30 Jun 2022 14:23:37 +0200 Subject: [PATCH] [skip ci] adapting to new yaml server --- include/core/processor/processor_base.h | 2 +- include/core/sensor/sensor_base.h | 2 +- include/core/sensor/sensor_odom.h | 25 +++++++++---- include/core/tree_manager/tree_manager_base.h | 2 +- schema/Prior.schema | 36 +++++++++++++++++++ schema/problem/Problem.schema | 0 schema/processor/ProcessorBase.schema | 5 +++ schema/sensor/SensorBase.schema | 5 +++ schema/sensor/SensorOdom2d.schema | 27 ++++++++++++++ schema/sensor/SensorOdom3d.schema | 32 +++++++++++++++++ schema/solver/SolverManager.schema | 0 schema/tree_manager/TreeManagerBase.schema | 0 src/problem/problem.cpp | 2 +- src/sensor/sensor_odom.cpp | 20 +++++++++-- test/gtest_sensor_base.cpp | 2 +- test/gtest_tree_manager.cpp | 4 +-- 16 files changed, 149 insertions(+), 15 deletions(-) create mode 100644 schema/Prior.schema create mode 100644 schema/problem/Problem.schema create mode 100644 schema/processor/ProcessorBase.schema create mode 100644 schema/sensor/SensorBase.schema create mode 100644 schema/sensor/SensorOdom2d.schema create mode 100644 schema/sensor/SensorOdom3d.schema create mode 100644 schema/solver/SolverManager.schema create mode 100644 schema/tree_manager/TreeManagerBase.schema diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 4bced936e..124744ce3 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -71,7 +71,7 @@ static ProcessorBasePtr create(const std::string& _yaml_filepath, { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ \ - if (not server.validate(#ProcessorClass)) \ + if (not server.applySchema(#ProcessorClass)) \ { \ WOLF_ERROR(server.getLog().str()); \ return nullptr; \ diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index f1322b58b..5910fe3c4 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -75,7 +75,7 @@ static SensorBasePtr create(SizeEigen _dim, { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ \ - if (not server.validate(#SensorClass)) \ + if (not server.applySchema(#SensorClass)) \ { \ WOLF_ERROR(server.getLog().str()); \ return nullptr; \ diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h index 94dea3c13..b73c95ce1 100644 --- a/include/core/sensor/sensor_odom.h +++ b/include/core/sensor/sensor_odom.h @@ -67,13 +67,10 @@ class SensorOdom : public SensorBase SizeEigen dim_; ParamsSensorOdomPtr params_odom_; - public: - SensorOdom(const SizeEigen& _dim, - ParamsSensorOdomPtr _params, - const Priors& _priors); - - WOLF_SENSOR_CREATE(SensorOdom, ParamsSensorOdom); + // protected constructor to avoid creation of SensorOdom without dimension + SensorOdom(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors); + public: ~SensorOdom() override = default; double getDispVarToDispNoiseFactor() const; @@ -86,6 +83,22 @@ class SensorOdom : public SensorBase }; +WOLF_PTR_TYPEDEFS(SensorOdom2d); +class SensorOdom2d : public SensorOdom +{ + public: + SensorOdom2d(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors); + WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom); +}; + +WOLF_PTR_TYPEDEFS(SensorOdom3d); +class SensorOdom3d : public SensorOdom +{ + public: + SensorOdom3d(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors); + WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom); +}; + inline double SensorOdom::getDispVarToDispNoiseFactor() const { return params_odom_->k_disp_to_disp; diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h index d979849a0..508f48438 100644 --- a/include/core/tree_manager/tree_manager_base.h +++ b/include/core/tree_manager/tree_manager_base.h @@ -55,7 +55,7 @@ static TreeManagerBasePtr create(const std::string& _yaml_filepath, { \ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \ \ - if (not server.validate(#TreeManagerClass)) \ + if (not server.applySchema(#TreeManagerClass)) \ { \ WOLF_ERROR(server.getLog().str()); \ return nullptr; \ diff --git a/schema/Prior.schema b/schema/Prior.schema new file mode 100644 index 000000000..8cf11e634 --- /dev/null +++ b/schema/Prior.schema @@ -0,0 +1,36 @@ +type: + type: string + yaml_type: scalar + mandatory: true + doc: The derived type of the StateBlock +state: + type: VectorXd + yaml_type: scalar + mandatory: true + doc: A vector containing the state values +mode: + type: string + yaml_type: scalar + mandatory: true + options: + - "fix" + - "factor" + - "initial_guess" + doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values +dynamic: + type: bool + yaml_type: scalar + mandatory: true + doc: If the state is dynamic, i.e. it changes along time. +noise_std: + type: VectorXd + yaml_type: scalar + mandatory: false + default: [] + doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. +drift_std: + type: VectorXd + yaml_type: scalar + mandatory: false + default: [] + doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema new file mode 100644 index 000000000..e69de29bb diff --git a/schema/processor/ProcessorBase.schema b/schema/processor/ProcessorBase.schema new file mode 100644 index 000000000..f06ab44a7 --- /dev/null +++ b/schema/processor/ProcessorBase.schema @@ -0,0 +1,5 @@ +name: + mandatory: true + type: string + yaml_type: scalar + doc: The sensor's name. It has to be unique. \ No newline at end of file diff --git a/schema/sensor/SensorBase.schema b/schema/sensor/SensorBase.schema new file mode 100644 index 000000000..f06ab44a7 --- /dev/null +++ b/schema/sensor/SensorBase.schema @@ -0,0 +1,5 @@ +name: + mandatory: true + type: string + yaml_type: scalar + doc: The sensor's name. It has to be unique. \ No newline at end of file diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema new file mode 100644 index 000000000..bb88b9ec5 --- /dev/null +++ b/schema/sensor/SensorOdom2d.schema @@ -0,0 +1,27 @@ +follow: SensorBase.schema +k_disp_to_disp: + mandatory: true + type: double + yaml_type: scalar + doc: ratio of displacement variance to displacement, for odometry noise calculation. +k_disp_to_rot: + mandatory: true + type: double + yaml_type: scalar + doc: ratio of displacement variance to rotation, for odometry noise calculation. +k_rot_to_rot: + mandatory: true + type: double + yaml_type: scalar + doc: ratio of rotation variance to rotation, for odometry noise calculation. +min_disp_var: + mandatory: true + type: double + yaml_type: scalar + doc: minimum displacement variance, for odometry noise calculation. +min_rot_var: + mandatory: true + type: double + yaml_type: scalar + doc: minimum rotation variance, for odometry noise calculation. + diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema new file mode 100644 index 000000000..1c1f877db --- /dev/null +++ b/schema/sensor/SensorOdom3d.schema @@ -0,0 +1,32 @@ +follow: SensorBase.schema +k_disp_to_disp: + mandatory: true + type: double + yaml_type: scalar + doc: ratio of displacement variance to displacement, for odometry noise calculation. +k_disp_to_rot: + mandatory: true + type: double + yaml_type: scalar + doc: ratio of displacement variance to rotation, for odometry noise calculation. +k_rot_to_rot: + mandatory: true + type: double + yaml_type: scalar + doc: ratio of rotation variance to rotation, for odometry noise calculation. +min_disp_var: + mandatory: true + type: double + yaml_type: scalar + doc: minimum displacement variance, for odometry noise calculation. +min_rot_var: + mandatory: true + type: double + yaml_type: scalar + doc: minimum rotation variance, for odometry noise calculation. + +states: + P: + follow: Prior.schema + O: + follow: Prior.schema \ No newline at end of file diff --git a/schema/solver/SolverManager.schema b/schema/solver/SolverManager.schema new file mode 100644 index 000000000..e69de29bb diff --git a/schema/tree_manager/TreeManagerBase.schema b/schema/tree_manager/TreeManagerBase.schema new file mode 100644 index 000000000..e69de29bb diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index d839895ba..98a12bb81 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -123,7 +123,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve auto server = yaml_schema_cpp::YamlServer(schema_folders, _input_yaml_file); // Validate agains schema files - if (not server.validate("Problem.schema")) + if (not server.applySchema("Problem.schema")) { WOLF_ERROR(server.getLog().str()); return nullptr; diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp index e216ba19d..1b18d951b 100644 --- a/src/sensor/sensor_odom.cpp +++ b/src/sensor/sensor_odom.cpp @@ -33,7 +33,22 @@ SensorOdom::SensorOdom(const SizeEigen& _dim, dim_(_dim), params_odom_(_params) { - assert(dim_ == 2 or dim_ == 3); +} + +SensorOdom2d::SensorOdom2d(const SizeEigen& _dim, + ParamsSensorOdomPtr _params, + const Priors& _priors) : + SensorOdom(_dim,_params,_priors) +{ + assert(_dim == 2); +} + +SensorOdom3d::SensorOdom3d(const SizeEigen& _dim, + ParamsSensorOdomPtr _params, + const Priors& _priors) : + SensorOdom(_dim,_params,_priors) +{ + assert(_dim == 3); } Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const @@ -86,5 +101,6 @@ Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const // Register in the FactorySensor #include "core/sensor/factory_sensor.h" namespace wolf { -WOLF_REGISTER_SENSOR(SensorOdom); +WOLF_REGISTER_SENSOR(SensorOdom2d); +WOLF_REGISTER_SENSOR(SensorOdom3d); } // namespace wolf diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 46dee21fc..3eb4b8d4f 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -416,7 +416,7 @@ TEST(SensorBase, factory) WOLF_INFO("Creating sensor from ", name, ".yaml"); - ASSERT_EQ(server.validate("SensorDummy"), not wrong); + ASSERT_EQ(server.applySchema("SensorDummy"), not wrong); // CORRECT YAML if (not wrong) diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index c77181e03..0c3f031eb 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -66,7 +66,7 @@ TEST(TreeManager, createNode) auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root + "/schemas"}, wolf_root + "/test/yaml/params_tree_manager1.yaml"); - ASSERT_TRUE(yaml_server.validate("TreeManagerDummy")); + ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy")); auto GM = TreeManagerDummy::create(yaml_server.getNode()); @@ -114,7 +114,7 @@ TEST(TreeManager, FactoryParam) auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root + "/schemas"}, wolf_root + "/test/yaml/params_tree_manager1.yaml"); - ASSERT_TRUE(yaml_server.validate("TreeManagerDummy")); + ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy")); auto GM = FactoryTreeManager::create("TreeManagerDummy", yaml_server.getNode()); -- GitLab