diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 4bced936e7fcdfd70e508b6fdb7476b7f10a1bf6..124744ce38b67ca53be5654aa34b1b8cb9fa593f 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 f1322b58b96c074c2e6c9032002b1dd947f77df5..5910fe3c4d4dad444f29cad67f1da641af643518 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 94dea3c13079eeeef8c407bc24b867fcf9ddfe3d..b73c95ce190892d8430195e1d7e31e6b02e555da 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 d979849a05cad59641f4f7550eb16138168685ff..508f48438cb50229c919d74a5f4fcd1f29f811fc 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 0000000000000000000000000000000000000000..8cf11e6341a115101bc245bd5f2b713313160a67 --- /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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/schema/processor/ProcessorBase.schema b/schema/processor/ProcessorBase.schema new file mode 100644 index 0000000000000000000000000000000000000000..f06ab44a7ab230fa3a8a2a249223a3ab15e28647 --- /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 0000000000000000000000000000000000000000..f06ab44a7ab230fa3a8a2a249223a3ab15e28647 --- /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 0000000000000000000000000000000000000000..bb88b9ec536df2ebd6a52106a9ad232b1f4ff0f6 --- /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 0000000000000000000000000000000000000000..1c1f877db4590c5e0f700627f65e518d7f6f2baf --- /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 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/schema/tree_manager/TreeManagerBase.schema b/schema/tree_manager/TreeManagerBase.schema new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index d839895ba7bc3957e11b85e4e5f3753660845e5f..98a12bb81f48e26171cf7117407a788671a0ff61 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 e216ba19d9ff4509d0b5b7b4a751e18242317741..1b18d951b9307ce10d9918d07856ac76f1056e29 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 46dee21fc84b18e9f778cdbd00354223eb1c1682..3eb4b8d4f34b3d88805c1ade5992bebc74907c28 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 c77181e037bda6da8c7982ad60ea7f49acf6b679..0c3f031eb264cc8772dd2f99b9834a466e98066e 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());