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