From 18fdbe1fafd2ee0622e92397591bd30bbf8a5f79 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 4 Jul 2022 15:40:11 +0200
Subject: [PATCH] [skip ci] adding schemas

---
 include/core/solver/solver_manager.h       |  4 +-
 schema/PriorO2d.schema                     | 38 ++++++++++++++++
 schema/PriorP2d.schema                     | 38 ++++++++++++++++
 schema/Setup.schema                        | 14 ++++++
 schema/problem/Problem.schema              | 17 ++++++++
 schema/processor/ProcessorBase.schema      |  2 +-
 schema/solver/SolverCeres.schema           | 51 ++++++++++++++++++++++
 schema/solver/SolverManager.schema         | 28 ++++++++++++
 schema/tree_manager/TreeManagerBase.schema |  5 +++
 test/CMakeLists.txt                        |  2 +-
 test/dummy/SensorDummy.schema              | 16 +++++++
 test/gtest_capture_base.cpp                |  4 +-
 test/gtest_sensor_base.cpp                 | 40 +++++++++++------
 test/yaml/sensor_odom_2d.yaml              |  1 +
 14 files changed, 240 insertions(+), 20 deletions(-)
 create mode 100644 schema/PriorO2d.schema
 create mode 100644 schema/PriorP2d.schema
 create mode 100644 schema/Setup.schema
 create mode 100644 schema/solver/SolverCeres.schema
 create mode 100644 test/dummy/SensorDummy.schema

diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index ccfcb62b6..1e621b8f6 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -82,8 +82,8 @@ class SolverManager
         enum class ReportVerbosity : std::size_t
         {
             QUIET = 0,
-            BRIEF,
-            FULL
+            BRIEF = 1,
+            FULL = 2
         };
 
         // PROFILING
diff --git a/schema/PriorO2d.schema b/schema/PriorO2d.schema
new file mode 100644
index 000000000..08173f33b
--- /dev/null
+++ b/schema/PriorO2d.schema
@@ -0,0 +1,38 @@
+type:
+  type: string
+  yaml_type: scalar
+  mandatory: false
+  default: StateAngle
+  options: [StateAngle]
+  doc: The derived type of the StateBlock
+state:
+  type: Vector1d
+  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/PriorP2d.schema b/schema/PriorP2d.schema
new file mode 100644
index 000000000..3e17abd5f
--- /dev/null
+++ b/schema/PriorP2d.schema
@@ -0,0 +1,38 @@
+type:
+  type: string
+  yaml_type: scalar
+  mandatory: false
+  default: StatePoint2d
+  options: [StatePoint2d]
+  doc: The derived type of the StateBlock
+state:
+  type: Vector2d
+  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/Setup.schema b/schema/Setup.schema
new file mode 100644
index 000000000..d5c2e7d71
--- /dev/null
+++ b/schema/Setup.schema
@@ -0,0 +1,14 @@
+problem:
+  follow: Poblem.schema
+map:
+  follow: Map.schema
+solver:
+  follow: Solver.schema
+sensors:
+  yaml_type: sequence
+  mandatory: true
+  doc: A sequence of all the sensors.
+processors:
+  yaml_type: sequence
+  mandatory: true
+  doc: A sequence of all the processors.
diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema
index e69de29bb..76ed61a80 100644
--- a/schema/problem/Problem.schema
+++ b/schema/problem/Problem.schema
@@ -0,0 +1,17 @@
+tree_manager:
+  yaml_type: scalar
+  type: string
+  mandatory: true
+  doc: Tree manager parameters
+frame_structure:
+  yaml_type: scalar
+  type: string
+  mandatory: true
+  doc: The keys of the default frames in problems
+dimension:
+  yaml_type: scalar
+  type: int
+  mandatory: true
+  options: [2, 3]
+  doc: Dimension of the problem: '2' for 2D or '3' for 3D
+#prior:
\ No newline at end of file
diff --git a/schema/processor/ProcessorBase.schema b/schema/processor/ProcessorBase.schema
index f06ab44a7..63d4b609b 100644
--- a/schema/processor/ProcessorBase.schema
+++ b/schema/processor/ProcessorBase.schema
@@ -2,4 +2,4 @@ name:
   mandatory: true
   type: string
   yaml_type: scalar
-  doc: The sensor's name. It has to be unique.
\ No newline at end of file
+  doc: The processor's name. It has to be unique.
\ No newline at end of file
diff --git a/schema/solver/SolverCeres.schema b/schema/solver/SolverCeres.schema
new file mode 100644
index 000000000..81663b678
--- /dev/null
+++ b/schema/solver/SolverCeres.schema
@@ -0,0 +1,51 @@
+follow: SolverManager.schema
+minimizer:
+  mandatory: true
+  type: string
+  options: [LEVENBERG_MARQUARDT, levenberg_marquardt, DOGLEG, dogleg, LBFGS, lbfgs, BFGS, bfgs]
+  yaml_type: scalar
+  doc: Type of minimizer.
+interrupt_on_problem_change:
+  mandatory: true
+  type: bool
+  yaml_type: scalar
+  doc: If the solver has to interrupted each time the problem changes to rebuild the problem.
+min_num_iterations:
+  mandatory: false
+  default: 0
+  type: unsigned int
+  yaml_type: scalar
+  doc: Amount of solver iterations during which the solver cannot be interrupted (used in interrupt_on_problem_change == true).
+max_num_iterations:
+  mandatory: true
+  type: unsigned int
+  yaml_type: scalar
+  doc: Maximum amount of solver iterations. If the solver didn't converge after this amount of iterations, it stops anyway. 
+function_tolerance:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: Function tolerance. Convergence criterion. Typical value: 1e-8
+gradient_tolerance:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: gradient tolerance. Convergence criterion. Typical value: 1e-8
+num_threads:
+  mandatory: true
+  type: unsigned int
+  options: [1, 2, 3, 4]
+  yaml_type: scalar
+  doc: threads used by ceres.
+use_nonmonotonic_steps:
+  mandatory: false
+  default: false
+  type: bool
+  yaml_type: scalar
+  doc: If the solver is allowed to update the solution with non-monotonic steps. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers.
+max_consecutive_nonmonotonic_steps:
+  mandatory: false
+  default: 0
+  type: unsigned int
+  yaml_type: scalar
+  doc: Amount of consecutive non-monotonic steps allowed. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers.
\ No newline at end of file
diff --git a/schema/solver/SolverManager.schema b/schema/solver/SolverManager.schema
index e69de29bb..5386ee1ae 100644
--- a/schema/solver/SolverManager.schema
+++ b/schema/solver/SolverManager.schema
@@ -0,0 +1,28 @@
+period:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: Period of the solver thread.
+verbose:
+  mandatory: true
+  type: int
+  yaml_type: scalar
+  options: [0, 1, 2]
+  doc: Verbosity of the solver. 0: Nothing, 1: Brief report, 2: Full report.
+compute_cov:
+  mandatory: true
+  type: bool
+  yaml_type: scalar
+  doc: If the solver has to compute any covariance matrix block.
+cov_enum:
+  mandatory: false
+  default: 0
+  type: int
+  options: [0, 1, 2, 3, 4, 5]
+  yaml_type: scalar
+  doc: Which covariance matrix blocks have to be computed. 0: All blocks and all cross-covariances. 1: All marginals. 2: Marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks. 3: Last frame P and V. 4: Last frame P, O, V and T. 5: Last frame P and T.
+cov_period:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: Period of the covariance computation.
\ No newline at end of file
diff --git a/schema/tree_manager/TreeManagerBase.schema b/schema/tree_manager/TreeManagerBase.schema
index e69de29bb..79e6c6e4a 100644
--- a/schema/tree_manager/TreeManagerBase.schema
+++ b/schema/tree_manager/TreeManagerBase.schema
@@ -0,0 +1,5 @@
+type:
+  mandatory: true
+  type: string
+  yaml_type: scalar
+  doc: Type of the TreeManager. To keep all frames, use "none".
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 82337923b..34537dbc5 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -112,7 +112,7 @@ wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
 wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
 
 # SensorBase test
-# wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
+wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
 
 # shared_from_this test
 wolf_add_gtest(gtest_shared_from_this gtest_shared_from_this.cpp)
diff --git a/test/dummy/SensorDummy.schema b/test/dummy/SensorDummy.schema
new file mode 100644
index 000000000..0175bd14b
--- /dev/null
+++ b/test/dummy/SensorDummy.schema
@@ -0,0 +1,16 @@
+follow: SensorBase.schema
+states:
+  P:
+    follow: PriorP2d.schema
+  O:
+    follow: PriorO2d.schema
+noise_p_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in p.
+noise_o_std:
+  mandatory: true
+  type: double
+  yaml_type: scalar
+  doc: noise std in p.
\ No newline at end of file
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index e8ba8dcd0..1e72d21ae 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -43,7 +43,7 @@ TEST(CaptureBase, ConstructorNoSensor)
 
 TEST(CaptureBase, ConstructorWithSensor)
 {
-    SensorBasePtr S = FactorySensorYaml::create("SensorOdom", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+    SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
     CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
     ASSERT_EQ(C->getTimeStamp(), 1.5);
     ASSERT_FALSE(C->getFrame());
@@ -53,7 +53,7 @@ TEST(CaptureBase, ConstructorWithSensor)
 
 TEST(CaptureBase, Static_sensor_params)
 {
-    SensorBasePtr S = FactorySensorYaml::create("SensorOdom", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+    SensorBasePtr S = FactorySensorYaml::create("SensorOdom2d", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
     CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
 
     // query capture blocks
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 3eb4b8d4f..2154adcb7 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -100,10 +100,11 @@ void checkSensor(SensorBasePtr S,
 TEST(SensorBase, makeshared_priors_POfix2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
+  auto S = std::make_shared<SensorDummy>(2, params, 
                                          Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
                                                  {'O',Prior("O", o_state_2D)}}));
 
@@ -119,10 +120,11 @@ TEST(SensorBase, makeshared_priors_POfix2D)
 TEST(SensorBase, makeshared_priors_POfix3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
+  auto S = std::make_shared<SensorDummy>(3, params, 
                                          Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
                                                  {'O',Prior("O", o_state_3D)}}));
 
@@ -141,10 +143,11 @@ TEST(SensorBase, makeshared_priors_POfix3D)
 TEST(SensorBase, makeshared_priors_POinitial_guess2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
+  auto S = std::make_shared<SensorDummy>(2, params, 
                                          Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
                                                  {'O',Prior("O", o_state_2D, "initial_guess")}}));
 
@@ -163,10 +166,11 @@ TEST(SensorBase, makeshared_priors_POinitial_guess2D)
 TEST(SensorBase, makeshared_priors_POinitial_guess3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
+  auto S = std::make_shared<SensorDummy>(3, params, 
                                          Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
                                                  {'O',Prior("O", o_state_3D, "initial_guess")}}));
 
@@ -185,10 +189,11 @@ TEST(SensorBase, makeshared_priors_POinitial_guess3D)
 TEST(SensorBase, makeshared_priors_POfactor2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
+  auto S = std::make_shared<SensorDummy>(2, params, 
                                          Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
                                                  {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
 
@@ -207,10 +212,11 @@ TEST(SensorBase, makeshared_priors_POfactor2D)
 TEST(SensorBase, makeshared_priors_POfactor3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
+  auto S = std::make_shared<SensorDummy>(3, params, 
                                          Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
                                                  {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
 
@@ -229,10 +235,11 @@ TEST(SensorBase, makeshared_priors_POfactor3D)
 TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
+  auto S = std::make_shared<SensorDummy>(2, params, 
                                          Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
                                                  {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
 
@@ -251,10 +258,11 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D)
 TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
+  auto S = std::make_shared<SensorDummy>(3, params, 
                                          Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
                                                  {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
 
@@ -273,10 +281,11 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D)
 TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 2, params, 
+  auto S = std::make_shared<SensorDummy>(2, params, 
                                          Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
                                                  {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
 
@@ -295,10 +304,11 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic2D_drift)
 TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
 {
   auto params = std::make_shared<ParamsSensorDummy>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
-  auto S = std::make_shared<SensorDummy>("sensor1", 3, params, 
+  auto S = std::make_shared<SensorDummy>(3, params, 
                                          Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
                                                  {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
 
@@ -317,12 +327,13 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift)
 TEST(SensorBase, makeshared_priors_POIA_mixed)
 {
   auto params = std::make_shared<ParamsSensorDummyPoia>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   VectorXd i_state_3D = VectorXd::Random(5);
 
-  auto S = std::make_shared<SensorDummyPoia>("sensor1", 3, params, 
+  auto S = std::make_shared<SensorDummyPoia>(3, params, 
                                              Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
                                                      {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
                                                      {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
@@ -345,13 +356,14 @@ TEST(SensorBase, makeshared_priors_POIA_mixed)
 TEST(SensorBase, makeshared_priors_POIA_wrong)
 {
   auto params = std::make_shared<ParamsSensorDummyPoia>();
+  params->name = "sensor_1";
   params->noise_p_std = noise_p_std;
   params->noise_o_std = noise_o_std;
   
   VectorXd i_state_3D = VectorXd::Random(5);
 
   // missing I
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>("sensor1", 3, params, 
+  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, params, 
                                                  Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
                                                          {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
                                                          //{'I',Prior("StateBlock", i_state_3D, "initial_guess")},
@@ -359,7 +371,7 @@ TEST(SensorBase, makeshared_priors_POIA_wrong)
                std::runtime_error);
 
   // missing A
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>("sensor1", 3, params, 
+  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, params, 
                                                  Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
                                                          {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
                                                          {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
@@ -367,7 +379,7 @@ TEST(SensorBase, makeshared_priors_POIA_wrong)
                std::runtime_error);
 
   // wrong A type (expected StateQuaternion)
-  ASSERT_THROW(std::make_shared<SensorDummyPoia>("sensor1", 3, params, 
+  ASSERT_THROW(std::make_shared<SensorDummyPoia>(3, params, 
                                                  Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
                                                          {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
                                                          {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
index 0ab13d73f..278374ccf 100644
--- a/test/yaml/sensor_odom_2d.yaml
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -1,3 +1,4 @@
+name: some_sensor_odom_2d
 states:
   P:
     mode: fix
-- 
GitLab