diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h index 3a77700e87d23ffdc58418046c6da4f573d55d93..d51b407e3e33ba37ab087bd12373c68bba6f51a3 100644 --- a/include/core/sensor/factory_sensor.h +++ b/include/core/sensor/factory_sensor.h @@ -69,27 +69,21 @@ namespace wolf * Sensor creators have the following API: * * \code - * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor); + * static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server); * \endcode * * They follow the general implementation shown below: * * \code - * static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _params_extrinsics, ParamsSensorBasePtr _params_sensor) + * static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server) * { - * // check extrinsics vector --- example: 3D pose - * assert(_params_extrinsics.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d."); - * - * // cast sensor parameters to good type --- example: ParamsSensorCamera - * auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_params_sensor); - * + * // Do create the Sensor Parameters --- example: ParamsSensorCamera + * auto params = std::make_shared<ParamsSensorCamera>(_unique_name, _server); + * * // Do create the Sensor object --- example: SensorCamera - * auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr); - * - * // Complete the sensor setup with a unique name identifying the sensor - * sen_ptr->setName(_unique_name); - * - * return sen_ptr; + * auto sensor = std::make_shared<SensorCamera>(_unique_name, _dim, params, _server); + * + * return sensor; * } * \endcode * @@ -97,42 +91,27 @@ namespace wolf * Note: Prior to invoking the creation of a sensor of a particular type, * you must register the creator for this type into the factory. * - * To create e.g. a SensorCamera, you type: + * To create e.g. a SensorCamera in 3D, you type: * * \code - * auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", params_extrinsics, params_camera); + * auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", 3, param_server); * \endcode * * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS! * - * #### See also - * - FactoryParamsSensor: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files. - * - FactoryProcessor: to create processors that will be bound to sensors. - * - Problem::installSensor() : to install sensors in WOLF Problem. - * - * #### Example 1: writing a SensorCamera creator - * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp: - * + * We RECOMMEND using the macro WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) to automatically add the sensor creator, provided in 'sensor_base.h'. + * + * To do so, you should implement a constructor with the API: + * * \code - * static SensorBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics) - * { - * // check extrinsics vector - * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d."); - * - * // cast instrinsics to good type - * auto intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_intrinsics); - * - * // Do create the SensorCamera object, and complete its setup - * auto sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr); - * - * sen_ptr->setName(_unique_name); - * - * return sen_ptr; - * } + * SensorDerived(const std::string& _unique_name, + * SizeEigen _dim, + * ParamsSensorDummyPtr _params, + * const ParamsServer& _server) * \endcode * - * #### Example 2: registering a sensor creator into the factory + * #### Registering sensor creator into the factory * Registration can be done manually or automatically. It involves the call to static functions. * It is advisable to put these calls within unnamed namespaces. * @@ -159,7 +138,7 @@ namespace wolf * const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create); * } * \endcode - * Automatic registration is recommended in wolf, and implemented in the classes shipped with it. + * Automatic registration is recommended in wolf, and implemented in the classes shipped with it using the macro WOLF_REGISTER_SENSOR(SensorType). * You are free to comment out these lines and place them wherever you consider it more convenient for your design. * * #### Example 2: creating sensors @@ -177,15 +156,8 @@ namespace wolf * // To create a camera, provide: * // a type = "SensorCamera", * // a name = "Front-left camera", - * // an extrinsics vector, and - * // a pointer to the intrinsics struct: - * - * Eigen::VectorXd extrinsics_1(7); // give it some values... - * - * // Create a pointer to the struct of sensor parameters stored in a YAML file ( see FactoryParamsSensor ) - * ParamsSensorCameraPtr intrinsics_1 = - * FactoryParamsSensor::create("ParamsSensorCamera", - * camera_1.yaml); + * // the problem dimension dim = 3, and + * // the parameter server or a yaml file * * SensorBasePtr camera_1_ptr = * FactorySensor::create ( "SensorCamera" , @@ -211,47 +183,34 @@ namespace wolf * } * \endcode * + * #### See also + * - FactoryProcessor: to create processors that will be bound to sensors. + * - Problem::installSensor() : to install sensors in WOLF Problem. */ + typedef Factory<SensorBase, const std::string&, - const Eigen::VectorXd&, - const ParamsSensorBasePtr> FactorySensor; - -template<> -inline std::string FactorySensor::getClass() const -{ - return "FactorySensor"; -} - -// ParamsSensor factory -struct ParamsSensorBase; -typedef Factory<ParamsSensorBase, - const std::string&> FactoryParamsSensor; -template<> -inline std::string FactoryParamsSensor::getClass() const -{ - return "FactoryParamsSensor"; -} - -#define WOLF_REGISTER_SENSOR(SensorType) \ - namespace{ const bool WOLF_UNUSED SensorType##Registered = \ - FactorySensor::registerCreator(#SensorType, SensorType::create); } \ - + SizeEigen, + const ParamsServer&> FactorySensor; typedef Factory<SensorBase, const std::string&, - const ParamsServer&> AutoConfFactorySensor; + SizeEigen, + const std::string&> FactorySensorYaml; template<> -inline std::string AutoConfFactorySensor::getClass() const +inline std::string FactorySensor::getClass() const { - return "AutoConfFactorySensor"; + return "FactorySensor"; } -#define WOLF_REGISTER_SENSOR_AUTO(SensorType) \ - namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered = \ - AutoConfFactorySensor::registerCreator(#SensorType, SensorType::create); } \ +#define WOLF_REGISTER_SENSOR(SensorType) \ + namespace{ const bool WOLF_UNUSED SensorType##Registered = \ + FactorySensor::registerCreator(#SensorType, SensorType::create); } \ +#define WOLF_REGISTER_SENSOR_YAML(SensorType) \ + namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered = \ + FactorySensorYaml::registerCreator(#SensorType, SensorType::create); } \ } /* namespace wolf */ diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 1157e55c2a5613a3bf3e43fdc5bcfca4fc275103..cf693f57c9b8b3fa8de09299d01c7b69ddbe968c 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -37,6 +37,7 @@ class ParamsServer; #include "core/common/time_stamp.h" #include "core/common/params_base.h" #include "core/state_block/has_state_blocks.h" +#include "core/yaml/parser_yaml.h" //std includes @@ -51,54 +52,49 @@ namespace wolf { * In order to use this macro, the derived sensor class, SensorClass, * must have a constructor available with the API: * - * SensorClass(const VectorXd & _extrinsics, const ParamsSensorClassPtr _intrinsics); + * SensorClass(const std::string& _unique_name, + * SizeEigen _dim, + * const ParamsServer& _server, + * ParamsSensorClassPtr _params) * - * We recommend writing one of such constructors in your derived sensors. */ -#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize) \ -static \ -SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \ -{ \ - Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \ - \ - assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ - \ - auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \ - \ - auto sensor = std::make_shared<SensorClass>(extrinsics, params); \ - \ - sensor ->setName(_unique_name); \ - \ - return sensor; \ -} \ - \ -static \ -SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\ -{ \ - assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ - \ - auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \ - \ - auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \ - \ - sensor ->setName(_unique_name); \ - \ - return sensor; \ -} \ - +#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \ +static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server) \ +{ \ + auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \ + \ + auto sensor = std::make_shared<SensorClass>(_unique_name, _dim, params, _server); \ + \ + return sensor; \ +} \ +static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const std::string& _yaml_file) \ +{ \ + auto parser = ParserYaml(_yaml_file, "", true); \ + \ + auto server = ParamsServer(parser.getParams()); \ + \ + auto params = std::make_shared<ParamsSensorClass>(_unique_name, server); \ + \ + auto sensor = std::make_shared<SensorClass>(_unique_name, _dim, params, server); \ + \ + return sensor; \ +} \ /** \brief base struct for intrinsic sensor parameters * * Derive from this struct to create structs of sensor intrinsic parameters. */ +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase); struct ParamsSensorBase: public ParamsBase { + std::string prefix = "sensor/"; + Eigen::VectorXd noise_std; ParamsSensorBase() = default; - ParamsSensorBase(std::string prefix, const wolf::ParamsServer& _server) + ParamsSensorBase(std::string _unique_name, const wolf::ParamsServer& _server) { - noise_std = _server.getParam<Eigen::VectorXd>(prefix + "/noise_std"); + noise_std = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/noise_std"); } ~ParamsSensorBase() override = default; @@ -108,6 +104,7 @@ struct ParamsSensorBase: public ParamsBase } }; +WOLF_PTR_TYPEDEFS(SensorBase); class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase> { friend Problem; @@ -121,6 +118,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh unsigned int sensor_id_; // sensor ID + ParamsSensorBasePtr params_; ///< Params struct + std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_) @@ -128,11 +127,9 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree) protected: - Eigen::MatrixXd noise_cov_; // cov matrix of noise std::map<char, Eigen::MatrixXd> drift_covs_; // covariance of the drift of dynamic state blocks [unit²/s] void setProblem(ProblemPtr _problem) override final; - virtual void loadParams(ParamsSensorBasePtr _params); virtual void loadPriors(const Priors& _priors, SizeEigen _dim); public: @@ -143,15 +140,15 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh * \param _tp Type of the sensor (types defined at wolf.h) * \param _unique_name Name of the sensor * \param _dim Problem dimension - * \param _priors priors of the sensor state blocks * \param _params params struct + * \param _priors priors of the sensor state blocks * **/ SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, - const Priors& _priors, - ParamsSensorBasePtr _params); + ParamsSensorBasePtr _params, + const Priors& _priors); /** \brief Constructor with ParamServer and keys * @@ -160,6 +157,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh * \param _tp Type of the sensor (types defined at wolf.h) * \param _unique_name Name of the sensor * \param _dim Problem dimension + * \param _params params struct * \param _server parameter server * \param _keys_types_apart_from_PO Map containing the keys and the types of the state blocks (apart from extrinsics: P, O) * @@ -167,6 +165,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, + ParamsSensorBasePtr _params, const ParamsServer& _server, const std::unordered_map<char, std::string>& _keys_types_apart_from_PO={}); @@ -364,12 +363,14 @@ inline bool SensorBase::isStateBlockDynamic(const char& _key) const inline Eigen::VectorXd SensorBase::getNoiseStd() const { - return noise_cov_.diagonal().cwiseSqrt(); + assert(params_ != nullptr); + return params_->noise_std; } inline Eigen::MatrixXd SensorBase::getNoiseCov() const { - return noise_cov_; + assert(params_ != nullptr); + return params_->noise_std.cwiseAbs2().asDiagonal(); } inline Eigen::VectorXd SensorBase::getDriftStd(char _key) const diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h index a734043ce8a94776831d3ed43819d0c321935e16..c06fc2ba6c430232b0bd34ee727e1fffc44faed6 100644 --- a/include/core/sensor/sensor_odom_2d.h +++ b/include/core/sensor/sensor_odom_2d.h @@ -32,14 +32,11 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d); struct ParamsSensorOdom2d : public ParamsSensorBase { - ~ParamsSensorOdom2d() override = default; + ~ParamsSensorOdom2d() override = default; - double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation - double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation - ParamsSensorOdom2d() - { - //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. - } + double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation + double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation + ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server): ParamsSensorBase(_unique_name, _server) { @@ -63,9 +60,15 @@ class SensorOdom2d : public SensorBase double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation public: - SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics); - SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics); - WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d, 3); + SensorOdom2d(const std::string& _unique_name, + const SizeEigen& _dim, + const Priors& _priors, + ParamsSensorOdom2d _params); + SensorOdom2d(const std::string& _unique_name, + const SizeEigen& _dim, + const ParamsServer& _server, + ParamsSensorOdom2d _params); + WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d); ~SensorOdom2d() override; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 604770acc4629ba97125e1da097d6e5871083983..dc064e6b7d6b234132e2ece6127246811d23aa7e 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -214,9 +214,10 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, // const Eigen::VectorXd& _extrinsics, // ParamsSensorBasePtr _intrinsics) { - SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics); - sen_ptr->link(getHardware()); - return sen_ptr; + //SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics); + // sen_ptr->link(getHardware()); + // return sen_ptr; + return nullptr; } SensorBasePtr Problem::installSensor(const std::string& _sen_type, // @@ -228,8 +229,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, // if (_intrinsics_filename != "") { assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist."); - ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename); - return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr); + //ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename); + return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr()); } else return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr()); @@ -240,7 +241,8 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, // const std::string& _unique_sensor_name, // const ParamsServer& _server) { - SensorBasePtr sen_ptr = AutoConfFactorySensor::create(_sen_type, _unique_sensor_name, _server); + //ParamsSensorBasePtr params = FactoryParamsSensor::create(_sen_type, _unique_sensor_name, _server); + SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, dim_, _server); sen_ptr->link(getHardware()); return sen_ptr; } diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index ecd199c2ef098d4e942dc777018226938b30432e..47fc91f406d062fd3da00df7322d79f76a36fb51 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -36,22 +36,19 @@ unsigned int SensorBase::sensor_id_count_ = 0; SensorBase::SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, - const Priors& _priors, - ParamsSensorBasePtr _params) : + ParamsSensorBasePtr _params, + const Priors& _priors) : NodeBase("SENSOR", _type, _unique_name), HasStateBlocks(""), hardware_ptr_(), sensor_id_(++sensor_id_count_), // simple ID factory + params_(_params), state_block_dynamic_(), params_prior_map_(), - last_capture_(nullptr), - noise_cov_(0,0) + last_capture_(nullptr) { assert(_dim == 2 or _dim == 3); - // load params - loadParams(_params); - // load priors loadPriors(_priors, _dim); } @@ -59,12 +56,14 @@ SensorBase::SensorBase(const std::string& _type, SensorBase::SensorBase(const std::string& _type, const std::string& _unique_name, const SizeEigen& _dim, + ParamsSensorBasePtr _params, const ParamsServer& _server, const std::unordered_map<char, std::string>& _keys_types_apart_from_PO) : NodeBase("SENSOR", _type, _unique_name), HasStateBlocks(""), hardware_ptr_(), sensor_id_(++sensor_id_count_), // simple ID factory + params_(_params), state_block_dynamic_(), params_prior_map_(), last_capture_(nullptr) @@ -74,19 +73,13 @@ SensorBase::SensorBase(const std::string& _type, _keys_types_apart_from_PO.count('O') == 0 and "SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys"); - // create params - auto params = std::make_shared<ParamsSensorBase>("sensor/" + _unique_name, _server); - - // load params - loadParams(params); - // create priors std::unordered_map<char, std::string> keys_types = _keys_types_apart_from_PO; keys_types['P'] = "P"; // equivalent to "StateBlock"; keys_types['O'] = "O"; // equivalent to (_dim == 2 ? "StateAngle" : "StateQuaternion"); Priors priors; for (auto pair : keys_types) - priors[pair.first] = Prior("sensor/" + _unique_name + "/" + std::string(1, pair.first), pair.second, _server); + priors[pair.first] = Prior("sensor/" + _unique_name + "/states/" + std::string(1, pair.first), pair.second, _server); // load priors loadPriors(priors, _dim); @@ -98,11 +91,6 @@ SensorBase::~SensorBase() removeStateBlocks(getProblem()); } -void SensorBase::loadParams(ParamsSensorBasePtr _params) -{ - setNoiseStd(_params->noise_std); -} - void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim) { assert(_dim == 2 or _dim == 3); @@ -286,12 +274,14 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem) void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) { - noise_cov_ = _noise_std.cwiseAbs2().asDiagonal(); + assert(params_ != nullptr); + params_->noise_std = _noise_std; } void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) { - noise_cov_ = _noise_cov; + assert(params_ != nullptr); + params_->noise_std = _noise_cov.diagonal().cwiseSqrt(); } void SensorBase::setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std) diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h new file mode 100644 index 0000000000000000000000000000000000000000..8b37639c862202dab8014f3f4ad2ef472ee3ad3b --- /dev/null +++ b/test/dummy/sensor_dummy.h @@ -0,0 +1,98 @@ +//--------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-------- +#ifndef SENSOR_DUMMY_H_ +#define SENSOR_DUMMY_H_ + +/************************** + * WOLF includes * + **************************/ +#include "core/sensor/sensor_base.h" + +namespace wolf { + +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDummy); + +struct ParamsSensorDummy : public ParamsSensorBase +{ + ~ParamsSensorDummy() override = default; + + double param1; + int param2; + + ParamsSensorDummy(std::string _unique_name, const ParamsServer& _server): + ParamsSensorBase(_unique_name, _server) + { + param1 = _server.getParam<double>(prefix + _unique_name + "/param1"); + param2 = _server.getParam<int>(prefix + _unique_name + "/param2"); + } + std::string print() const override + { + return ParamsSensorBase::print() + "\n" + + "param1: " + std::to_string(param1) + "\n" + + "param2: " + std::to_string(param2) + "\n"; + } +}; + +WOLF_PTR_TYPEDEFS(SensorDummy); + +class SensorDummy : public SensorBase +{ + public: + SensorDummy(const std::string& _unique_name, + SizeEigen _dim, + ParamsSensorDummyPtr _params, + const ParamsServer& _server) : + SensorBase("SensorDummy", + _unique_name, + _dim, + _params, + _server, + std::unordered_map<char, std::string>{{'I',"StateBlock"},{'A',"StateQuaternion"}}) + { + } + WOLF_SENSOR_CREATE(SensorDummy, ParamsSensorDummy); + + + SensorDummy(const std::string& _unique_name, + SizeEigen _dim, + ParamsSensorDummyPtr _params, + const Priors& _priors) : + SensorBase("SensorDummy", + _unique_name, + _dim, + _params, + _priors) + { + } + + virtual ~SensorDummy(){}; +}; + +} + +// Register in the FactorySensor +#include "core/sensor/factory_sensor.h" +namespace wolf { +WOLF_REGISTER_SENSOR(SensorDummy); +WOLF_REGISTER_SENSOR_YAML(SensorDummy); +} // namespace wolf +#endif diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 1b98fed3c06c6f08977be4305116170bae8ebf5b..6d7e6bcab14701fcc3669cfb9b62ff0baaff2189 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -30,6 +30,7 @@ #include "core/utils/utils_gtest.h" #include "core/yaml/parser_yaml.h" #include "core/utils/params_server.h" +#include "dummy/sensor_dummy.h" using namespace wolf; using namespace Eigen; @@ -103,10 +104,9 @@ TEST(SensorBase, POfix2D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic - {'O',Prior("O", o_state_2D)}}), - params); + {'O',Prior("O", o_state_2D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -123,10 +123,9 @@ TEST(SensorBase, POfix3D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic - {'O',Prior("O", o_state_3D)}}), - params); + {'O',Prior("O", o_state_3D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -146,10 +145,9 @@ TEST(SensorBase, POinitial_guess2D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, Priors({{'P',Prior("P", p_state_2D, "initial_guess")}, - {'O',Prior("O", o_state_2D, "initial_guess")}}), - params); + {'O',Prior("O", o_state_2D, "initial_guess")}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -169,10 +167,9 @@ TEST(SensorBase, POinitial_guess3D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, Priors({{'P',Prior("P", p_state_3D, "initial_guess")}, - {'O',Prior("O", o_state_3D, "initial_guess")}}), - params); + {'O',Prior("O", o_state_3D, "initial_guess")}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -192,10 +189,9 @@ TEST(SensorBase, POfactor2D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)}, - {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}), - params); + {'O',Prior("O", o_state_2D, "factor", o_std_2D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -215,10 +211,9 @@ TEST(SensorBase, POfactor3D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)}, - {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}), - params); + {'O',Prior("O", o_state_3D, "factor", o_std_3D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -238,10 +233,9 @@ TEST(SensorBase, POinitial_guess_dynamic2D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params, Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)}, - {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}), - params); + {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -261,10 +255,9 @@ TEST(SensorBase, POinitial_guess_dynamic3D) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params, Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)}, - {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}), - params); + {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -284,10 +277,9 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, + auto S = std::make_shared<SensorBase>("SensorBase", "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)}}), - params); + {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -307,10 +299,9 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift) auto params = std::make_shared<ParamsSensorBase>(); params->noise_std = noise_std; - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, + auto S = std::make_shared<SensorBase>("SensorBase", "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)}}), - params); + {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -332,12 +323,11 @@ TEST(SensorBase, POI_mixed) VectorXd i_state_3D = VectorXd::Random(5); - auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, + auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 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)}}), - params); + {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); // noise ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); @@ -384,7 +374,8 @@ TEST(SensorBase, POfix2D_server) std::string name = "sensor_PO_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : ""); WOLF_INFO("Creating sensor from name ", name); - auto S = std::make_shared<SensorBase>("SensorBase", name, dim, server); + auto params = std::make_shared<ParamsSensorBase>(name, server); + auto S = std::make_shared<SensorBase>("SensorBase", name, dim, params, server); auto p_size = dim; auto o_size = dim == 2 ? 1 : 4; @@ -407,17 +398,16 @@ TEST(SensorBase, POfix2D_server) // INCORRECT YAML WOLF_INFO("Creating sensor from name ", name + "_wrong"); - ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim, server),std::runtime_error); + ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim, + std::make_shared<ParamsSensorBase>(name + "_wrong", server), + server),std::runtime_error); } - // P & O & I & A - 3D - // CORRECT YAML + // POIA - 3D - CORRECT YAML WOLF_INFO("Creating sensor from name sensor_POIA_3D"); - auto S = std::make_shared<SensorBase>("SensorBase", - "sensor_POIA_3D", - 3, - server, + auto params = std::make_shared<ParamsSensorBase>("sensor_POIA_3D", server); + auto S = std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D", 3, params, server, std::unordered_map<char, std::string>({{'I',"StateBlock"}, {'A',"StateQuaternion"}})); @@ -433,6 +423,125 @@ TEST(SensorBase, POfix2D_server) testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0); testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4)); testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3)); + + // POIA - 3D - INCORRECT YAML + WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong"); + + ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D_wrong", 3, + std::make_shared<ParamsSensorBase>("sensor_POIA_3D_wrong", server), + server, + std::unordered_map<char, std::string>({{'I',"StateBlock"}, + {'A',"StateQuaternion"}})), + std::runtime_error); +} + +// DUMMY CLASS +TEST(SensorBase, dummy_make_shared) +{ + ParserYaml parser = ParserYaml("test/yaml/sensor_dummy.yaml", wolf_root, true); + ParamsServer server = ParamsServer(parser.getParams()); + + 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; + + // POIA - 3D - CORRECT YAML + WOLF_INFO("Creating sensor from name sensor_dummy_1"); + + auto params = std::make_shared<ParamsSensorDummy>("sensor_dummy_1", server); + auto S = std::make_shared<SensorDummy>("sensor_dummy_1", 3, params, server); + + // noise + ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS); + ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS); + + // factors + ASSERT_EQ(S->getPriorFeatures().size(), 2); + + // check + testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0); + testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0); + testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4)); + testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3)); + + + // POIA - 3D - INCORRECT YAML + WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong"); + + ASSERT_THROW(std::make_shared<SensorDummy>("sensor_dummy_1_wrong", 3, + std::make_shared<ParamsSensorDummy>("sensor_dummy_1_wrong", server), + server), + std::runtime_error); +} + +TEST(SensorBase, dummy_factory) +{ + 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; + + ParserYaml parser = ParserYaml("test/yaml/sensor_dummy.yaml", wolf_root, true); + ParamsServer server = ParamsServer(parser.getParams()); + + // POIA - 3D - CORRECT YAML + WOLF_INFO("Creating sensor from name sensor_dummy_1"); + + auto S = FactorySensor::create("SensorDummy", "sensor_dummy_1", 3, server); + + // noise + ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS); + ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS); + + // factors + ASSERT_EQ(S->getPriorFeatures().size(), 2); + + // check + testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0); + testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0); + testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4)); + testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3)); + + + // POIA - 3D - INCORRECT YAML + WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong"); + + ASSERT_THROW(FactorySensor::create("SensorDummy", "sensor_dummy_1_wrong", 3, server), + std::runtime_error); +} + +TEST(SensorBase, dummy_factory_yaml) +{ + 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; + + // POIA - 3D - CORRECT YAML + WOLF_INFO("Creating sensor from name sensor_dummy_1"); + + auto S = FactorySensorYaml::create("SensorDummy", "sensor_dummy_1", 3, wolf_root + "/test/yaml/sensor_dummy.yaml"); + + // noise + ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS); + ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS); + + // factors + ASSERT_EQ(S->getPriorFeatures().size(), 2); + + // check + testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0); + testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0); + testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4)); + testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3)); + + + // POIA - 3D - INCORRECT YAML + WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong"); + + ASSERT_THROW(FactorySensorYaml::create("SensorDummy", "sensor_dummy_1_wrong", 3, wolf_root + "/test/yaml/sensor_dummy.yaml"), + std::runtime_error); } int main(int argc, char **argv) diff --git a/test/yaml/sensor_base.yaml b/test/yaml/sensor_base.yaml index 469dbd637fc87f3b381d6e339f87eb13c55852e8..11e3bbe82ce94ea8d48866cb72ca7fb25b78f052 100644 --- a/test/yaml/sensor_base.yaml +++ b/test/yaml/sensor_base.yaml @@ -5,248 +5,267 @@ sensor: ############################################################################################# sensor_PO_2D_fix: - P: - mode: fix - state: [1, 2] - dynamic: false - O: - mode: fix - state: [1] - dynamic: false + states: + P: + mode: fix + state: [1, 2] + dynamic: false + O: + mode: fix + state: [1] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_3D_fix: - P: - mode: fix - state: [1, 2, 3] - dynamic: false - O: - mode: fix - state: [1, 0, 0, 0] - dynamic: false + states: + P: + mode: fix + state: [1, 2, 3] + dynamic: false + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_2D_initial_guess: - P: - mode: initial_guess - state: [1, 2] - dynamic: false - O: - mode: initial_guess - state: [1] - dynamic: false + states: + P: + mode: initial_guess + state: [1, 2] + dynamic: false + O: + mode: initial_guess + state: [1] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_3D_initial_guess: - P: - mode: initial_guess - state: [1, 2, 3] - dynamic: false - O: - mode: initial_guess - state: [1, 0, 0, 0] - dynamic: false + states: + P: + mode: initial_guess + state: [1, 2, 3] + dynamic: false + O: + mode: initial_guess + state: [1, 0, 0, 0] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_2D_factor: - P: - mode: factor - state: [1, 2] - noise_std: [0.1, 0.2] - dynamic: false - O: - mode: factor - state: [1] - noise_std: [0.1] - dynamic: false + states: + P: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2] + dynamic: false + O: + mode: factor + state: [1] + noise_std: [0.1] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_3D_factor: - P: - mode: factor - state: [1, 2, 3] - noise_std: [0.1, 0.2, 0.3] - dynamic: false - O: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3] - dynamic: false + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: false + O: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_2D_fix_dynamic: - P: - mode: fix - state: [1, 2] - dynamic: true - O: - mode: fix - state: [1] - dynamic: true + states: + P: + mode: fix + state: [1, 2] + dynamic: true + O: + mode: fix + state: [1] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_3D_fix_dynamic: - P: - mode: fix - state: [1, 2, 3] - dynamic: true - O: - mode: fix - state: [1, 0, 0, 0] - dynamic: true + states: + P: + mode: fix + state: [1, 2, 3] + dynamic: true + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_2D_initial_guess_dynamic: - P: - mode: initial_guess - state: [1, 2] - dynamic: true - O: - mode: initial_guess - state: [1] - dynamic: true + states: + P: + mode: initial_guess + state: [1, 2] + dynamic: true + O: + mode: initial_guess + state: [1] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_3D_initial_guess_dynamic: - P: - mode: initial_guess - state: [1, 2, 3] - dynamic: true - O: - mode: initial_guess - state: [1, 0, 0, 0] - dynamic: true + states: + P: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + O: + mode: initial_guess + state: [1, 0, 0, 0] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_2D_factor_dynamic: - P: - mode: factor - state: [1, 2] - noise_std: [0.1, 0.2] - dynamic: true - O: - mode: factor - state: [1] - noise_std: [0.1] - dynamic: true + states: + P: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2] + dynamic: true + O: + mode: factor + state: [1] + noise_std: [0.1] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_3D_factor_dynamic: - P: - mode: factor - state: [1, 2, 3] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - O: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3] - dynamic: true + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + O: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_2D_fix_dynamic_drift: - P: - mode: fix - state: [1, 2] - dynamic: true - drift_std: [0.1, 0.2] - O: - mode: fix - state: [1] - dynamic: true - drift_std: [0.1] + states: + P: + mode: fix + state: [1, 2] + dynamic: true + drift_std: [0.1, 0.2] + O: + mode: fix + state: [1] + dynamic: true + drift_std: [0.1] noise_std: [0.1, 0.2] sensor_PO_3D_fix_dynamic_drift: - P: - mode: fix - state: [1, 2, 3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] - O: - mode: fix - state: [1, 0, 0, 0] - dynamic: true - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: fix + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: true + drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] sensor_PO_2D_initial_guess_dynamic_drift: - P: - mode: initial_guess - state: [1, 2] - dynamic: true - drift_std: [0.1, 0.2] - O: - mode: initial_guess - state: [1] - dynamic: true - drift_std: [0.1] + states: + P: + mode: initial_guess + state: [1, 2] + dynamic: true + drift_std: [0.1, 0.2] + O: + mode: initial_guess + state: [1] + dynamic: true + drift_std: [0.1] noise_std: [0.1, 0.2] sensor_PO_3D_initial_guess_dynamic_drift: - P: - mode: initial_guess - state: [1, 2, 3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] - O: - mode: initial_guess - state: [1, 0, 0, 0] - dynamic: true - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O: + mode: initial_guess + state: [1, 0, 0, 0] + dynamic: true + drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] sensor_PO_2D_factor_dynamic_drift: - P: - mode: factor - state: [1, 2] - noise_std: [0.1, 0.2] - dynamic: true - drift_std: [0.1, 0.2] - O: - mode: factor - state: [1] - noise_std: [0.1] - dynamic: true - drift_std: [0.1] + states: + P: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2] + dynamic: true + drift_std: [0.1, 0.2] + O: + mode: factor + state: [1] + noise_std: [0.1] + dynamic: true + drift_std: [0.1] noise_std: [0.1, 0.2] sensor_PO_3D_factor_dynamic_drift: - P: - mode: factor - state: [1, 2, 3] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] - O: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] sensor_POIA_3D: - P: - mode: factor - state: [1, 2, 3] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - O: - mode: fix - state: [1, 0, 0, 0] - dynamic: false - I: - mode: initial_guess - state: [1, 2, 3, 4] - dynamic: true - drift_std: [0.1, 0.2, 0.3, 0.4] - A: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: false + I: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + A: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] ############################################################################################# @@ -254,223 +273,265 @@ sensor: ############################################################################################# sensor_PO_2D_fix_wrong: - P: - mode: fix - state: [1, 2] - #dynamic: false #missing - O: - mode: fix - state: [1] - dynamic: false + states: + P: + mode: fix + state: [1, 2] + #dynamic: false #missing + O: + mode: fix + state: [1] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_3D_fix_wrong: - P: - mode: fix - state: [1, 2, 3] - dynamic: false - O: - mode: fix - state: [1, 0, 0, 0] - dynamic: false + states: + P: + mode: fix + state: [1, 2, 3] + dynamic: false + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: false #noise_std: [0.1, 0.2] #missing sensor_PO_2D_initial_guess_wrong: - P: - mode: initial_guess - state: [1, 2] - dynamic: false - O: - #mode: initial_guess #missing - state: [1] - dynamic: false + states: + P: + mode: initial_guess + state: [1, 2] + dynamic: false + O: + #mode: initial_guess #missing + state: [1] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_3D_initial_guess_wrong: - P: - mode: initial_guess - state: [1, 2, 3] - dynamic: false - # O: #missing - # mode: initial_guess - # state: [1, 0, 0, 0] - # dynamic: false + states: + P: + mode: initial_guess + state: [1, 2, 3] + dynamic: false + # O: #missing + # mode: initial_guess + # state: [1, 0, 0, 0] + # dynamic: false noise_std: [0.1, 0.2] sensor_PO_2D_factor_wrong: - P: - mode: factor - state: [1, 2] - noise_std: [0.1, 0.2] - dynamic: false - O: - mode: factor - #state: [1] #missing - noise_std: [0.1] - dynamic: false + states: + P: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2] + dynamic: false + O: + mode: factor + #state: [1] #missing + noise_std: [0.1] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_3D_factor_wrong: - P: - mode: factor - state: [1, 2, 3] - #noise_std: [0.1, 0.2, 0.3] #missing - dynamic: false - O: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3] - dynamic: false + states: + P: + mode: factor + state: [1, 2, 3] + #noise_std: [0.1, 0.2, 0.3] #missing + dynamic: false + O: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: false noise_std: [0.1, 0.2] sensor_PO_2D_fix_dynamic_wrong: - P: - mode: fix - state: [1, 2, 3] # wrong size - dynamic: true - O: - mode: fix - state: [1] - dynamic: true + states: + P: + mode: fix + state: [1, 2, 3] # wrong size + dynamic: true + O: + mode: fix + state: [1] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_3D_fix_dynamic_wrong: - P: - mode: fix - state: [1, 2, 3] - dynamic: true - O: - mode: fix - state: [1, 0, 0, 1] # not normalized - dynamic: true + states: + P: + mode: fix + state: [1, 2, 3] + dynamic: true + O: + mode: fix + state: [1, 0, 0, 1] # not normalized + dynamic: true noise_std: [0.1, 0.2] sensor_PO_2D_initial_guess_dynamic_wrong: - # P: #missing - # mode: initial_guess - # state: [1, 2] - # dynamic: true - O: - mode: initial_guess - state: [1] - dynamic: true + states: + # P: #missing + # mode: initial_guess + # state: [1, 2] + # dynamic: true + O: + mode: initial_guess + state: [1] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_3D_initial_guess_dynamic_wrong: - P: - mode: initial_guess - state: [1, 2] # wrong size - dynamic: true - O: - mode: initial_guess - state: [1, 0, 0, 0] - dynamic: true + states: + P: + mode: initial_guess + state: [1, 2] # wrong size + dynamic: true + O: + mode: initial_guess + state: [1, 0, 0, 0] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_2D_factor_dynamic_wrong: - P: - mode: factor - state: [1, 2] - noise_std: [0.1, 0.2, 0.3] #wrong size - dynamic: true - O: - mode: factor - state: [1] - noise_std: [0.1] - dynamic: true + states: + P: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2, 0.3] #wrong size + dynamic: true + O: + mode: factor + state: [1] + noise_std: [0.1] + dynamic: true noise_std: [0.1, 0.2] sensor_PO_3D_factor_dynamic_wrong: - P: - mode: factor - state: [1, 2, 3] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - O: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size - dynamic: true + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + O: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size + dynamic: true noise_std: [0.1, 0.2] sensor_PO_2D_fix_dynamic_drift_wrong: - P: - mode: fix - state: [1, 2] - dynamic: true - drift_std: [0.1, 0.2, 0.3] #wrong size - O: - mode: fix - state: [1] - dynamic: true - drift_std: [0.1] + states: + P: + mode: fix + state: [1, 2] + dynamic: true + drift_std: [0.1, 0.2, 0.3] #wrong size + O: + mode: fix + state: [1] + dynamic: true + drift_std: [0.1] noise_std: [0.1, 0.2] sensor_PO_3D_fix_dynamic_drift_wrong: - P: - mode: fix - state: [1, 2, 3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] - O: - mode: fix - state: [1, 0, 0, 0] - #dynamic: true #missing - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: fix + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O: + mode: fix + state: [1, 0, 0, 0] + #dynamic: true #missing + drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] sensor_PO_2D_initial_guess_dynamic_drift_wrong: - P: - mode: initial_guess - state: [1] #wrong size - dynamic: true - drift_std: [0.1, 0.2] - O: - mode: initial_guess - state: [1] - dynamic: true - drift_std: [0.1] + states: + P: + mode: initial_guess + state: [1] #wrong size + dynamic: true + drift_std: [0.1, 0.2] + O: + mode: initial_guess + state: [1] + dynamic: true + drift_std: [0.1] noise_std: [0.1, 0.2] sensor_PO_3D_initial_guess_dynamic_drift_wrong: - P: - mode: initial_guess - state: [1, 2, 3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] - O: - #mode: initial_guess #missing - state: [1, 0, 0, 0] - dynamic: true - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O: + #mode: initial_guess #missing + state: [1, 0, 0, 0] + dynamic: true + drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] sensor_PO_2D_factor_dynamic_drift_wrong: - P: - mode: factor - state: [1, 2] - noise_std: [0.1, 0.2] - dynamic: true - drift_std: [0.1] #wrong size - O: - mode: factor - state: [1] - noise_std: [0.1] - dynamic: true - drift_std: [0.1] + states: + P: + mode: factor + state: [1, 2] + noise_std: [0.1, 0.2] + dynamic: true + drift_std: [0.1] #wrong size + O: + mode: factor + state: [1] + noise_std: [0.1] + dynamic: true + drift_std: [0.1] noise_std: [0.1, 0.2] sensor_PO_3D_factor_dynamic_drift_wrong: - P: - mode: factor - state: [1, 2, 3] - noise_std: [0.1, 0.2, 0.3] - dynamic: true - drift_std: [0.1, 0.2, 0.3] - O: - mode: factor - state: [1, 0, 0, 0] - noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size - dynamic: true - drift_std: [0.1, 0.2, 0.3] + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + O: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size + dynamic: true + drift_std: [0.1, 0.2, 0.3] + noise_std: [0.1, 0.2] + + sensor_POIA_3D_wrong: + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: false + I: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + # A: + # mode: factor + # state: [1, 0, 0, 0] + # noise_std: [0.1, 0.2, 0.3] + # dynamic: true + # drift_std: [0.1, 0.2, 0.3] noise_std: [0.1, 0.2] \ No newline at end of file diff --git a/test/yaml/sensor_dummy.yaml b/test/yaml/sensor_dummy.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6fb39c70fe6506e0c822fe29b9ddcc57f0a02c5d --- /dev/null +++ b/test/yaml/sensor_dummy.yaml @@ -0,0 +1,52 @@ +sensor: + sensor_dummy_1: + param1: 1.2 + param2: 3 + noise_std: [0.1, 0.2] + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: false + I: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + drift_std: [0.1, 0.2, 0.3, 0.4] + A: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] + + sensor_dummy_1_wrong: + param1: 1.2 + param2: 3 + noise_std: [0.1, 0.2] + states: + P: + mode: factor + state: [1, 2, 3] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + O: + mode: fix + state: [1, 0, 0, 0] + dynamic: false + # I: + # mode: initial_guess + # state: [1, 2, 3, 4] + # dynamic: true + # drift_std: [0.1, 0.2, 0.3, 0.4] + A: + mode: factor + state: [1, 0, 0, 0] + noise_std: [0.1, 0.2, 0.3] + dynamic: true + drift_std: [0.1, 0.2, 0.3] \ No newline at end of file