wip

include/core/state_block/prior.h

@@ -38,7 +38,7 @@ typedef std::unordered_map<char, Prior> Priors;
 class Prior
 {
   private:
-    char key_;                     // State key
+    std::string type_;             // State type
     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')
@@ -47,18 +47,18 @@ class Prior
 
   public:
     Prior() = default;
-    Prior(char _key, 
+    Prior(const std::string& _type, 
           const std::string& _mode,  
           const Eigen::VectorXd& _state, 
           const Eigen::VectorXd& _sigma = Eigen::VectorXd(0), 
           bool _dynamic = false, 
           const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0));
 
-    Prior(const std::string& _prefix, char _key, const ParamsServer& _server);
+    Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server);
 
     virtual ~Prior() = default;
 
-    char getKey() const;
+    const std::string& getType() const;
     const std::string& getMode() const;
     const Eigen::VectorXd& getState() const;
     const Eigen::VectorXd& getSigma() const;
@@ -73,10 +73,9 @@ class Prior
     virtual std::string print() const final;
 };
 
-
-inline char Prior::getKey() const
+inline const std::string& Prior::getType() const
 {
-  return key_;
+  return type_;
 }
 
 inline const std::string& Prior::getMode() const

src/sensor/sensor_base.cpp

@@ -58,7 +58,7 @@ SensorBase::SensorBase(const std::string& _type,
                        const std::string& _unique_name,
                        const SizeEigen& _dim,
                        const ParamsServer& _server,
-                       std::string _keys) :
+                       std::unordered_map<char, std::string> _keys_types) :
         NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
@@ -75,8 +75,8 @@ SensorBase::SensorBase(const std::string& _type,
 
     // create priors
     Priors priors;
-    for (auto key : _keys)
-        priors[key] = Prior("sensor/" + _unique_name, key, _server);
+    for (auto pair : _keys_types)
+        priors[pair.first] = Prior("sensor/" + _unique_name, pair.second, _server);
 
     // load priors
     loadPriors(priors, _dim);

src/state_block/prior.cpp

@@ -26,13 +26,13 @@
 
 namespace wolf {
 
-Prior::Prior(char _key, 
+Prior::Prior(const std::string& _type, 
              const std::string& _mode,  
              const Eigen::VectorXd& _state, 
              const Eigen::VectorXd& _sigma, 
              bool _dynamic, 
              const Eigen::VectorXd& _sigma_drift) : 
-  key_(_key),
+  type_(_type),
   mode_(_mode),
   state_(_state),
   sigma_(_sigma),
@@ -42,24 +42,25 @@ Prior::Prior(char _key,
   check();
 }
 
-Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
+Prior::Prior(const std::string& _prefix, const ParamsServer& _server, const std::string& _type)
 {
-  mode_   = _server.getParam<double>(_prefix + _key + "/mode");
+  type_ = _type;
+  mode_   = _server.getParam<double>(_prefix + "/mode");
 
   if (mode_ != "nothing")
-    state_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
+    state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state");
   else
     state_ = Eigen::VectorXd(0);
 
   if (mode_ == "factor")
-    sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
+    sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma");
   else
     sigma_ = Eigen::VectorXd(0);
 
-  dynamic_ = _server.getParam<bool>(_prefix + _key + "/dynamic");
+  dynamic_ = _server.getParam<bool>(_prefix + "/dynamic");
 
   if (dynamic_)
-    sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
+    sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift");
   else
     sigma_drift_ = Eigen::VectorXd(0);
 
@@ -71,35 +72,35 @@ void Prior::check() const
     if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") 
         throw std::runtime_error("wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
 
-    if ( mode_ == "nothing" and (key_ == 'P' or key_ == 'O'))
+    if ( mode_ == "nothing" and (type_ == "P" or type_ == "O"))
         throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
 
-    if (dynamic_ and (key_ == 'P' or key_ == 'O') )
+    if (dynamic_ and (type_ == "P" or type_ == "O") )
         throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
 
     // try to create a state block and check for local parameterization and dimensions consistency
-    auto sb = FactoryStateBlock::create(std::string(1, key_), state_, mode_ == "fix");
+    auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
 
     // check local param
     if (not sb->isValid())
-        throw std::runtime_error("Prior " + std::string(1, key_) + " state is not valid (local param violation)");
+        throw std::runtime_error("Prior " + type_ + " state is not valid (local param violation)");
 
     // check state size
     if (mode_ != "nothing" and state_.size() != sb->getSize())
-        throw std::runtime_error("Prior " + std::string(1, key_) + " state size different of state size");
+        throw std::runtime_error("Prior " + type_ + " state size different of state size");
 
     // check sigma size
     if (mode_ == "factor" and sigma_.size() != sb->getLocalSize())
-        throw std::runtime_error("Prior " + std::string(1, key_) + " sigma size different of state local size");
+        throw std::runtime_error("Prior " + type_ + " sigma size different of state local size");
 
     // check sigma size
     if (dynamic_ and sigma_drift_.size() != sb->getLocalSize())
-        throw std::runtime_error("Prior " + std::string(1, key_) + " sigma_drift size different of state local size");
+        throw std::runtime_error("Prior " + type_ + " sigma_drift size different of state local size");
 }
 
 std::string Prior::print() const
 {
-    return "Prior " + std::string(1,key_) + "\n"
+    return "Prior " + type_ + "\n"
         + "mode: " + mode_  + "\n"
         //+ "state: "      + to_string(state_)       + "\n"
         //+ (mode_ == "factor" ? "sigma: "      + to_string(sigma_)       + "\n" : "")

test/gtest_prior.cpp

@@ -230,6 +230,9 @@ TEST(Prior, K)
   testPriors(setups_death, false);
 }
 
+// TODO: param_server constructor!
+
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);

test/gtest_sensor_base.cpp

@@ -39,6 +39,7 @@ TEST(SensorBase, POfix)
 
   auto params = std::make_shared<ParamsSensorBase>();
   params->noise_std = Eigen::Vector2d::Constant(0.1);
+  
   auto S = std::make_shared<SensorBase>("SensorBase",
                                         "sensor1",
                                         2,