From 2addf05245c9479780390bca7edffe7aa1b5f672 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 17:55:59 +0200 Subject: [PATCH] [skip ci] working on gtests --- include/core/problem/problem.h | 19 - include/core/processor/factory_processor.h | 10 - include/core/processor/processor_base.h | 14 +- include/core/sensor/factory_sensor.h | 26 +- include/core/sensor/sensor_base.h | 14 - include/core/sensor/sensor_odom.h | 20 +- include/core/state_block/prior.h | 8 +- .../core/tree_manager/factory_tree_manager.h | 10 - include/core/tree_manager/tree_manager_base.h | 6 - schema/PriorModeDynamicNoiseDrift.schema | 26 ++ schema/PriorO2d.schema | 27 +- schema/PriorO3d.schema | 13 + schema/PriorP2d.schema | 27 +- schema/PriorP3d.schema | 13 + src/problem/problem.cpp | 36 -- src/sensor/sensor_base.cpp | 12 +- src/sensor/sensor_odom.cpp | 20 +- src/state_block/prior.cpp | 19 +- ...ensorDummy.schema => SensorDummy2d.schema} | 0 test/dummy/SensorDummy3d.schema | 16 + test/dummy/SensorDummyPoia3d.schema | 44 +++ test/dummy/sensor_dummy.h | 5 +- test/dummy/sensor_dummy_poia.h | 87 ---- test/gtest_processor_base.cpp | 12 +- test/gtest_sensor_base.cpp | 372 ++---------------- test/gtest_sensor_odom.cpp | 292 -------------- test/gtest_tree_manager.cpp | 32 -- test/yaml/sensor_tests/sensor_POIA_3D.yaml | 4 +- .../sensor_tests/sensor_POIA_3D_wrong.yaml | 3 +- .../sensor_tests/sensor_PO_2D_factor.yaml | 3 +- .../sensor_PO_2D_factor_dynamic.yaml | 3 +- .../sensor_PO_2D_factor_dynamic_drift.yaml | 2 + ...nsor_PO_2D_factor_dynamic_drift_wrong.yaml | 2 + .../sensor_PO_2D_factor_dynamic_wrong.yaml | 2 + .../sensor_PO_2D_factor_wrong.yaml | 3 +- test/yaml/sensor_tests/sensor_PO_2D_fix.yaml | 3 +- .../sensor_PO_2D_fix_dynamic.yaml | 3 +- .../sensor_PO_2D_fix_dynamic_drift.yaml | 3 +- .../sensor_PO_2D_fix_dynamic_drift_wrong.yaml | 3 +- .../sensor_PO_2D_fix_dynamic_wrong.yaml | 3 +- .../sensor_tests/sensor_PO_2D_fix_wrong.yaml | 3 +- .../sensor_PO_2D_initial_guess.yaml | 3 +- .../sensor_PO_2D_initial_guess_dynamic.yaml | 3 +- ...sor_PO_2D_initial_guess_dynamic_drift.yaml | 3 +- ..._2D_initial_guess_dynamic_drift_wrong.yaml | 3 +- ...sor_PO_2D_initial_guess_dynamic_wrong.yaml | 3 +- .../sensor_PO_2D_initial_guess_wrong.yaml | 3 +- .../sensor_tests/sensor_PO_3D_factor.yaml | 3 +- .../sensor_PO_3D_factor_dynamic.yaml | 3 +- .../sensor_PO_3D_factor_dynamic_drift.yaml | 3 +- ...nsor_PO_3D_factor_dynamic_drift_wrong.yaml | 3 +- .../sensor_PO_3D_factor_dynamic_wrong.yaml | 3 +- .../sensor_PO_3D_factor_wrong.yaml | 3 +- test/yaml/sensor_tests/sensor_PO_3D_fix.yaml | 3 +- .../sensor_PO_3D_fix_dynamic.yaml | 3 +- .../sensor_PO_3D_fix_dynamic_drift.yaml | 3 +- .../sensor_PO_3D_fix_dynamic_drift_wrong.yaml | 3 +- .../sensor_PO_3D_fix_dynamic_wrong.yaml | 3 +- .../sensor_tests/sensor_PO_3D_fix_wrong.yaml | 3 +- .../sensor_PO_3D_initial_guess.yaml | 3 +- .../sensor_PO_3D_initial_guess_dynamic.yaml | 3 +- ...sor_PO_3D_initial_guess_dynamic_drift.yaml | 3 +- ..._3D_initial_guess_dynamic_drift_wrong.yaml | 3 +- ...sor_PO_3D_initial_guess_dynamic_wrong.yaml | 3 +- .../sensor_PO_3D_initial_guess_wrong.yaml | 3 +- 65 files changed, 261 insertions(+), 1031 deletions(-) create mode 100644 schema/PriorModeDynamicNoiseDrift.schema create mode 100644 schema/PriorO3d.schema create mode 100644 schema/PriorP3d.schema rename test/dummy/{SensorDummy.schema => SensorDummy2d.schema} (100%) create mode 100644 test/dummy/SensorDummy3d.schema create mode 100644 test/dummy/SensorDummyPoia3d.schema delete mode 100644 test/dummy/sensor_dummy_poia.h diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 2a779c07e..027b1b4e8 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -146,14 +146,6 @@ class Problem : public std::enable_shared_from_this<Problem> SensorBasePtr installSensor(const std::string& _sen_type, const std::string& _params_yaml_filename, const std::vector<std::string>& _folders_schema); - /** \brief Factory method to install (create and add) sensors only from its properties - * \param _sen_type type of sensor - * \param _params a base-pointer to a derived struct defining the intrinsic parameters. - * \param _priors an unordered map of the states priors indexed by their key. - */ - SensorBasePtr installSensor(const std::string& _sen_type, - ParamsSensorBasePtr _params, - const Priors& _priors); /** \brief Factory method to install (create, and add to sensor) processors only from a YAML node -- Helper method loading parameters from file * \param _processor_node YAML node containing all necessary information to call the factory and create the processor. @@ -175,17 +167,6 @@ class Problem : public std::enable_shared_from_this<Problem> const std::string& _params_yaml_filename, const std::vector<std::string>& _folders_schema); - /** \brief Factory method to install (create, and add to sensor) processors only from its properties - * - * This method creates a Processor, and adds it to the specified sensor's list of processors - * \param _prc_type type of processor - * \param _corresponding_sensor_ptr pointer to the sensor where the processor will be installed. - * \param _prc_params a base-pointer to a derived struct defining the processor parameters. - */ - ProcessorBasePtr installProcessor(const std::string& _prc_type, - SensorBasePtr _corresponding_sensor_ptr, - ParamsProcessorBasePtr _prc_params); - /** \brief get a sensor pointer by its name * \param _sensor_name The sensor name, as it was installed with installSensor() */ diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h index 253ccb936..b432824fb 100644 --- a/include/core/processor/factory_processor.h +++ b/include/core/processor/factory_processor.h @@ -134,20 +134,10 @@ inline std::string FactoryProcessorYaml::getClass() const return "FactoryProcessorYaml"; } -typedef Factory<ProcessorBase, - std::shared_ptr<ParamsProcessorBase>> FactoryProcessorParams; -template<> -inline std::string FactoryProcessorParams::getClass() const -{ - return "FactoryProcessorParams"; -} - #define WOLF_REGISTER_PROCESSOR(ProcessorType) \ namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \ wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \ namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered = \ wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); } \ - namespace{ const bool WOLF_UNUSED ProcessorType##ParamsRegistered = \ - wolf::FactoryProcessorParams::registerCreator(#ProcessorType, ProcessorType::create); } \ } /* namespace wolf */ \ No newline at end of file diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index 124744ce3..adc03e088 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -80,19 +80,6 @@ static ProcessorBasePtr create(const std::string& _yaml_filepath, \ return std::make_shared<ProcessorClass>(params); \ } \ -static ProcessorBasePtr create(const ParamsProcessorBasePtr _params) \ -{ \ - auto params_derived = std::dynamic_pointer_cast<ParamsProcessorClass>(_params); \ - if (not params_derived) \ - { \ - WOLF_ERROR("In " #ProcessorClass " creator:", \ - " _params is not of type " #ParamsProcessorClass "!"); \ - return nullptr; \ - } \ - \ - return std::make_shared<ProcessorClass>(params_derived); \ -} \ - /** \brief Buffer for arbitrary type objects * @@ -468,6 +455,7 @@ template<typename classType, typename... T> std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&... all) { std::shared_ptr<classType> prc = std::make_shared<classType>(std::forward<T>(all)...); + prc->configure(_sen_ptr); prc->link(_sen_ptr); return prc; } diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h index fa8cc69e3..976097d45 100644 --- a/include/core/sensor/factory_sensor.h +++ b/include/core/sensor/factory_sensor.h @@ -218,11 +218,6 @@ typedef Factory<SensorBase, const std::string&, const std::vector<std::string>&> FactorySensorYaml; -typedef Factory<SensorBase, - SizeEigen, - std::shared_ptr<ParamsSensorBase>, - const Priors& > FactorySensorPriors; - template<> inline std::string FactorySensor::getClass() const { @@ -233,18 +228,15 @@ inline std::string FactorySensorYaml::getClass() const { return "FactorySensorYaml"; } -template<> -inline std::string FactorySensorPriors::getClass() const -{ - return "FactorySensorPriors"; -} -#define WOLF_REGISTER_SENSOR(SensorType) \ - namespace{ const bool WOLF_UNUSED SensorType##Registered = \ - FactorySensor::registerCreator(#SensorType, SensorType::create);} \ - namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered = \ - FactorySensorYaml::registerCreator(#SensorType, SensorType::create);} \ - namespace{ const bool WOLF_UNUSED SensorType##PriorsRegistered = \ - FactorySensorPriors::registerCreator(#SensorType, SensorType::create);} \ + + +#define WOLF_REGISTER_SENSOR_WITH_KEY(SensorClassName, SensorType) \ + namespace{ const bool WOLF_UNUSED SensorClassName##Registered = \ + FactorySensor::registerCreator(#SensorClassName, SensorType::create);} \ + namespace{ const bool WOLF_UNUSED SensorClassName##YamlRegistered = \ + FactorySensorYaml::registerCreator(#SensorClassName, SensorType::create);} \ + +#define WOLF_REGISTER_SENSOR(SensorType) WOLF_REGISTER_SENSOR_WITH_KEY(SensorType, SensorType) \ } /* namespace wolf */ \ No newline at end of file diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 5910fe3c4..cc1f5d408 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -86,20 +86,6 @@ static SensorBasePtr create(SizeEigen _dim, \ return std::make_shared<SensorClass>(_dim, params, priors); \ } \ -static SensorBasePtr create(SizeEigen _dim, \ - ParamsSensorBasePtr _params, \ - const Priors& _priors) \ -{ \ - auto params_derived = std::dynamic_pointer_cast<ParamsSensorClass>(_params); \ - if (not params_derived) \ - { \ - WOLF_ERROR("In " #SensorClass " creator:", \ - " _params is not of type " #ParamsSensorClass "!"); \ - return nullptr; \ - } \ - \ - return std::make_shared<SensorClass>(_dim, params_derived, _priors); \ -} \ /** \brief base struct for intrinsic sensor parameters * diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h index b73c95ce1..6cb43d327 100644 --- a/include/core/sensor/sensor_odom.h +++ b/include/core/sensor/sensor_odom.h @@ -67,10 +67,10 @@ class SensorOdom : public SensorBase SizeEigen dim_; ParamsSensorOdomPtr params_odom_; - // protected constructor to avoid creation of SensorOdom without dimension + public: SensorOdom(const SizeEigen& _dim, ParamsSensorOdomPtr _params, const Priors& _priors); + WOLF_SENSOR_CREATE(SensorOdom, ParamsSensorOdom); - public: ~SensorOdom() override = default; double getDispVarToDispNoiseFactor() const; @@ -83,22 +83,6 @@ 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/state_block/prior.h b/include/core/state_block/prior.h index 0864028a4..67da9837d 100644 --- a/include/core/state_block/prior.h +++ b/include/core/state_block/prior.h @@ -74,13 +74,7 @@ class Priors : public PriorMap public: using PriorMap::PriorMap; - Priors(const YAML::Node& priors_node) - { - for (auto prior_pair : priors_node) - { - this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second)); - } - } + Priors(const YAML::Node& priors_node); virtual ~Priors() = default; }; diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h index edd9a44ef..c4f4e4d56 100644 --- a/include/core/tree_manager/factory_tree_manager.h +++ b/include/core/tree_manager/factory_tree_manager.h @@ -52,20 +52,10 @@ inline std::string FactoryTreeManagerYaml::getClass() const return "FactoryTreeManagerYaml"; } -typedef Factory<TreeManagerBase, - ParamsTreeManagerBasePtr> FactoryTreeManagerParams; -template<> -inline std::string FactoryTreeManagerParams::getClass() const -{ - return "FactoryTreeManagerParams"; -} - #define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \ namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \ wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \ namespace{ const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \ wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); } \ - namespace{ const bool WOLF_UNUSED TreeManagerType##ParamsRegistered = \ - wolf::FactoryTreeManagerParams::registerCreator(#TreeManagerType, TreeManagerType::create); } \ } /* namespace wolf */ \ No newline at end of file diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h index 508f48438..588f735c9 100644 --- a/include/core/tree_manager/tree_manager_base.h +++ b/include/core/tree_manager/tree_manager_base.h @@ -64,12 +64,6 @@ static TreeManagerBasePtr create(const std::string& _yaml_filepath, \ return std::make_shared<TreeManagerClass>(params); \ } \ -static TreeManagerBasePtr create(ParamsTreeManagerBasePtr _params) \ -{ \ - auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \ - \ - return std::make_shared<TreeManagerClass>(params); \ -} \ struct ParamsTreeManagerBase : public ParamsBase { diff --git a/schema/PriorModeDynamicNoiseDrift.schema b/schema/PriorModeDynamicNoiseDrift.schema new file mode 100644 index 000000000..4c14a5652 --- /dev/null +++ b/schema/PriorModeDynamicNoiseDrift.schema @@ -0,0 +1,26 @@ +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/PriorO2d.schema b/schema/PriorO2d.schema index 08173f33b..402d0c37e 100644 --- a/schema/PriorO2d.schema +++ b/schema/PriorO2d.schema @@ -10,29 +10,4 @@ state: 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 +follow: PriorModeDynamicNoiseDrift.schema \ No newline at end of file diff --git a/schema/PriorO3d.schema b/schema/PriorO3d.schema new file mode 100644 index 000000000..e7b1dd38b --- /dev/null +++ b/schema/PriorO3d.schema @@ -0,0 +1,13 @@ +type: + type: string + yaml_type: scalar + mandatory: false + default: StateQuaternion + options: [StateQuaternion] + doc: The derived type of the State in 'O' +state: + type: Vector4d + yaml_type: scalar + mandatory: true + doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized) +follow: PriorModeDynamicNoiseDrift.schema \ No newline at end of file diff --git a/schema/PriorP2d.schema b/schema/PriorP2d.schema index 3e17abd5f..d8f19390f 100644 --- a/schema/PriorP2d.schema +++ b/schema/PriorP2d.schema @@ -10,29 +10,4 @@ state: 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 +follow: PriorModeDynamicNoiseDrift.schema \ No newline at end of file diff --git a/schema/PriorP3d.schema b/schema/PriorP3d.schema new file mode 100644 index 000000000..d413a9029 --- /dev/null +++ b/schema/PriorP3d.schema @@ -0,0 +1,13 @@ +type: + type: string + yaml_type: scalar + mandatory: false + default: StatePoint3d + options: [StatePoint3d] + doc: The derived type of the state in 'P' +state: + type: Vector3d + yaml_type: scalar + mandatory: true + doc: A vector containing the state 'P' values +follow: PriorModeDynamicNoiseDrift.schema \ No newline at end of file diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 98a12bb81..0442805fe 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -259,18 +259,6 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, return sen_ptr; } -SensorBasePtr Problem::installSensor(const std::string& _sen_type, - ParamsSensorBasePtr _params, - const Priors& _priors) -{ - SensorBasePtr sen_ptr = FactorySensorPriors::create(_sen_type, - dim_, - _params, - _priors); - sen_ptr->link(getHardware()); - return sen_ptr; -} - ProcessorBasePtr Problem::installProcessor(const YAML::Node& _processor_node) { auto corresponding_sensor_name = _processor_node["sensor_name"].as<std::string>(); @@ -316,30 +304,6 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, return prc_ptr; } -ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, - SensorBasePtr _corresponding_sensor_ptr, - ParamsProcessorBasePtr _prc_params) -{ - if (_corresponding_sensor_ptr == nullptr) - { - WOLF_ERROR("Cannot install processor '", _prc_params->name, - "' since the associated sensor does not exist !"); - return ProcessorBasePtr(); - } - - ProcessorBasePtr prc_ptr = FactoryProcessorParams::create(_prc_type, _prc_params); - - //Dimension check - int prc_dim = prc_ptr->getDim(); - auto prb = this; - assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension"); - - prc_ptr->configure(_corresponding_sensor_ptr); - prc_ptr->link(_corresponding_sensor_ptr); - - return prc_ptr; -} - SensorBaseConstPtr Problem::findSensor(const std::string& _sensor_name) const { return getHardware()->getSensor(_sensor_name); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 9ecb93796..d6ad0ad51 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -69,10 +69,14 @@ void SensorBase::loadPriors(const Priors& _priors, auto prior = state_pair.second; // type - if (key == 'P' and prior.getType() != "P" and prior.getType() != "StateBlock") - throw std::runtime_error("Prior type for key P has to be 'P' or 'StateBlock'"); - if (key == 'V' and prior.getType() != "V" and prior.getType() != "StateBlock") - throw std::runtime_error("Prior type for key V has to be 'V' or 'StateBlock'"); + if (key == 'P' and _dim == 2 and prior.getType() != "P" and prior.getType() != "StatePoint2d") + throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint2d'"); + if (key == 'P' and _dim == 3 and prior.getType() != "P" and prior.getType() != "StatePoint3d") + throw std::runtime_error("Prior type for key P has to be 'P' or 'StatePoint3d'"); + if (key == 'V' and _dim == 2 and prior.getType() != "V" and prior.getType() != "StatePoint2d") + throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint2d'"); + if (key == 'V' and _dim == 3 and prior.getType() != "V" and prior.getType() != "StatePoint3d") + throw std::runtime_error("Prior type for key V has to be 'V' or 'StatePoint3d'"); if (key == 'O' and _dim == 2 and prior.getType() != "O" and prior.getType() != "StateAngle") throw std::runtime_error("Prior type for key O in 2D has to be 'O' or 'StateAngle'"); if (key == 'O' and _dim == 3 and prior.getType() != "O" and prior.getType() != "StateQuaternion") diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp index 1b18d951b..c3ac836ca 100644 --- a/src/sensor/sensor_odom.cpp +++ b/src/sensor/sensor_odom.cpp @@ -35,22 +35,6 @@ SensorOdom::SensorOdom(const SizeEigen& _dim, { } -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 { double d; // displacement @@ -101,6 +85,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(SensorOdom2d); -WOLF_REGISTER_SENSOR(SensorOdom3d); +WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom2d, SensorOdom); +WOLF_REGISTER_SENSOR_WITH_KEY(SensorOdom3d, SensorOdom); } // namespace wolf diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp index 098855b99..40fcb72e3 100644 --- a/src/state_block/prior.cpp +++ b/src/state_block/prior.cpp @@ -26,6 +26,20 @@ namespace wolf { + +Priors::Priors(const YAML::Node& priors_node) +{ + WOLF_INFO("Priors: \n", priors_node); + if (not priors_node.IsMap()) + { + throw std::runtime_error("Priors: constructor with a non-map yaml node"); + } + for (auto prior_pair : priors_node) + { + this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second)); + } +} + Prior::Prior(const std::string& _type, const Eigen::VectorXd& _state, const std::string& _mode, @@ -42,7 +56,6 @@ Prior::Prior(const YAML::Node& prior_node) type_ = prior_node["type"].as<std::string>(); state_ = prior_node["state"].as<Eigen::VectorXd>(); mode_ = prior_node["mode"].as<std::string>(); - if (mode_ == "factor") noise_std_ = prior_node["noise_std"].as<Eigen::VectorXd>(); else @@ -86,12 +99,12 @@ void Prior::check() const if (state_.size() != sb->getSize()) throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of StateBlock size. " + print()); - // check sigma size + // check factor sigma size if (mode_ == "factor" and noise_std_.size() != sb->getLocalSize()) throw std::runtime_error("Prior::check() Prior " + type_ + " noise_std size different of StateBlock local size. " + print()); - // check sigma size + // check drift sigma size if (dynamic_ and drift_std_.size() > 0 and drift_std_.size() != sb->getLocalSize()) throw std::runtime_error("Prior::check() Prior " + type_ + " drift_std size different of StateBlock local size. " + print()); diff --git a/test/dummy/SensorDummy.schema b/test/dummy/SensorDummy2d.schema similarity index 100% rename from test/dummy/SensorDummy.schema rename to test/dummy/SensorDummy2d.schema diff --git a/test/dummy/SensorDummy3d.schema b/test/dummy/SensorDummy3d.schema new file mode 100644 index 000000000..941cb53d0 --- /dev/null +++ b/test/dummy/SensorDummy3d.schema @@ -0,0 +1,16 @@ +follow: SensorBase.schema +states: + P: + follow: PriorP3d.schema + O: + follow: PriorO3d.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 O. \ No newline at end of file diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema new file mode 100644 index 000000000..ac24447e5 --- /dev/null +++ b/test/dummy/SensorDummyPoia3d.schema @@ -0,0 +1,44 @@ +follow: SensorBase.schema +states: + P: + follow: PriorP3d.schema + O: + follow: PriorO3d.schema + I: + type: + type: string + yaml_type: scalar + mandatory: false + default: StateParam5 + options: [StateParam5] + doc: The derived type of the state in 'I' + state: + type: Vector5d + yaml_type: scalar + mandatory: true + doc: A vector containing the state 'I' values + follow: PriorModeDynamicNoiseDrift.schema + A: + type: + type: string + yaml_type: scalar + mandatory: false + default: StateQuaternion + options: [StateQuaternion] + doc: The derived type of the state in 'A' + state: + type: Vector4d + yaml_type: scalar + mandatory: true + doc: A vector containing the state 'A' values + follow: PriorModeDynamicNoiseDrift.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 o. \ No newline at end of file diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h index 96e8b5777..23ba2a36c 100644 --- a/test/dummy/sensor_dummy.h +++ b/test/dummy/sensor_dummy.h @@ -86,5 +86,8 @@ class SensorDummy : public SensorBase // Register in the FactorySensor #include "core/sensor/factory_sensor.h" namespace wolf { -WOLF_REGISTER_SENSOR(SensorDummy); +WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy2d, SensorDummy); +WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummy3d, SensorDummy); +WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia2d, SensorDummy); +WOLF_REGISTER_SENSOR_WITH_KEY(SensorDummyPoia3d, SensorDummy); } // namespace wolf \ No newline at end of file diff --git a/test/dummy/sensor_dummy_poia.h b/test/dummy/sensor_dummy_poia.h deleted file mode 100644 index f81eca9ef..000000000 --- a/test/dummy/sensor_dummy_poia.h +++ /dev/null @@ -1,87 +0,0 @@ -//--------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-------- -#pragma once - -/************************** - * WOLF includes * - **************************/ -#include "core/sensor/sensor_base.h" -#include "sensor_dummy.h" - -namespace wolf { - -// SpecStates specs_states_dummy_poia({{'I',SpecState("StateBlock", 5, "some doc for state I")}, - // {'A',SpecState("StateQuaternion", 4, "some doc for state A")}}); - -WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummyPoia); - -struct ParamsSensorDummyPoia : public ParamsSensorDummy -{ - ParamsSensorDummyPoia() = default; - ParamsSensorDummyPoia(const YAML::Node& _n): - ParamsSensorDummy(_n) - { - } - ~ParamsSensorDummyPoia() override = default; - std::string print() const override - { - return ParamsSensorDummy::print(); - } -}; - -WOLF_PTR_TYPEDEFS(SensorDummyPoia); - -class SensorDummyPoia : public SensorBase -{ - private: - ParamsSensorDummyPoiaPtr params_dummy_poia_; - SizeEigen dim_; - - public: - SensorDummyPoia(SizeEigen _dim, - ParamsSensorDummyPoiaPtr _params, - const Priors& _priors) : - SensorBase("SensorDummyPoia", - _dim, - _params, - _priors), - params_dummy_poia_(_params) - { - } - WOLF_SENSOR_CREATE(SensorDummyPoia, ParamsSensorDummyPoia); - - virtual ~SensorDummyPoia(){}; - - Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override - { - return (Eigen::Vector2d() << params_dummy_poia_->noise_p_std, - params_dummy_poia_->noise_o_std).finished().cwiseAbs2().asDiagonal(); - } -}; - -} - -// Register in the FactorySensor -#include "core/sensor/factory_sensor.h" -namespace wolf { -WOLF_REGISTER_SENSOR(SensorDummyPoia); -} // namespace wolf diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index e71a0a1e7..fa60723ff 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -66,7 +66,7 @@ TEST(ProcessorBase, MotionProvider) {wolf_root}); auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>(); proc_trk_params->name = "proc tracker"; - auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", sens_trk, proc_trk_params); + auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params); // Install odometer (sensor and processor) auto sens_odo = problem->installSensor("SensorOdom", @@ -75,9 +75,7 @@ TEST(ProcessorBase, MotionProvider) auto proc_odo_params = make_shared<ParamsProcessorOdom2d>(); proc_odo_params->time_tolerance = dt/2; proc_odo_params->name = "odom processor"; - auto proc_odo = problem->installProcessor("ProcessorOdom2d", - sens_odo, - proc_odo_params); + auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params); ASSERT_FALSE(proc_trk->isMotionProvider()); ASSERT_TRUE (proc_odo->isMotionProvider()); @@ -106,7 +104,7 @@ TEST(ProcessorBase, KeyFrameCallback) {wolf_root}); auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>(); proc_trk_params->name = "proc tracker"; - auto proc_trk = problem->installProcessor("ProcessorTrackerFeatureDummy", sens_trk, proc_trk_params); + auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params); // Install odometer (sensor and processor) auto sens_odo = problem->installSensor("SensorOdom", @@ -115,9 +113,7 @@ TEST(ProcessorBase, KeyFrameCallback) auto proc_odo_params = make_shared<ParamsProcessorOdom2d>(); proc_odo_params->time_tolerance = dt/2; proc_odo_params->name = "odom processor"; - auto proc_odo = problem->installProcessor("ProcessorOdom2d", - sens_odo, - proc_odo_params); + auto proc_odo = ProcessorBase::emplace<ProcessorOdom2d>(sens_odo, proc_odo_params); std::cout << "sensor & processor created and added to wolf problem" << std::endl; diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 2154adcb7..0771d6b26 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -23,7 +23,6 @@ #include "core/sensor/sensor_base.h" #include "core/utils/utils_gtest.h" #include "dummy/sensor_dummy.h" -#include "dummy/sensor_dummy_poia.h" #include "yaml-schema-cpp/yaml_server.hpp" using namespace wolf; @@ -326,18 +325,18 @@ TEST(SensorBase, makeshared_priors_POinitial_guess_dynamic3D_drift) // 3D POIA mixed TEST(SensorBase, makeshared_priors_POIA_mixed) { - auto params = std::make_shared<ParamsSensorDummyPoia>(); + auto params = std::make_shared<ParamsSensorDummy>(); 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>(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")}, - {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); + auto S = std::make_shared<SensorDummy>(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")}, + {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); // noise ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); @@ -352,41 +351,6 @@ TEST(SensorBase, makeshared_priors_POIA_mixed) checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0); } -// 3D POIA wrong -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>(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")}, - {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})), - std::runtime_error); - - // missing A - 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")}, - /*{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}*/})), - std::runtime_error); - - // wrong A type (expected StateQuaternion) - 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")}, - {'A',Prior("StateBlock", p_state_3D, "factor", p_std_3D)}})), - std::runtime_error); -} - ////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////// FactorySensor /////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -428,12 +392,15 @@ TEST(SensorBase, factory) WOLF_INFO("Creating sensor from ", name, ".yaml"); - ASSERT_EQ(server.applySchema("SensorDummy"), not wrong); + auto valid = server.applySchema("SensorDummy"+toString(dim)+"d"); + WOLF_WARN_COND(not valid and not wrong, server.getLog().str()); // CORRECT YAML if (not wrong) { - auto S = FactorySensor::create("SensorDummy", dim, server.getNode()); + ASSERT_TRUE(valid); + + auto S = FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode()); auto p_size = dim; auto o_size = dim == 2 ? 1 : 4; @@ -444,8 +411,8 @@ TEST(SensorBase, factory) // noise ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), - noise_cov_dummy, - Constants::EPS); + noise_cov_dummy, + Constants::EPS); // factors ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0); @@ -457,7 +424,11 @@ TEST(SensorBase, factory) // INCORRECT YAML else { - ASSERT_THROW(FactorySensor::create("SensorDummy", dim, server.getNode()),std::runtime_error); + // either is not valid (schema) or it throws an error + if (valid) + { + ASSERT_THROW(FactorySensor::create("SensorDummy"+toString(dim)+"d", dim, server.getNode()),std::runtime_error); + } } } @@ -469,7 +440,7 @@ TEST(SensorBase, factory) YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml"); // create sensor - auto S = FactorySensor::create("SensorDummyPoia", 3, server.getNode()); + auto S = FactorySensor::create("SensorDummyPoia3d", 3, server.getNode()); // noise ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), @@ -493,7 +464,7 @@ TEST(SensorBase, factory) YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml"); // create sensor - ASSERT_THROW(FactorySensor::create("SensorDummyPoia", 3, server.getNode()),std::runtime_error); + ASSERT_THROW(FactorySensor::create("SensorDummyPoia3d", 3, server.getNode()),std::runtime_error); } } @@ -542,7 +513,7 @@ TEST(SensorBase, factory_yaml) // CORRECT YAML if (not wrong) { - auto S = FactorySensorYaml::create("SensorDummy", dim, yaml_filepath, {wolf_root}); + auto S = FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root}); auto p_size = dim; auto o_size = dim == 2 ? 1 : 4; @@ -566,7 +537,7 @@ TEST(SensorBase, factory_yaml) // INCORRECT YAML else { - ASSERT_THROW(FactorySensorYaml::create("SensorDummy", dim, yaml_filepath, {wolf_root}),std::runtime_error); + ASSERT_THROW(FactorySensorYaml::create("SensorDummy"+toString(dim)+"d", dim, yaml_filepath, {wolf_root}),std::runtime_error); } } @@ -575,7 +546,7 @@ TEST(SensorBase, factory_yaml) WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml"); // create sensor - auto S = FactorySensorYaml::create("SensorDummyPoia", + auto S = FactorySensorYaml::create("SensorDummy3d", 3, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml", {wolf_root}); @@ -599,7 +570,7 @@ TEST(SensorBase, factory_yaml) WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml"); // create sensor - ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia", + ASSERT_THROW(FactorySensorYaml::create("SensorDummyPoia3d", 3, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", {wolf_root}), @@ -607,301 +578,6 @@ TEST(SensorBase, factory_yaml) } } -////////////////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// FactorySensorPriors //////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////////////// - -// 2D Fix -TEST(SensorBase, factory_priors_POfix2D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 2, params, - Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic - {'O',Prior("O", o_state_2D)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // checks - checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0); - checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0); -} - -// 3D Fix -TEST(SensorBase, factory_priors_POfix3D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 3, params, - Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic - {'O',Prior("O", o_state_3D)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0); - checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0); -} - -// 2D Initial guess -TEST(SensorBase, factory_priors_POinitial_guess2D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 2, params, - Priors({{'P',Prior("P", p_state_2D, "initial_guess")}, - {'O',Prior("O", o_state_2D, "initial_guess")}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0); - checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0); -} - -// 3D Initial guess -TEST(SensorBase, factory_priors_POinitial_guess3D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 3, params, - Priors({{'P',Prior("P", p_state_3D, "initial_guess")}, - {'O',Prior("O", o_state_3D, "initial_guess")}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0); - checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0); -} - -// 2D Factor -TEST(SensorBase, factory_priors_POfactor2D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 2, params, - Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)}, - {'O',Prior("O", o_state_2D, "factor", o_std_2D)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 2); - - // check - checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0); - checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0); -} - -// 3D Factor -TEST(SensorBase, factory_priors_POfactor3D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 3, params, - Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)}, - {'O',Prior("O", o_state_3D, "factor", o_std_3D)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 2); - - // check - checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0); - checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0); -} - -// 2D Initial guess dynamic -TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 2, params, - Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)}, - {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0); - checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0); -} - -// 3D Initial guess dynamic -TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("SensorDummy", 3, params, - Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)}, - {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0); - checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0); -} - -// 2D Initial guess dynamic drift -TEST(SensorBase, factory_priors_POinitial_guess_dynamic2D_drift) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("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)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D); - checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D); -} - -// 3D Initial guess dynamic drift -TEST(SensorBase, factory_priors_POinitial_guess_dynamic3D_drift) -{ - auto params = std::make_shared<ParamsSensorDummy>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - auto S = FactorySensorPriors::create("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)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D); - checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D); -} - -// 3D POIA mixed -TEST(SensorBase, factory_priors_POIA_mixed) -{ - auto params = std::make_shared<ParamsSensorDummyPoia>(); - params->name = "sensor1"; - params->noise_p_std = noise_p_std; - params->noise_o_std = noise_o_std; - - VectorXd i_state_3D = VectorXd::Random(5); - - auto S = FactorySensorPriors::create("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")}, - {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); - - // noise - ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 2); - - // check - checkSensor(S, 'P', p_state_3D, true, vector0, true, vector0); - checkSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D); - checkSensor(S, 'I', i_state_3D, false, vector0, false, vector0); - checkSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0); -} - -// 3D POIA wrong -TEST(SensorBase, factory_priors_POIA_wrong) -{ - auto params = std::make_shared<ParamsSensorDummyPoia>(); - params->name = "sensor1"; - 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(FactorySensorPriors::create("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")}, - {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})), - std::runtime_error); - - // missing A - ASSERT_THROW(FactorySensorPriors::create("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")}, - /*{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}*/})), - std::runtime_error); - - // wrong A type (expected StateQuaternion) - ASSERT_THROW(FactorySensorPriors::create("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")}, - {'A',Prior("StateBlock", p_state_3D, "factor", p_std_3D)}})), - std::runtime_error); -} - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp index f3349edfb..23e6b4604 100644 --- a/test/gtest_sensor_odom.cpp +++ b/test/gtest_sensor_odom.cpp @@ -316,77 +316,6 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D) checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D); } -////////////////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES //////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST(SensorOdom, makeshared_server) -{ - std::vector<int> dims({2, 3}); - std::vector<std::string> modes({"fix", "initial_guess", "factor"}); - std::vector<bool> dynamics({false, true}); - std::vector<bool> drifts({false, true}); - std::vector<bool> wrongs({false, true}); - - VectorXd p_state(4), o_state(4), po_std(4); - p_state << 1, 2, 3, 4; - o_state << 1, 0, 0, 0; - po_std << 0.1, 0.2, 0.3, 0.4; - - // P & O - for (auto dim : dims) - for (auto mode : modes) - for (auto dynamic : dynamics) - for (auto drift : drifts) - for (auto wrong : wrongs) - { - // nonsense combination - if (not dynamic and drift) - continue; - - std::string name = "sensor_PO_" + - toString(dim) + - "D_" + - mode + - (dynamic ? "_dynamic" : "") + - (drift ? "_drift" : "") + - (wrong ? "_wrong" : ""); - - // Yaml parser - ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true); - ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1"); - - WOLF_INFO("Creating sensor from ", name, ".yaml"); - - // CORRECT YAML - if (not wrong) - { - auto params = std::make_shared<ParamsSensorOdom>("sensor_1", server); - auto S = std::make_shared<SensorOdom>("sensor_1", dim, params, server); - - auto p_size = dim; - auto o_size = dim == 2 ? 1 : 4; - auto p_size_std = mode == "factor" ? dim : 0; - auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0; - auto p_size_std_drift = drift ? dim : 0; - auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0; - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0); - - // check - checkSensor(S, 'P', p_state.head(p_size), mode == "fix", po_std.head(p_size_std), dynamic, po_std.head(p_size_std_drift)); - checkSensor(S, 'O', o_state.head(o_size), mode == "fix", po_std.head(o_size_std), dynamic, po_std.head(o_size_std_drift)); - } - // INCORRECT YAML - else - { - ASSERT_THROW(std::make_shared<SensorOdom>("sensor_1", dim, - std::make_shared<ParamsSensorOdom>("sensor_1", server), - server),std::runtime_error); - } - } -} - ////////////////////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////// FactorySensor /////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -521,227 +450,6 @@ TEST(SensorOdom, factory_yaml) } } -////////////////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// FactorySensorPriors //////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////////////////// - -// 2D Fix -TEST(SensorOdom, factory_priors_fix_2D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, - Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic - {'O',Prior("O", o_state_2D)}})); - - // checks - checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0); - checkSensor(S, 'O', o_state_2D, true, vector0, false, vector0); -} - -// 3D Fix -TEST(SensorOdom, factory_priors_fix_3D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, - Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic - {'O',Prior("O", o_state_3D)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, true, vector0, false, vector0); - checkSensor(S, 'O', o_state_3D, true, vector0, false, vector0); -} - -// 2D Initial guess -TEST(SensorOdom, factory_priors_initial_guess_2D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, - Priors({{'P',Prior("P", p_state_2D, "initial_guess")}, - {'O',Prior("O", o_state_2D, "initial_guess")}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_2D, false, vector0, false, vector0); - checkSensor(S, 'O', o_state_2D, false, vector0, false, vector0); -} - -// 3D Initial guess -TEST(SensorOdom, factory_priors_initial_guess_3D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, - Priors({{'P',Prior("P", p_state_3D, "initial_guess")}, - {'O',Prior("O", o_state_3D, "initial_guess")}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, false, vector0, false, vector0); - checkSensor(S, 'O', o_state_3D, false, vector0, false, vector0); -} - -// 2D Factor -TEST(SensorOdom, factory_priors_factor_2D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, - Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)}, - {'O',Prior("O", o_state_2D, "factor", o_std_2D)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 2); - - // check - checkSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0); - checkSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0); -} - -// 3D Factor -TEST(SensorOdom, factory_priors_factor_3D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, - Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)}, - {'O',Prior("O", o_state_3D, "factor", o_std_3D)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 2); - - // check - checkSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0); - checkSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0); -} - -// 2D Initial guess dynamic -TEST(SensorOdom, factory_priors_initial_guess_dynamic_2D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 2, params, - Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)}, - {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_2D, false, vector0, true, vector0); - checkSensor(S, 'O', o_state_2D, false, vector0, true, vector0); -} - -// 3D Initial guess dynamic -TEST(SensorOdom, factory_priors_initial_guess_dynamic_3D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params, - Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)}, - {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, false, vector0, true, vector0); - checkSensor(S, 'O', o_state_3D, false, vector0, true, vector0); -} - -// 2D Initial guess dynamic drift -TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_2D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 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)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D); - checkSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D); -} - -// 3D Initial guess dynamic drift -TEST(SensorOdom, factory_priors_initial_guess_dynamic_drift_3D) -{ - auto params = std::make_shared<ParamsSensorOdom>(); - params->k_disp_to_disp = k_disp_to_disp; - params->k_disp_to_rot = k_disp_to_rot; - params->k_rot_to_rot = k_rot_to_rot; - params->min_disp_var = min_disp_var; - params->min_rot_var = min_rot_var; - - auto S = FactorySensorPriors::create("SensorOdom","sensor1", 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)}})); - - // factors - ASSERT_EQ(S->getPriorFeatures().size(), 0); - - // check - checkSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D); - checkSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D); -} - ////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////// COMPUTE NOISE COV ///////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index 0c3f031eb..463b82987 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -44,22 +44,6 @@ TEST(TreeManager, make_shared) ASSERT_EQ(P->getTreeManager(), GM); } -TEST(TreeManager, createParams) -{ - ProblemPtr P = Problem::create("PO", 2); - - auto ParamsGM = std::make_shared<ParamsTreeManagerBase>(); - - auto GM = TreeManagerDummy::create(ParamsGM); - - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr); - - P->setTreeManager(GM); - - ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0); - ASSERT_EQ(P->getTreeManager(), GM); -} - TEST(TreeManager, createNode) { ProblemPtr P = Problem::create("PO", 2); @@ -92,22 +76,6 @@ TEST(TreeManager, createYaml) ASSERT_EQ(P->getTreeManager(), GM); } -TEST(TreeManager, FactoryParams) -{ - ProblemPtr P = Problem::create("PO", 2); - - auto ParamsGM = std::make_shared<ParamsTreeManagerBase>(); - - auto GM = FactoryTreeManagerParams::create("TreeManagerDummy",ParamsGM); - - ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr); - - P->setTreeManager(GM); - - ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0); - ASSERT_EQ(P->getTreeManager(), GM); -} - TEST(TreeManager, FactoryParam) { ProblemPtr P = Problem::create("PO", 2); diff --git a/test/yaml/sensor_tests/sensor_POIA_3D.yaml b/test/yaml/sensor_tests/sensor_POIA_3D.yaml index abe471720..b2530ce9a 100644 --- a/test/yaml/sensor_tests/sensor_POIA_3D.yaml +++ b/test/yaml/sensor_tests/sensor_POIA_3D.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_POIA_3D: states: P: @@ -21,8 +23,6 @@ states: dynamic: true drift_std: [0.1, 0.2, 0.3] - - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml index 5cf6bffb4..9eec9a201 100644 --- a/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_POIA_3D_wrong: states: P: @@ -21,7 +23,6 @@ states: # dynamic: true # drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml index 9bfc63bcd..96c51ec98 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_factor.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_factor: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml index ce4287ed5..aa801e760 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_factor_dynamic: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml index d4c05d48d..4c703e7da 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_factor_dynamic_drift: states: P: diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml index a6787204c..a897b3566 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_drift_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_factor_dynamic_drift_wrong: states: P: diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml index c66c96442..0f6c0630f 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_dynamic_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_factor_dynamic_wrong: states: P: diff --git a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml index b99691f90..1a6591ae9 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_factor_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_factor_wrong: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml index e96ee1849..296fd0bc3 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_fix: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml index 715da540e..74f8fd2fb 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_fix_dynamic: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml index 4e6680029..2fcf456a4 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_fix_dynamic_drift: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml index 0bcee886c..0d257867a 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_drift_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_fix_dynamic_drift_wrong: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml index 1c9d81679..381414ba6 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_dynamic_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_fix_dynamic_wrong: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml index 229f89094..8f7b6cd33 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_fix_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_fix_wrong: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml index f593fc53c..bffef6ad2 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_initial_guess: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml index 50705368a..0ceb01c8a 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_initial_guess_dynamic: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml index d5060eeaf..faeceb548 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_initial_guess_dynamic_drift: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml index 9a53f8300..615a03f76 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_initial_guess_dynamic_drift_wrong: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml index 9b3da538f..0f93bc7ba 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_dynamic_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_initial_guess_dynamic_wrong: states: # P: #missing @@ -9,7 +11,6 @@ states: state: [1] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml index 65d170426..d2a1cd57a 100644 --- a/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_2D_initial_guess_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_2D_initial_guess_wrong: states: P: @@ -9,7 +11,6 @@ states: state: [1] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml index 23737df98..d1b61af3b 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_factor.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_factor: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1, 0.2, 0.3] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml index 8c9342ab8..ccd18fe7a 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_factor_dynamic: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1, 0.2, 0.3] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml index 9f7e14d04..c6840055a 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_factor_dynamic_drift: states: P: @@ -13,7 +15,6 @@ states: dynamic: true drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml index a4980b27d..28c0b1745 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_drift_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_factor_dynamic_drift_wrong: states: P: @@ -13,7 +15,6 @@ states: dynamic: true drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml index 104237208..ec9ab37c1 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_dynamic_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_factor_dynamic_wrong: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml index d99e58074..f8440b8c7 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_factor_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_factor_wrong: states: P: @@ -11,7 +13,6 @@ states: noise_std: [0.1, 0.2, 0.3] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml index 44eef97f1..cd91be67d 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_fix.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_fix: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 0] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml index 606751a7b..52ea8fc40 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_fix_dynamic: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 0] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml index f6166c91b..4864053e6 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_fix_dynamic_drift: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml index 17467074e..3536ded54 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_drift_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_fix_dynamic_drift_wrong: states: P: @@ -11,7 +13,6 @@ states: #dynamic: true #missing drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml index 4e6da9d21..f7cd8d862 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_dynamic_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_fix_dynamic_wrong: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 1] # not normalized dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml index acc563566..23aae62a0 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_fix_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_fix_wrong: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 0] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 # noise_o_std: 0.01 #missing diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml index 7b5e1398d..86488c0b2 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_initial_guess: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 0] dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml index e758e8703..1a0c2d774 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_initial_guess_dynamic: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 0] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml index 1de0b8d93..1991d7089 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_initial_guess_dynamic_drift: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml index 5322cd09f..642749806 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_initial_guess_dynamic_drift_wrong: states: P: @@ -11,7 +13,6 @@ states: dynamic: true drift_std: [0.1, 0.2, 0.3] - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml index 873cf7425..ee02d6607 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_dynamic_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_initial_guess_dynamic_wrong: states: P: @@ -9,7 +11,6 @@ states: state: [1, 0, 0, 0] dynamic: true - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 diff --git a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml index 5eed19d62..8d74c207d 100644 --- a/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml +++ b/test/yaml/sensor_tests/sensor_PO_3D_initial_guess_wrong.yaml @@ -1,3 +1,5 @@ +name: sensor_1 + # sensor_PO_3D_initial_guess_wrong: states: P: @@ -9,7 +11,6 @@ states: # state: [1, 0, 0, 0] # dynamic: false - # used in gtest_sensor_base noise_p_std: 0.1 noise_o_std: 0.01 -- GitLab