[skip ci] gtest_problem ok

schema/Prior.schema

@@ -12,10 +12,7 @@ mode:
   type: string
   yaml_type: scalar
   mandatory: true
-  options:
-    - "fix"
-    - "factor"
-    - "initial_guess"
+  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

schema/problem/Problem.schema

 problem:
   tree_manager:
     type: derived
+    base: TreeManagerBase.schema
     yaml_type: scalar
     mandatory: true
     doc: Tree manager parameters
@@ -18,18 +19,19 @@ problem:
   #prior:
 map:
   type: derived
+  base: Map.schema
   yaml_type: scalar
   mandatory: false
   doc: The map used in the wolf problem.
-solver:
-  follow: Solver.schema
 sensors:
   yaml_type: sequence
   type: derived
+  base: SensorSequence.schema
   mandatory: true
   doc: A sequence of all the sensors.
 processors:
   yaml_type: sequence
   type: derived
+  base: ProcessorSequence.schema
   mandatory: true
   doc: A sequence of all the processors.

schema/processor/ProcessorBase.schema

@@ -4,18 +4,6 @@ name:
   yaml_type: scalar
   doc: The processor's name. It has to be unique.
 
-type:
-  mandatory: true
-  type: string
-  yaml_type: scalar
-  doc: The processor's class name.
-
-plugin:
-  mandatory: true
-  type: string
-  yaml_type: scalar
-  doc: The name of the wolf plugin where the processor is implemented.
-
 time_tolerance:
   mandatory: true
   type: double

schema/processor/ProcessorSequence.schema

+follow: ProcessorBase.schema
+
+type:
+  mandatory: true
+  type: string
+  yaml_type: scalar
+  doc: The processor's class name.
+
+plugin:
+  mandatory: true
+  type: string
+  yaml_type: scalar
+  doc: The name of the wolf plugin where the processor is implemented.
\ No newline at end of file

schema/sensor/SensorBase.schema

@@ -2,16 +2,4 @@ name:
   mandatory: true
   type: string
   yaml_type: scalar
-  doc: The sensor's name. It has to be unique.
-
-type:
-  mandatory: true
-  type: string
-  yaml_type: scalar
-  doc: The sensor's class name.
-  
-plugin:
-  mandatory: true
-  type: string
-  yaml_type: scalar
-  doc: The name of the wolf plugin where the sensor is implemented.
\ No newline at end of file
+  doc: The sensor's name. It has to be unique.
\ No newline at end of file

schema/sensor/SensorSequence.schema

+follow: SensorBase.schema
+
+type:
+  mandatory: true
+  type: string
+  yaml_type: scalar
+  doc: The sensor's class name.
+  
+plugin:
+  mandatory: true
+  type: string
+  yaml_type: scalar
+  doc: The name of the wolf plugin where the sensor is implemented.
\ No newline at end of file

schema/solver/SolverCeres.schema

@@ -25,18 +25,18 @@ function_tolerance:
   mandatory: true
   type: double
   yaml_type: scalar
-  doc: Function tolerance. Convergence criterion. Typical value: 1e-8
+  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
+  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.
+  doc: Amount of hreads used by ceres.
 use_nonmonotonic_steps:
   mandatory: false
   default: false

schema/solver/SolverManager.schema

@@ -8,7 +8,7 @@ verbose:
   type: int
   yaml_type: scalar
   options: [0, 1, 2]
-  doc: Verbosity of the solver. 0: Nothing, 1: Brief report, 2: Full report.
+  doc: "Verbosity of the solver. 0: Nothing, 1: Brief report, 2: Full report."
 compute_cov:
   mandatory: true
   type: bool
@@ -20,7 +20,7 @@ cov_enum:
   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.
+  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

schema/tree_manager/None.schema

+follow: TreeManagerBase.schema
\ No newline at end of file

src/problem/problem.cpp

@@ -149,8 +149,12 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
     WOLF_INFO("Loading tree manager parameters");
     YAML::Node tree_manager_node = problem_node["tree_manager"];
     std::string tree_manager_type = tree_manager_node["type"].as<std::string>();
-    problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
-
+    if (tree_manager_type != "none" and 
+        tree_manager_type != "None" and 
+        tree_manager_type != "NONE")
+    {
+        problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
+    }
     // TODO: Prior -- first keyframe 
     // std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
     // assert((prior_mode == "nothing" || prior_mode == "initial_guess" || prior_mode == "fix" || prior_mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");

test/CMakeLists.txt

@@ -214,6 +214,9 @@ wolf_add_gtest(gtest_tree_manager gtest_tree_manager.cpp)
 # wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
 # target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)
 
+# Schema test
+wolf_add_gtest(gtest_schema gtest_schema.cpp)
+
 # # SensorDiffDriveSelfcalib class test
 # wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
 

test/gtest_schema.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/utils_gtest.h"
+#include "core/internal/config.h"
+#include "yaml-schema-cpp/yaml_schema.hpp"
+
+using namespace yaml_schema_cpp;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+TEST(Schema, validate_all_schemas)
+{
+    ASSERT_TRUE(validateAllSchemas({wolf_root}));
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/yaml/params1.yaml

-config:
-  problem:
-    frame_structure: "POV"
-    dimension: 3
-  sensors: 
-    -
-      type: "ODOM 2d"
-      name: "odom"
-      plugin: "core"
-      intrinsic:
-        k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
-      extrinsic:
-        pose: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
-      plugin: "core"
-  processors:
-    -
-      type: "ODOM 2d"
-      name: "processor1"
-      sensor_name: "odom"
-      plugin: "core"
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor_name: "rb"
-      plugin: "core"
-    -
-      type: "ODOM 2d"
-      name: "my_proc_test"
-      sensor_name: "odom"
-      plugin: "core"
-      follow: "params3.1.yaml"
-    -
-      type: "ODOM 3d"
-      name: "my_proc_odom3d"
-      sensor_name: "odom"
-      plugin: "core"
-      follow: "processor_odom_3d.yaml"
+problem:
+  frame_structure: "POV"
+  dimension: 3
+sensors: 
+  -
+    type: "ODOM 2d"
+    name: "odom"
+    plugin: "core"
+    intrinsic:
+      k_disp_to_disp: 0.1
+      k_rot_to_rot: 0.1 
+    extrinsic:
+      pose: [1,2,3]
+  -
+    type: "RANGE BEARING"
+    name: "rb"
+    plugin: "core"
+processors:
+  -
+    type: "ODOM 2d"
+    name: "processor1"
+    sensor_name: "odom"
+    plugin: "core"
+  -
+    type: "RANGE BEARING"
+    name: "rb_processor"
+    sensor_name: "rb"
+    plugin: "core"
+  -
+    type: "ODOM 2d"
+    name: "my_proc_test"
+    sensor_name: "odom"
+    plugin: "core"
+    follow: "params3.1.yaml"
+  -
+    type: "ODOM 3d"
+    name: "my_proc_odom3d"
+    sensor_name: "odom"
+    plugin: "core"
+    follow: "processor_odom_3d.yaml"

test/yaml/params2.yaml

-config:
-  problem:
-    frame_structure: "POV"
-    dimension: 2
-  sensors: 
-    -
-      type: "ODOM 2d"
-      name: "odom"
-      plugin: "core"
-      intrinsic:
-        k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1 
-      extrinsic:
-        pose: [1,2,3]
-    -
-      type: "RANGE BEARING"
-      name: "rb"
-      plugin: "core"
-  processors:
-    -
-      type: "ODOM 2d"
-      name: "processor1"
-      sensor_name: "odom"
-      plugin: "core"
-      $mymap:
-        k1: v1
-        k2: v2
-        k3: [v3,v4,v5]
-    -
-      type: "RANGE BEARING"
-      name: "rb_processor"
-      sensor_name: "rb"
-      plugin: "core"
-    -
-      type: "ODOM 2d"
-      name: "my_proc_test"
-      sensor_name: "odom"
-      plugin: "core"
+problem:
+  frame_structure: "POV"
+  dimension: 2
+sensors: 
+  -
+    type: "ODOM 2d"
+    name: "odom"
+    plugin: "core"
+    intrinsic:
+      k_disp_to_disp: 0.1
+      k_rot_to_rot: 0.1 
+    extrinsic:
+      pose: [1,2,3]
+  -
+    type: "RANGE BEARING"
+    name: "rb"
+    plugin: "core"
+processors:
+  -
+    type: "ODOM 2d"
+    name: "processor1"
+    sensor_name: "odom"
+    plugin: "core"
+    $mymap:
+      k1: v1
+      k2: v2
+      k3: [v3,v4,v5]
+  -
+    type: "RANGE BEARING"
+    name: "rb_processor"
+    sensor_name: "rb"
+    plugin: "core"
+  -
+    type: "ODOM 2d"
+    name: "my_proc_test"
+    sensor_name: "odom"
+    plugin: "core"

test/yaml/params3.yaml

-config:
-  problem:
-    frame_structure: "POV"
-    dimension: 2
-  sensors:
-    -
-      type: "ODOM 2d"
-      name: "odom"
-      plugin: "core"
-      intrinsic:
-        k_disp_to_disp: 0.1
-        k_rot_to_rot: 0.1
-      extrinsic:
-        pose: [1,2,3]
-  processors:
-    -
-      type: "ODOM 2d"
-      name: "my_proc_test"
-      plugin: "core"
-      sensor_name: "odom"
-      extern_params: 
-        follow: "params3.1.yaml"
\ No newline at end of file
+problem:
+  frame_structure: "POV"
+  dimension: 2
+sensors:
+  -
+    type: "ODOM 2d"
+    name: "odom"
+    plugin: "core"
+    intrinsic:
+      k_disp_to_disp: 0.1
+      k_rot_to_rot: 0.1
+    extrinsic:
+      pose: [1,2,3]
+processors:
+  -
+    type: "ODOM 2d"
+    name: "my_proc_test"
+    plugin: "core"
+    sensor_name: "odom"
+    extern_params: 
+      follow: "params3.1.yaml"
\ No newline at end of file

test/yaml/params_basic.yaml

-config:
-  problem:
-    dim: 2
-    
-  int_1: -3
-  int_2: 0
-  int_3: "6"
-  uint_1: 2
-  uint_2: 0
-  uint_3: "6"
-  double_1: 3.6
-  double_2: -3
-  double_3: "3.6"
-  string_1: wolf
-  string_2: "Wolf"
-  bool_1: true
-  bool_2: True
-  bool_3: TRUE
-  bool_4: "true"
-  bool_5: false
-  bool_6: False
-  bool_7: FALSE
-  bool_8: "false"
-  
-  int_wrong_1: 2.3
-  int_wrong_2: "wolf"
-  int_wrong_3: true
-  uint_wrong_1: -2
-  uint_wrong_2: 3.5
-  uint_wrong_3: "wolf"
-  uint_wrong_4: true
-  double_wrong_1: "wolf"
-  double_wrong_2: true
-  bool_wrong: 1
+problem:
+  dim: 2
   
+int_1: -3
+int_2: 0
+int_3: "6"
+uint_1: 2
+uint_2: 0
+uint_3: "6"
+double_1: 3.6
+double_2: -3
+double_3: "3.6"
+string_1: wolf
+string_2: "Wolf"
+bool_1: true
+bool_2: True
+bool_3: TRUE
+bool_4: "true"
+bool_5: false
+bool_6: False
+bool_7: FALSE
+bool_8: "false"
+
+int_wrong_1: 2.3
+int_wrong_2: "wolf"
+int_wrong_3: true
+uint_wrong_1: -2
+uint_wrong_2: 3.5
+uint_wrong_3: "wolf"
+uint_wrong_4: true
+double_wrong_1: "wolf"
+double_wrong_2: true
+bool_wrong: 1
+

test/yaml/params_problem_autosetup.yaml

-config:
-  problem:
-    frame_structure: "PO"
-    dimension: 3
-    prior:
-      mode: "factor"
-      $state:
-        P: [0,0,0]
-        O: [0,0,0,1]
-      $sigma:
-        P: [0.31, 0.31, 0.31]
-        O: [0.31, 0.31, 0.31]
-      time_tolerance: 0.1
-    tree_manager: 
-      type: "None"
-  map:
-    type: "MapBase"
+problem:
+  frame_structure: "PO"
+  dimension: 3
+  prior:
+    mode: "factor"
+    $state:
+      P: [0,0,0]
+      O: [0,0,0,1]
+    $sigma:
+      P: [0.31, 0.31, 0.31]
+      O: [0.31, 0.31, 0.31]
+    time_tolerance: 0.1
+  tree_manager: 
+    type: "None"
+map:
+  type: "MapBase"
+  plugin: "core"
+sensors: 
+  -
+    type: "SensorOdom3d"
+    name: "odom"
     plugin: "core"
-  sensors: 
-    -
-      type: "SensorOdom3d"
-      name: "odom"
-      plugin: "core"
-      k_disp_to_disp: 0.1
-      k_disp_to_rot: 0.1
-      k_rot_to_rot: 0.1 
-      min_disp_var: 0.1 
-      min_rot_var: 0.1
-      states:
-        P:
-          state: [1,2,3]
-          mode: fix
-          dynamic: false
-        O:
-          state: [0,0,0,1]
-          mode: "fix"
-          dynamic: false
-  processors:
-    -
-      type: "ProcessorOdom3d"
-      name: "my_proc_odom3d"
-      sensor_name: "odom"
-      plugin: "core"
-      apply_loss_function: false
-      time_tolerance:         0.01  # seconds
-      keyframe_vote:
-        voting_active:        true
-        max_time_span:          1.95  # seconds
-        max_buff_length:        999   # motion deltas
-        dist_traveled:          999   # meters
-        angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
-      
-      unmeasured_perturbation_std: 0.001
-      
-      state_getter: true
-      state_priority: 1
+    k_disp_to_disp: 0.1
+    k_disp_to_rot: 0.1
+    k_rot_to_rot: 0.1 
+    min_disp_var: 0.1 
+    min_rot_var: 0.1
+    states:
+      P:
+        state: [1,2,3]
+        mode: fix
+        dynamic: false
+      O:
+        state: [0,0,0,1]
+        mode: "fix"
+        dynamic: false
+processors:
+  -
+    type: "ProcessorOdom3d"
+    name: "my_proc_odom3d"
+    sensor_name: "odom"
+    plugin: "core"
+    apply_loss_function: false
+    time_tolerance:         0.01  # seconds
+    keyframe_vote:
+      voting_active:        true
+      max_time_span:          1.95  # seconds
+      max_buff_length:        999   # motion deltas
+      dist_traveled:          999   # meters
+      angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
+    
+    unmeasured_perturbation_std: 0.001
+    
+    state_getter: true
+    state_priority: 1

test/yaml/params_problem_autosetup_no_map.yaml

-config:
-  problem:
-    frame_structure: "PO"
-    dimension: 3
-    prior:
-      mode: "factor"
-      $state:
-        P: [0,0,0]
-        O: [0,0,0,1]
-      $sigma:
-        P: [0.31, 0.31, 0.31]
-        O: [0.31, 0.31, 0.31]
-      time_tolerance: 0.1
-    tree_manager: 
-      type: "None"
-  sensors:  
-    -
-      type: "SensorOdom3d"
-      name: "odom"
-      plugin: "core"
-      k_disp_to_disp: 0.1
-      k_disp_to_rot: 0.1
-      k_rot_to_rot: 0.1 
-      min_disp_var: 0.1 
-      min_rot_var: 0.1
-      states:
-        P:
-          state: [1,2,3]
-          mode: fix
-          dynamic: false
-        O:
-          state: [0,0,0,1]
-          mode: "fix"
-          dynamic: false
-  processors:
-    -
-      type: "ProcessorOdom3d"
-      name: "my_proc_odom3d"
-      sensor_name: "odom"
-      plugin: "core"
-      apply_loss_function: false
-      time_tolerance:         0.01  # seconds
-      keyframe_vote:
-        voting_active:        true
-        max_time_span:          1.95  # seconds
-        max_buff_length:        999   # motion deltas
-        dist_traveled:          999   # meters
-        angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
-      
-      unmeasured_perturbation_std: 0.00111
-      
-      state_getter: true
-      state_priority: 1
+problem:
+  frame_structure: "PO"
+  dimension: 3
+  prior:
+    mode: "factor"
+    $state:
+      P: [0,0,0]
+      O: [0,0,0,1]
+    $sigma:
+      P: [0.31, 0.31, 0.31]
+      O: [0.31, 0.31, 0.31]
+    time_tolerance: 0.1
+  tree_manager: 
+    type: "None"
+sensors:  
+  -
+    type: "SensorOdom3d"
+    name: "odom"
+    plugin: "core"
+    k_disp_to_disp: 0.1
+    k_disp_to_rot: 0.1
+    k_rot_to_rot: 0.1 
+    min_disp_var: 0.1 
+    min_rot_var: 0.1
+    states:
+      P:
+        state: [1,2,3]
+        mode: fix
+        dynamic: false
+      O:
+        state: [0,0,0,1]
+        mode: "fix"
+        dynamic: false
+processors:
+  -
+    type: "ProcessorOdom3d"
+    name: "my_proc_odom3d"
+    sensor_name: "odom"
+    plugin: "core"
+    apply_loss_function: false
+    time_tolerance:         0.01  # seconds
+    keyframe_vote:
+      voting_active:        true
+      max_time_span:          1.95  # seconds
+      max_buff_length:        999   # motion deltas
+      dist_traveled:          999   # meters
+      angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
+    
+    unmeasured_perturbation_std: 0.00111
+    
+    state_getter: true
+    state_priority: 1

test/yaml/params_problem_odom_3d.yaml

-config:
-  problem:
-    frame_structure: "PO"
-    dimension: 3
-    prior:
-      mode: "factor"
-      $state:
-        P: [0,0,0]
-        O: [0,0,0,1]
-      $sigma:
-        P: [0.31, 0.31, 0.31]
-        O: [0.31, 0.31, 0.31]
-      time_tolerance: 0.1
-    tree_manager: 
-      type: "None"
-  sensors: 
-    -
-      type: "SensorOdom3d"
-      name: "odom"
-      plugin: "core"
-      k_disp_to_disp: 0.1
-      k_disp_to_rot: 0.1
-      k_rot_to_rot: 0.1 
-      min_disp_var: 0.1 
-      min_rot_var: 0.1
-      extrinsic:
-        pose: [1,2,3,0,0,0,1]
-  processors:
-    -
-      type: "ProcessorOdom3d"
-      name: "my_proc_odom3d"
-      sensor_name: "odom"
-      plugin: "core"
-      apply_loss_function: false
-      time_tolerance:         0.01  # seconds
-      keyframe_vote:
-        voting_active:        true
-        max_time_span:          1.95  # seconds
-        max_buff_length:        999   # motion deltas
-        dist_traveled:          999   # meters
-        angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
-      
-      unmeasured_perturbation_std: 0.00111
-      
-      state_getter: true
-      state_priority: 1
+problem:
+  frame_structure: "PO"
+  dimension: 3
+  prior:
+    mode: "factor"
+    $state:
+      P: [0,0,0]
+      O: [0,0,0,1]
+    $sigma:
+      P: [0.31, 0.31, 0.31]
+      O: [0.31, 0.31, 0.31]
+    time_tolerance: 0.1
+  tree_manager: 
+    type: "None"
+sensors: 
+  -
+    type: "SensorOdom3d"
+    name: "odom"
+    plugin: "core"
+    k_disp_to_disp: 0.1
+    k_disp_to_rot: 0.1
+    k_rot_to_rot: 0.1 
+    min_disp_var: 0.1 
+    min_rot_var: 0.1
+    extrinsic:
+      pose: [1,2,3,0,0,0,1]
+processors:
+  -
+    type: "ProcessorOdom3d"
+    name: "my_proc_odom3d"
+    sensor_name: "odom"
+    plugin: "core"
+    apply_loss_function: false
+    time_tolerance:         0.01  # seconds
+    keyframe_vote:
+      voting_active:        true
+      max_time_span:          1.95  # seconds
+      max_buff_length:        999   # motion deltas
+      dist_traveled:          999   # meters
+      angle_turned:           999   # radians (1 rad approx 57 deg, approx 60 deg)
+    
+    unmeasured_perturbation_std: 0.00111
+    
+    state_getter: true
+    state_priority: 1

test/yaml/params_tree_manager1.yaml

-config:
-  problem:
-    frame_structure: "POV"
-    dimension: 3
-    prior:
-      mode: "factor"
-      # state: [0,0,0,0,0,0,1,0,0,0]
-      # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
-      $state:
-        P: [0,0,0]
-        O: [0,0,0,1]
-        V: [0,0,0]
-      $sigma:
-        P: [0.31, 0.31, 0.31]
-        O: [0.31, 0.31, 0.31]
-        V: [0.31, 0.31, 0.31]
-      time_tolerance: 0.1
-    tree_manager:
-      follow: "tree_manager_dummy.yaml"
-  sensors: 
-    -
-      type: "SensorOdom"
-      name: "odom"
-      plugin: "core"
-      states:
-        P:
-          mode: fix
-          state: [1, 2, 3]
-          dynamic: false
-        O:
-          mode: fix
-          state: [0, 0, 0, 1]
-          dynamic: false
-      k_disp_to_disp: 0.1
-      k_disp_to_rot: 0.1
-      k_rot_to_rot: 0.1 
-      min_disp_var: 0.1 
-      min_rot_var: 0.1
-  processors:
-    -
-      type: "ProcessorOdom3d"
-      name: "my_proc_odom3d"
-      sensor_name: "odom"
-      plugin: "core"
-      apply_loss_function: false
-      time_tolerance:         0.01  # seconds
-      keyframe_vote:
-        voting_active:        false
-        max_time_span:          0.2   # seconds
-        max_buff_length:        10    # motion deltas
-        dist_traveled:          0.5   # meters
-        angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
-      
-      unmeasured_perturbation_std: 0.00111
-      
-      state_getter: true
-      state_priority: 1
-      
+problem:
+  frame_structure: "POV"
+  dimension: 3
+  prior:
+    mode: "factor"
+    # state: [0,0,0,0,0,0,1,0,0,0]
+    # cov: [[9,9],.1,0,0,0,0,0,0,0,0, 0,.1,0,0,0,0,0,0,0, 0,0,.1,0,0,0,0,0,0, 0,0,0,.1,0,0,0,0,0, 0,0,0,0,.1,0,0,0,0, 0,0,0,0,0,.1,0,0,0, 0,0,0,0,0,0,.1,0,0, 0,0,0,0,0,0,0,.1,0, 0,0,0,0,0,0,0,0,.1]
+    $state:
+      P: [0,0,0]
+      O: [0,0,0,1]
+      V: [0,0,0]
+    $sigma:
+      P: [0.31, 0.31, 0.31]
+      O: [0.31, 0.31, 0.31]
+      V: [0.31, 0.31, 0.31]
+    time_tolerance: 0.1
+  tree_manager:
+    follow: "tree_manager_dummy.yaml"
+sensors: 
+  -
+    type: "SensorOdom"
+    name: "odom"
+    plugin: "core"
+    states:
+      P:
+        mode: fix
+        state: [1, 2, 3]
+        dynamic: false
+      O:
+        mode: fix
+        state: [0, 0, 0, 1]
+        dynamic: false
+    k_disp_to_disp: 0.1
+    k_disp_to_rot: 0.1
+    k_rot_to_rot: 0.1 
+    min_disp_var: 0.1 
+    min_rot_var: 0.1
+processors:
+  -
+    type: "ProcessorOdom3d"
+    name: "my_proc_odom3d"
+    sensor_name: "odom"
+    plugin: "core"
+    apply_loss_function: false
+    time_tolerance:         0.01  # seconds
+    keyframe_vote:
+      voting_active:        false
+      max_time_span:          0.2   # seconds
+      max_buff_length:        10    # motion deltas
+      dist_traveled:          0.5   # meters
+      angle_turned:           0.1   # radians (1 rad approx 57 deg, approx 60 deg)
+    
+    unmeasured_perturbation_std: 0.00111
+    
+    state_getter: true
+    state_priority: 1
+