wip

include/core/sensor/sensor_base.h

@@ -128,7 +128,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
 
     protected:
-        Eigen::VectorXd noise_std_; // std of sensor noise
+        //Eigen::VectorXd noise_std_; // std of sensor noise
         Eigen::MatrixXd noise_cov_; // cov matrix of noise
 
         void setProblem(ProblemPtr _problem) override final;
@@ -146,6 +146,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          *
          **/
         SensorBase(const std::string& _type,
+                   const std::string& _unique_name,
                    const SizeEigen& _dim,
                    const Priors& _priors,
                    ParamsSensorBasePtr _params);
@@ -159,8 +160,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          *
          **/
         SensorBase(const std::string& _type,
-                   const SizeEigen& _dim,
                    const std::string& _unique_name,
+                   const SizeEigen& _dim,
                    const ParamsServer& _server,
                    std::string _keys);
 

include/core/state_block/has_state_blocks.h

@@ -76,13 +76,8 @@ class HasStateBlocks
         std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
         std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
 
-        // Emplace derived state blocks (angle, quaternion, etc).
-        template<typename SB, typename ... Args>
-        std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
-
         // Emplace base state blocks.
-        template<typename ... Args>
-        StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+        StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, const Eigen::VectorXd& _state, bool _fixed);
 
         // Register/remove state blocks to/from wolf::Problem
         virtual void registerNewStateBlocks(ProblemPtr _problem);
@@ -171,28 +166,6 @@ inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
     return state_block_const_map_.erase(_sb_type);
 }
 
-template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
-{
-    assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
-    std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
-
-    addStateBlock(_sb_type, sb, _problem);
-
-    return sb;
-}
-
-template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
-{
-    assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
-    auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
-
-    addStateBlock(_sb_type, sb, _problem);
-
-    return sb;
-}
-
 inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
 {
     assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");

include/core/state_block/prior.h

@@ -42,36 +42,14 @@ class Prior
 
   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");
+    Prior(char _key, 
+          const std::string& _mode,  
+          const Eigen::VectorXd& _state, 
+          const Eigen::VectorXd& _sigma, 
+          bool _dynamic, 
+          const Eigen::VectorXd& _sigma_drift);
 
-      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);
-    }
+    Prior(const std::string& _prefix, char _key, const ParamsServer& _server);
 
     virtual ~Prior() = default;
 
@@ -120,9 +98,21 @@ class Prior
       return sigma_drift;
     }
 
+    void check() const
+    {
+      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 (dynamic and (key == 'P' or key == 'O') )
+        throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
+    }
+
     virtual std::string print() const final
     {
-      return "Prior "       + _key + "\n"
+      return "Prior " + key + "\n"
           + "mode: " + std::to_string(mode)  + "\n"
           + "state: "      + std::to_string(state)       + "\n"
           + (mode == "factor" ? "sigma: "      + std::to_string(sigma)       + "\n" : "")
@@ -131,5 +121,45 @@ class Prior
     }
 };
 
+inline Prior::Prior(char _key, 
+                    const std::string& _mode,  
+                    const Eigen::VectorXd& _state, 
+                    const Eigen::VectorXd& _sigma, 
+                    bool _dynamic, 
+                    const Eigen::VectorXd& _sigma_drift) : 
+  key(_key),
+  mode(_mode),
+  state(_state),
+  sigma(_sigma),
+  dynamic(_dynamic),
+  sigma_drift(_sigma_drift)
+{
+  check();
+}
+
+inline Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
+{
+  mode   = _server.getParam<double>(_prefix + _key + "/mode");
+
+  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);
+
+  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);
+
+  check();
+}
+
 }
 #endif

src/sensor/sensor_base.cpp

@@ -22,6 +22,7 @@
 #include "core/sensor/sensor_base.h"
 #include "core/utils/params_server.h"
 #include "core/state_block/prior.h"
+#include "core/state_block/factory_state_block.h"
 #include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/factor/factor_block_absolute.h"
@@ -32,10 +33,11 @@ namespace wolf {
 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) :
-        NodeBase("SENSOR", _type),
+        NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
         sensor_id_(++sensor_id_count_), // simple ID factory
@@ -53,11 +55,11 @@ SensorBase::SensorBase(const std::string& _type,
 }
 
 SensorBase::SensorBase(const std::string& _type,
-                       const SizeEigen& _dim,
                        const std::string& _unique_name,
+                       const SizeEigen& _dim,
                        const ParamsServer& _server,
                        std::string _keys) :
-        NodeBase("SENSOR", _type),
+        NodeBase("SENSOR", _type, _unique_name),
         HasStateBlocks(""),
         hardware_ptr_(),
         sensor_id_(++sensor_id_count_), // simple ID factory
@@ -93,6 +95,8 @@ void SensorBase::loadParams(ParamsSensorBasePtr _params)
 
 void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
 {
+    assert(_dim == 2 or _dim == 3);
+
     for (auto&& prior_pair : _priors)
     {
         const Prior& prior = prior_pair.second;
@@ -104,11 +108,7 @@ void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
             throw std::runtime_error("Prior state for O has wrong size");
 
         // create state block
-        StateBlockPtr sb;
-        if (prior.getKey() == 'O' and prior.getState().size() == 4)
-            sb = std::make_shared<StateQuaternion>(prior.getState(), prior.isFixed());
-        else
-            sb = std::make_shared<StateBlock>(prior.getState(), prior.isFixed());
+        auto sb = FactoryStateBlock::create(std::string(1, prior.getKey()), prior.getState(), prior.isFixed());
 
         // Add state block
         addStateBlock(prior.getKey(), sb, prior.isDynamic());
@@ -116,7 +116,6 @@ void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
         // Factor
         if (prior.isFactor())
             addPriorParameter(prior.getKey(), prior.getState(), prior.getSigma());
-
     }
 }
 

src/state_block/has_state_blocks.cpp

@@ -21,10 +21,25 @@
 //--------LICENSE_END--------
 
 #include "core/state_block/has_state_blocks.h"
+#include "core/state_block/factory_state_block.h"
 
 namespace wolf
 {
 
+StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, 
+                                                ProblemPtr _problem, 
+                                                const Eigen::VectorXd& _state, 
+                                                bool _fixed)
+{
+    assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+    
+    auto sb = FactoryStateBlock::create(std::string(1,_sb_type), _state, _fixed);
+
+    addStateBlock(_sb_type, sb, _problem);
+
+    return sb;
+}
+
 StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
 {
     assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");

src/state_block/state_block.cpp

@@ -87,7 +87,7 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
     if (_state.size() == 4)
         return StateQuaternion::create(_state, _fixed);
 
-    throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
+    throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
 
     return nullptr;
 }