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