diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 8e6c6db7c6657037e7df0eddeea9b683635e5b71..701c5877277f64f24aee2c7641264a7ee148fdb7 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -126,6 +126,19 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh void setProblem(ProblemPtr _problem) override final; public: + + /** \brief Constructor with Prior and Params + * + * Constructor with parameter vector + * \param _tp Type of the sensor (types defined at wolf.h) + * \param _priors list of the priors of the sensor states + * \param _params params struct + * + **/ + SensorBase(const std::string& _type, + std::list<Prior> _priors, + ParamsSensorBasePtr _params); + /** \brief Constructor with noise size * * Constructor with parameter vector diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h new file mode 100644 index 0000000000000000000000000000000000000000..c70bf54837a903448d2c599f42b53c073fff7428 --- /dev/null +++ b/include/core/state_block/prior.h @@ -0,0 +1,117 @@ +//--------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 PRIOR_H_ +#define PRIOR_H_ + +#include "core/utils/params_server.h" + +namespace wolf +{ + +class Prior +{ + private: + char key; // State key + std::string mode; // Prior mode. Can be 'nothing', 'initial_guess', 'fix' and 'factor' + Eigen::VectorXd state; // state values (only filled from server if mode != 'nothing') + Eigen::VectorXd sigma; // factor sigmas (only filled from server if mode == 'factor') + bool dynamic; // State dynamic + Eigen::VectorXd sigma_drift; // drift of the state (only filled from server if dynamic) + + public: + Prior() = default; + Prior(const std::string& _prefix, char _key, const ParamsServer& _server) + { + mode = _server.getParam<double>(prefix + _key + "/mode"); + + if (mode != "nothing" and mode != "initial_guess" and mode != "fix" and mode == "factor") + throw std::runtime_error("wrong mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'"); + + if ( mode == "nothing" and (key == 'P' or key == 'O')) + throw std::runtime_error("For P and O keys, mode 'nothing' is not valid"); + + if (mode != "nothing") + state = _server.getParam<Eigen::VectorXd>(prefix + _key + "/state"); + else + state = Eigen::VectorXd(0); + + if (mode == "factor") + sigma = _server.getParam<Eigen::VectorXd>(prefix + _key + "/sigma"); + else + sigma = Eigen::VectorXd(0); + + if ( key == 'P' or key == 'O') + dynamic = false; + else + dynamic = _server.getParam<bool>(prefix + _key + "/dynamic"); + + if (dynamic) + sigma_drift = _server.getParam<Eigen::VectorXd>(prefix + _key + "/sigma_drift"); + else + sigma_drift = Eigen::VectorXd(0); + } + + virtual ~Prior() = default; + + char getKey() const + { + return key; + } + + const std::string& getMode() const + { + return mode; + } + + const Eigen::VectorXd& getState() const + { + return state; + } + + const Eigen::VectorXd& getSigma() const + { + return sigma; + } + + bool isDynamic() const + { + return dynamic; + } + + const Eigen::VectorXd& getSigma() const + { + return sigma_drift; + } + + virtual std::string print() const final + { + return "Prior " + _key + "\n" + + "mode: " + std::to_string(mode) + "\n" + + "state: " + std::to_string(state) + "\n" + + (mode == "factor" ? "sigma: " + std::to_string(sigma) + "\n" : "") + + "dynamic: " + std::to_string(dynamic) + "\n" + + (dynamic ? "sigma_drift: "+ std::to_string(sigma_drift) + "\n" : ""); + } +}; + +} +#endif