starting wip

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

include/core/state_block/prior.h

+//--------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