include/core/common/factory.h

@@ -126,18 +126,20 @@ namespace wolf
  *
  * Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>:
  * it can only exist once in your application.
- * To access it, use the static method get(),
+ * To access it, we internally use the static method get(),
  *
  *     \code
  *     Factory<MyTypeBase, MyTypeInput>::get()
  *     \endcode
  *
- * where, of course, you better make use of the appropriate typedef in place of ````Factory<MyTypeBase, MyTypeInput>````.
+ * but this is a private method.
  *
- * You can then call the methods you like, e.g. to create a landmark, you use:
+ * The interesting methods are designed static, and are the ones responsible for accessing the Factory instance via .get().
+ *
+ * You must therefore call the methods you like statically, e.g. to create a landmark, you use:
  *
  *     \code
- *     FactoryLandmark::get().create(TYPE, args...); // see below for creating objects ...
+ *     FactoryLandmark::create(TYPE, args...); // see below for creating objects ...
  *     \endcode
  *
  * #### Write creator methods (in your derived object classes)
@@ -172,7 +174,7 @@ namespace wolf
  * that knows how to create your specific object, e.g.:
  *
  *     \code
- *     FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
+ *     FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
  *     \endcode
  *
  * #### Automatic registration
@@ -180,7 +182,7 @@ namespace wolf
  * For example, in sensor_camera_yaml.cpp we find the line:
  *
  *     \code
- *     const bool registered_camera_intr = FactoryParamsSensor::get().registerCreator("ParamsSensorCamera", createParamsSensorCamera);
+ *     const bool registered_camera_intr = FactoryParamsSensor::registerCreator("ParamsSensorCamera", createParamsSensorCamera);
  *     \endcode
  *
  * which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
@@ -191,7 +193,7 @@ namespace wolf
  * The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of the object type.
  *
  *     \code
- *     Factory<MyTypeBase, MyTypeInput>::get().unregisterCreator("ParamsSensorCamera");
+ *     Factory<MyTypeBase, MyTypeInput>::unregisterCreator("ParamsSensorCamera");
  *     \endcode
  *
  * #### Create objects using the factory
@@ -201,13 +203,13 @@ namespace wolf
  * To create e.g. a LandmarkPolyline2d from a YAML node you type:
  *
  *     \code
- *     LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::get().create("LandmarkPolyline2d", lmk_yaml_node);
+ *     LandmarkBasePtr lmk_ptr = Factory<LandmarkBasePtr, YAML::Node>::create("LandmarkPolyline2d", lmk_yaml_node);
  *     \endcode
  *
  * or even better, make use of the convenient typedefs:
  *
  *     \code
- *     LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create("LandmarkPolyline2d", lmk_yaml_node);
+ *     LandmarkBasePtr lmk_ptr = FactoryLandmark::create("LandmarkPolyline2d", lmk_yaml_node);
  *     \endcode
  *
  * ### Examples
@@ -247,7 +249,7 @@ namespace wolf
  * // Register landmark creator (put the register code inside an unnamed namespace):
  *  namespace
  *  {
- *  const bool registered_lmk_polyline_2d = FactoryLandmark::get().registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
+ *  const bool registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
  *  }
  *
  * \endcode
@@ -280,9 +282,9 @@ class Factory
         typedef std::map<std::string, CreatorCallback> CallbackMap;
 
     public:
-        bool registerCreator(const std::string& _type, CreatorCallback createFn);
-        bool unregisterCreator(const std::string& _type);
-        TypeBasePtr create(const std::string& _type, TypeInput... _input);
+        static bool registerCreator(const std::string& _type, CreatorCallback createFn);
+        static bool unregisterCreator(const std::string& _type);
+        static TypeBasePtr create(const std::string& _type, TypeInput... _input);
         std::string getClass() const;
 
     private:
@@ -291,7 +293,7 @@ class Factory
         // Singleton ---------------------------------------------------
         // This class is a singleton. The code below guarantees this.
         // See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
-    public:
+    private:
         static Factory& get();
     public:
         Factory(const Factory&)         = delete;
@@ -304,11 +306,11 @@ class Factory
 template<class TypeBase, typename... TypeInput>
 inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
 {
-    bool reg = callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
+    bool reg = get().callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
     if (reg)
-        std::cout << std::setw(26) << std::left << getClass() << " <--  registered  " << _type << std::endl;
+        std::cout << std::setw(26) << std::left << get().getClass() << " <--  registered  " << _type << std::endl;
     else
-        std::cout << std::setw(26) << std::left << getClass() << " X--   skipping   " << _type << ": already registered." << std::endl;
+        std::cout << std::setw(26) << std::left << get().getClass() << " X--   skipping   " << _type << ": already registered." << std::endl;
 
     return reg;
 }
@@ -316,17 +318,17 @@ inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string&
 template<class TypeBase, typename... TypeInput>
 inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
 {
-    return callbacks_.erase(_type) == 1;
+    return get().callbacks_.erase(_type) == 1;
 }
 
 template<class TypeBase, typename... TypeInput>
 inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
 {
-    typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
+    typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
 
-    if (creator_callback_it == callbacks_.end())
+    if (creator_callback_it == get().callbacks_.end())
         // not found
-        throw std::runtime_error(getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
+        throw std::runtime_error(get().getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
 
     // Invoke the creation function
     return (creator_callback_it->second)(std::forward<TypeInput>(_input)...);

include/core/processor/factory_processor.h

@@ -75,7 +75,7 @@ namespace wolf
  * To create a ProcessorOdom2d, you type:
  *
  *     \code
- *     auto prc_odom2d_ptr = FactoryProcessor::get().create("ProcessorOdom2d", "main odometry", params_ptr);
+ *     auto prc_odom2d_ptr = FactoryProcessor::create("ProcessorOdom2d", "main odometry", params_ptr);
  *     \endcode
  *
  * #### Example 1 : Create a sensor and its processor
@@ -90,14 +90,14 @@ namespace wolf
  *     // Note: ProcessorOdom2d::create() is already registered, automatically.
  *
  *     // First create the sensor (See FactorySensor for details)
- *     SensorBasePtr sensor_ptr = FactorySensor::get().create ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
+ *     SensorBasePtr sensor_ptr = FactorySensor::create ( "SensorOdom2d" , "Main odometer" , extrinsics , &intrinsics );
  *
  *     // To create a odometry integrator, provide a type="ProcessorOdom2d", a name="main odometry", and a pointer to the parameters struct:
  *
  *     auto params = make_shared<ParamsProcessorOdom2d>({...});   // fill in the derived struct (note: ProcessorOdom2d actually has no input params)
  *
  *     ProcessorBasePtr processor_ptr =
- *         FactoryProcessor::get().create ( "ProcessorOdom2d" , "main odometry" , params );
+ *         FactoryProcessor::create ( "ProcessorOdom2d" , "main odometry" , params );
  *
  *     // Bind processor to sensor
  *     sensor_ptr->addProcessor(processor_ptr);
@@ -126,7 +126,7 @@ inline std::string FactoryParamsProcessor::getClass() const
 
 #define WOLF_REGISTER_PROCESSOR(ProcessorType)                                   \
   namespace{ const bool WOLF_UNUSED ProcessorType##Registered =                                 \
-    wolf::FactoryProcessor::get().registerCreator(#ProcessorType, ProcessorType::create); }      \
+    wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); }      \
 
 typedef Factory<ProcessorBase,
         const std::string&,
@@ -140,7 +140,7 @@ inline std::string AutoConfFactoryProcessor::getClass() const
 
 #define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType)                                  \
   namespace{ const bool WOLF_UNUSED ProcessorType##AutoConfRegistered =                             \
-    wolf::AutoConfFactoryProcessor::get().registerCreator(#ProcessorType, ProcessorType::create); }  \
+    wolf::AutoConfFactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); }  \
 
 } /* namespace wolf */
 

include/core/sensor/factory_sensor.h

@@ -79,7 +79,7 @@ namespace wolf
  * To create e.g. a SensorCamera, you type:
  *
  *     \code
- *     auto camera_ptr = FactorySensor::get().create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
+ *     auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", params_extrinsics, params_camera);
  *     \endcode
  *
  * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
@@ -119,14 +119,14 @@ namespace wolf
  *   Put the code either at global scope (you must define a dummy variable for this),
  *      \code
  *      namespace {
- *      const bool registered_camera = FactorySensor::get().registerCreator("SensorCamera", SensorCamera::create);
+ *      const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
  *      }
  *      main () { ... }
  *      \endcode
  *   or inside your main(), where a direct call is possible:
  *      \code
  *      main () {
- *          FactorySensor::get().registerCreator("SensorCamera", SensorCamera::create);
+ *          FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
  *          ...
  *      }
  *      \endcode
@@ -135,7 +135,7 @@ namespace wolf
  *   Put the code at the last line of the sensor_xxx.cpp file,
  *      \code
  *      namespace {
- *      const bool registered_camera = FactorySensor::get().registerCreator("SensorCamera", SensorCamera::create);
+ *      const bool registered_camera = FactorySensor::registerCreator("SensorCamera", SensorCamera::create);
  *      }
  *      \endcode
  *   Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
@@ -163,11 +163,11 @@ namespace wolf
  *
  *      // Create a pointer to the struct of sensor parameters stored in a YAML file ( see FactoryParamsSensor )
  *      ParamsSensorCameraPtr  intrinsics_1 =
- *          FactoryParamsSensor::get().create("ParamsSensorCamera",
+ *          FactoryParamsSensor::create("ParamsSensorCamera",
  *                                            camera_1.yaml);
  *
  *      SensorBasePtr camera_1_ptr =
- *          FactorySensor::get().create ( "SensorCamera" ,
+ *          FactorySensor::create ( "SensorCamera" ,
  *                                        "Front-left camera" ,
  *                                        extrinsics_1 ,
  *                                        intrinsics_1 );
@@ -177,11 +177,11 @@ namespace wolf
  *      Eigen::VectorXd        extrinsics_2(7);
  *
  *      ParamsSensorCameraPtr  intrinsics_2 =
- *          FactoryParamsSensor::get().create("ParamsSensorCamera",
+ *          FactoryParamsSensor::create("ParamsSensorCamera",
  *                                            camera_2.yaml);
  *
  *      SensorBasePtr camera_2_ptr =
- *          FactorySensor::get().create( "SensorCamera" ,
+ *          FactorySensor::create( "SensorCamera" ,
  *                                       "Front-right camera" ,
  *                                       extrinsics_2 ,
  *                                       intrinsics_2 );
@@ -214,7 +214,7 @@ inline std::string FactoryParamsSensor::getClass() const
 
 #define WOLF_REGISTER_SENSOR(SensorType)                            \
   namespace{ const bool WOLF_UNUSED SensorType##Registered =                    \
-    FactorySensor::get().registerCreator(#SensorType, SensorType::create); }     \
+    FactorySensor::registerCreator(#SensorType, SensorType::create); }     \
 
 
 typedef Factory<SensorBase,
@@ -229,7 +229,7 @@ inline std::string AutoConfFactorySensor::getClass() const
 
 #define WOLF_REGISTER_SENSOR_AUTO(SensorType)                               \
   namespace{ const bool WOLF_UNUSED SensorType##AutoConfRegistered =                    \
-     AutoConfFactorySensor::get().registerCreator(#SensorType, SensorType::create); }    \
+     AutoConfFactorySensor::registerCreator(#SensorType, SensorType::create); }    \
 
 
 } /* namespace wolf */

include/core/solver/factory_solver.h

@@ -37,7 +37,7 @@ inline std::string FactorySolver::getClass() const
 
 #define WOLF_REGISTER_SOLVER(SolverType) \
   namespace{ const bool WOLF_UNUSED SolverType##Registered = \
-     wolf::FactorySolver::get().registerCreator(#SolverType, SolverType::create); } \
+     wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
 
 } /* namespace wolf */
 

include/core/state_block/factory_state_block.h

@@ -68,19 +68,19 @@ namespace wolf
  * To create a StateQuaternion, you type:
  *
  *     \code
- *     auto sq_ptr = FactoryStateBlock::get().create("StateQuaternion", Vector4d(1,0,0,0), false);
+ *     auto sq_ptr = FactoryStateBlock::create("StateQuaternion", Vector4d(1,0,0,0), false);
  *     \endcode
  *
  * If your problem has dimension 3 (e.g. is 3D), you can use the key "O" to create a StateQuaternion,
  *
  *     \code
- *     auto sq_ptr = FactoryStateBlock::get().create("O", Vector4d(1,0,0,0), false);
+ *     auto sq_ptr = FactoryStateBlock::create("O", Vector4d(1,0,0,0), false);
  *     \endcode
  *
  * However if your problem has dimension 2 (e.g. is 2D), the key "O" will create a StateAngle,
  *
  *     \code
- *     auto sa_ptr = FactoryStateBlock::get().create("O", Vector1d(angle_in_radians), false);
+ *     auto sa_ptr = FactoryStateBlock::create("O", Vector1d(angle_in_radians), false);
  *     \endcode
  *
  * Note: It is an error to provide state vectors of the wrong size (4 for 3D orientation, 1 for 2D).
@@ -88,22 +88,22 @@ namespace wolf
  * To create StateBlocks to store 2D position and velocity, you type:
  *
  *     \code
- *     auto sp2_ptr = FactoryStateBlock::get().create("StateBlock", Vector2d(1,2), false);
- *     auto sv2_ptr = FactoryStateBlock::get().create("StateBlock", Vector2d(1,2), false);
+ *     auto sp2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false);
+ *     auto sv2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false);
  *     \endcode
  *
  * To create StateBlocks to store 2D position and velocity, you can also use the key letters:
  *
  *     \code
- *     auto sp2_ptr = FactoryStateBlock::get().create("P", Vector2d(1,2), false);
- *     auto sv2_ptr = FactoryStateBlock::get().create("V", Vector2d(1,2), false);
+ *     auto sp2_ptr = FactoryStateBlock::create("P", Vector2d(1,2), false);
+ *     auto sv2_ptr = FactoryStateBlock::create("V", Vector2d(1,2), false);
  *     \endcode
  *
  * To create StateBlocks to store 3D position and velocity, you type:
  *
  *     \code
- *     auto sp3_ptr = FactoryStateBlock::get().create("P", Vector3d(1,2,3), false);
- *     auto sv3_ptr = FactoryStateBlock::get().create("V", Vector3d(1,2,3), false);
+ *     auto sp3_ptr = FactoryStateBlock::create("P", Vector3d(1,2,3), false);
+ *     auto sv3_ptr = FactoryStateBlock::create("V", Vector3d(1,2,3), false);
  *     \endcode
  *
  * Note: for base state blocks, the size is determined by the size of the provided vector parameter `VectorXd& _state`.
@@ -118,9 +118,9 @@ inline std::string FactoryStateBlock::getClass() const
 template<>
 inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const Eigen::VectorXd& _state, bool _fixed)
 {
-    typename CallbackMap::const_iterator creator_callback_it = callbacks_.find(_type);
+    typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
 
-    if (creator_callback_it == callbacks_.end())
+    if (creator_callback_it == get().callbacks_.end())
         // not found: return StateBlock base
         return std::make_shared<StateBlock>(_state, _fixed);
 
@@ -130,11 +130,11 @@ inline StateBlockPtr FactoryStateBlock::create(const std::string& _type, const E
 
 #define WOLF_REGISTER_STATEBLOCK(StateBlockType)                                            \
   namespace{ const bool WOLF_UNUSED StateBlockType##Registered =                            \
-     FactoryStateBlock::get().registerCreator(#StateBlockType, StateBlockType::create); }   \
+     FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); }   \
 
 #define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType)                              \
   namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key =                   \
-     FactoryStateBlock::get().registerCreator(#Key, StateBlockType::create); }              \
+     FactoryStateBlock::registerCreator(#Key, StateBlockType::create); }              \
 
 
 

include/core/state_block/has_state_blocks.h

@@ -301,9 +301,9 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _sub_struct
     for (const char key : structure)
     {
         const auto& sb = getStateBlock(key);
-        if (!sb){
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
-        }
+
+        assert(sb != nullptr && "Requested StateBlock key not in the structure");
+
         state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
     }

include/core/state_block/state_composite.h

@@ -49,8 +49,6 @@ class VectorComposite : public std::unordered_map < std::string, Eigen::VectorXd
         VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
         VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
 
-        unsigned int size(const StateStructure& _structure) const;
-
         Eigen::VectorXd vector(const StateStructure& _structure) const;
 
         /**

include/core/tree_manager/factory_tree_manager.h

@@ -40,7 +40,7 @@ inline std::string FactoryTreeManager::getClass() const
 
 #define WOLF_REGISTER_TREE_MANAGER(TreeManagerType)                     \
   namespace{ const bool WOLF_UNUSED TreeManagerType##Registered =                                 \
-    wolf::FactoryTreeManager::get().registerCreator(#TreeManagerType, TreeManagerType::create); }      \
+    wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); }      \
 
 typedef Factory<TreeManagerBase,
         const std::string&,
@@ -54,7 +54,7 @@ inline std::string AutoConfFactoryTreeManager::getClass() const
 
 #define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType)                                  \
   namespace{ const bool WOLF_UNUSED TreeManagerType##AutoConfRegistered =                             \
-    wolf::AutoConfFactoryTreeManager::get().registerCreator(#TreeManagerType, TreeManagerType::create); }  \
+    wolf::AutoConfFactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); }  \
 
 } /* namespace wolf */
 

src/frame/frame_base.cpp

@@ -28,7 +28,7 @@ FrameBase::FrameBase(const FrameType & _tp,
         const auto& key  = pair_key_vec.first;
         const auto& vec  = pair_key_vec.second;
 
-        StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+        StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
 
         addStateBlock(key, sb);
     }
@@ -54,7 +54,7 @@ FrameBase::FrameBase(const FrameType & _tp,
         const auto& key = string(1,ckey);
         const auto& vec = *vec_it;
 
-        StateBlockPtr sb = FactoryStateBlock::get().create(key, vec, false); // false = non fixed
+        StateBlockPtr sb = FactoryStateBlock::create(key, vec, false); // false = non fixed
 
         addStateBlock(key, sb);
 

src/landmark/landmark_base.cpp

@@ -311,7 +311,7 @@ bool LandmarkBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bo
 // Register landmark creator
 namespace
 {
-const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::get().registerCreator("LandmarkBase", LandmarkBase::create);
+const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
 }
 
 } // namespace wolf

src/map/map_base.cpp

@@ -49,7 +49,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
     for (unsigned int i = 0; i < nlandmarks; i++)
     {
         YAML::Node lmk_node = map["landmarks"][i];
-        LandmarkBasePtr lmk_ptr = FactoryLandmark::get().create(lmk_node["type"].as<std::string>(), lmk_node);
+        LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
         lmk_ptr->link(shared_from_this());
     }
 

src/problem/problem.cpp

@@ -172,7 +172,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
     std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
     WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
     if (tree_manager_type != "None" and tree_manager_type != "none")
-        problem->setTreeManager(AutoConfFactoryTreeManager::get().create(tree_manager_type, "tree manager", _server));
+        problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
 
     // Prior
     std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
@@ -213,7 +213,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                      const Eigen::VectorXd& _extrinsics, //
                                      ParamsSensorBasePtr _intrinsics)
 {
-    SensorBasePtr sen_ptr = FactorySensor::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
+    SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
     sen_ptr->link(getHardware());
     return sen_ptr;
 }
@@ -228,7 +228,7 @@ 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::get().create(_sen_type, _intrinsics_filename);
+        ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename);
         return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
     }
     else
@@ -240,7 +240,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
                                      const std::string& _unique_sensor_name, //
                                      const ParamsServer& _server)
 {
-    SensorBasePtr sen_ptr = AutoConfFactorySensor::get().create(_sen_type, _unique_sensor_name, _server);
+    SensorBasePtr sen_ptr = AutoConfFactorySensor::create(_sen_type, _unique_sensor_name, _server);
     sen_ptr->link(getHardware());
     return sen_ptr;
 }
@@ -257,7 +257,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
       return ProcessorBasePtr();
     }
 
-    ProcessorBasePtr prc_ptr = FactoryProcessor::get().create(_prc_type, _unique_processor_name, _prc_params);
+    ProcessorBasePtr prc_ptr = FactoryProcessor::create(_prc_type, _unique_processor_name, _prc_params);
 
     //Dimension check
     int prc_dim = prc_ptr->getDim();
@@ -283,7 +283,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     else
     {
         assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
-        ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::get().create(_prc_type, _params_filename);
+        ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::create(_prc_type, _params_filename);
         return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
     }
 }
@@ -297,7 +297,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     if (sen_ptr == nullptr)
         throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
 
-    ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::get().create(_prc_type, _unique_processor_name, _server);
+    ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::create(_prc_type, _unique_processor_name, _server);
 
     //Dimension check
     int prc_dim = prc_ptr->getDim();
@@ -440,6 +440,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
                     state.insert(pair_key_vec);
             }
         }
+
         // check for empty blocks and fill them with zeros
         for (const auto& ckey : frame_structure_)
         {
@@ -483,6 +484,15 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
                 state.insert(pair_key_vec);
         }
     }
+
+    // check for empty blocks and fill them with zeros
+    for (const auto& ckey : frame_structure_)
+    {
+        const auto& key = string(1,ckey);
+        if (state.count(key) == 0)
+            state.emplace(key, stateZero(key).at(key));
+    }
+
     return state;
 }
 

src/state_block/state_block.cpp

@@ -73,5 +73,5 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
 
 //WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
 namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO =                   \
-     FactoryStateBlock::get().registerCreator("O", wolf::create_orientation); }
+     FactoryStateBlock::registerCreator("O", wolf::create_orientation); }
 }

src/state_block/state_composite.cpp

@@ -62,18 +62,6 @@ VectorComposite::VectorComposite (const StateStructure& _structure, const std::l
 }
 
 
-unsigned int VectorComposite::size(const StateStructure &_structure) const
-{
-    unsigned int size = 0;
-    for (const auto& ckey : _structure)
-    {
-        std::string     key(1,ckey); // ckey is char
-        const VectorXd& v       =  this->at(key);
-        size                    +=  v.size();
-    }
-    return size;
-}
-
 Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const
 {
     // traverse once with unordered_map access

src/yaml/parser_yaml.cpp

@@ -497,13 +497,11 @@ void ParserYAML::parse()
 
     YAML::Node n;
     n = loadYAML(generatePath(file_));
-
     if (n.Type() == YAML::NodeType::Map)
     {
         for (auto it : n)
         {
             auto node = it.second;
-            // WOLF_INFO("WUT ", it.first);
             std::vector<std::string> tags = std::vector<std::string>();
             if (it.first.as<std::string>() != "config")
                 walkTreeR(node, tags, it.first.as<std::string>());
@@ -515,7 +513,15 @@ void ParserYAML::parse()
                     if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
                         node_key != "ROS subscriber" and node_key != "ROS publisher")
                     {
-                        walkTreeR(itt.second, tags, node_key);
+                        std::regex rr("follow");
+                        if (not std::regex_match(node_key, rr))
+                        {
+                            walkTreeR(itt.second, tags, node_key);
+                        }
+                        else
+                        {
+                            walkTree(itt.second.as<std::string>(), tags, "");
+                        }
                     }
                 }
             }

src/yaml/processor_odom_3d_yaml.cpp

@@ -48,7 +48,7 @@ static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _f
 }
 
 // Register in the FactorySensor
-const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::get().registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
+const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
 
 } // namespace [unnamed]
 

src/yaml/sensor_odom_2d_yaml.cpp

@@ -40,7 +40,7 @@ static ParamsSensorBasePtr createParamsSensorOdom2d(const std::string & _filenam
 }
 
 // Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::get().registerCreator("SensorOdom2d", createParamsSensorOdom2d);
+const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::registerCreator("SensorOdom2d", createParamsSensorOdom2d);
 
 } // namespace [unnamed]
 

src/yaml/sensor_odom_3d_yaml.cpp

@@ -42,7 +42,7 @@ static ParamsSensorBasePtr createParamsSensorOdom3d(const std::string & _filenam
 }
 
 // Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::get().registerCreator("SensorOdom3d", createParamsSensorOdom3d);
+const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorOdom3d", createParamsSensorOdom3d);
 
 } // namespace [unnamed]
 

test/gtest_factory_state_block.cpp

@@ -41,20 +41,20 @@ TEST_F(TestInit, testName)
 }
 */
 
-TEST(FactoryStateBlock, get_getClass)
-{
-    const auto& f = FactoryStateBlock::get();
-
-    const std::string& s = f.getClass();
-
-    ASSERT_EQ(s, "FactoryStateBlock");
-}
+//TEST(FactoryStateBlock, get_getClass)
+//{
+//    const auto& f = FactoryStateBlock::get();
+//
+//    const std::string& s = f.getClass();
+//
+//    ASSERT_EQ(s, "FactoryStateBlock");
+//}
 
 TEST(FactoryStateBlock, creator_default)
 {
-    auto sbp = FactoryStateBlock::get().create("P", Eigen::Vector3d(1,2,3), false);
-    auto sbv = FactoryStateBlock::get().create("V", Eigen::Vector2d(4,5), false);
-    auto sbw = FactoryStateBlock::get().create("W", Eigen::Vector1d(6), false);
+    auto sbp = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
+    auto sbv = FactoryStateBlock::create("V", Eigen::Vector2d(4,5), false);
+    auto sbw = FactoryStateBlock::create("W", Eigen::Vector1d(6), false);
 
     ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
     ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5)   , sbv->getState(), 1e-20);
@@ -67,9 +67,9 @@ TEST(FactoryStateBlock, creator_default)
 
 TEST(FactoryStateBlock, creator_StateBlock)
 {
-    auto sbp = FactoryStateBlock::get().create("StateBlock", Eigen::Vector3d(1,2,3), false);
-    auto sbv = FactoryStateBlock::get().create("StateBlock", Eigen::Vector2d(4,5), true);
-    auto sbw = FactoryStateBlock::get().create("StateBlock", Eigen::Vector1d(6), false);
+    auto sbp = FactoryStateBlock::create("StateBlock", Eigen::Vector3d(1,2,3), false);
+    auto sbv = FactoryStateBlock::create("StateBlock", Eigen::Vector2d(4,5), true);
+    auto sbw = FactoryStateBlock::create("StateBlock", Eigen::Vector1d(6), false);
 
     ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
     ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5)   , sbv->getState(), 1e-20);
@@ -86,7 +86,7 @@ TEST(FactoryStateBlock, creator_StateBlock)
 
 TEST(FactoryStateBlock, creator_Quaternion)
 {
-    auto sbq = FactoryStateBlock::get().create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+    auto sbq = FactoryStateBlock::create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -95,7 +95,7 @@ TEST(FactoryStateBlock, creator_Quaternion)
 
 TEST(FactoryStateBlock, creator_Angle)
 {
-    auto sba = FactoryStateBlock::get().create("StateAngle", Eigen::Vector1d(1), false);
+    auto sba = FactoryStateBlock::create("StateAngle", Eigen::Vector1d(1), false);
 
     ASSERT_EQ(sba->getSize()     , 1);
     ASSERT_EQ(sba->getLocalSize(), 1);
@@ -104,7 +104,7 @@ TEST(FactoryStateBlock, creator_Angle)
 
 TEST(FactoryStateBlock, creator_Homogeneous3d)
 {
-    auto sbh = FactoryStateBlock::get().create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+    auto sbh = FactoryStateBlock::create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
 
     ASSERT_EQ(sbh->getSize()     , 4);
     ASSERT_EQ(sbh->getLocalSize(), 3);
@@ -113,7 +113,7 @@ TEST(FactoryStateBlock, creator_Homogeneous3d)
 
 TEST(FactoryStateBlock, creator_H)
 {
-    auto sbh = FactoryStateBlock::get().create("H", Eigen::Vector4d(1,2,3,4), false);
+    auto sbh = FactoryStateBlock::create("H", Eigen::Vector4d(1,2,3,4), false);
 
     ASSERT_EQ(sbh->getSize()     , 4);
     ASSERT_EQ(sbh->getLocalSize(), 3);
@@ -122,7 +122,7 @@ TEST(FactoryStateBlock, creator_H)
 
 TEST(FactoryStateBlock, creator_O_is_quaternion)
 {
-    auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
+    auto sbq = FactoryStateBlock::create("O", Eigen::Vector4d(1,2,3,4), false);
 
     ASSERT_EQ(sbq->getSize()     , 4);
     ASSERT_EQ(sbq->getLocalSize(), 3);
@@ -131,7 +131,7 @@ TEST(FactoryStateBlock, creator_O_is_quaternion)
 
 TEST(FactoryStateBlock, creator_O_is_angle)
 {
-    auto sba = FactoryStateBlock::get().create("O", Eigen::Vector1d(1), false);
+    auto sba = FactoryStateBlock::create("O", Eigen::Vector1d(1), false);
 
     ASSERT_EQ(sba->getSize()     , 1);
     ASSERT_EQ(sba->getLocalSize(), 1);
@@ -140,7 +140,7 @@ TEST(FactoryStateBlock, creator_O_is_angle)
 
 TEST(FactoryStateBlock, creator_O_is_wrong_size)
 {
-    ASSERT_THROW(auto sba = FactoryStateBlock::get().create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+    ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
 }
 
 int main(int argc, char **argv)

test/gtest_state_composite.cpp

@@ -331,20 +331,6 @@ TEST(VectorComposite, unary_Minus)
     ASSERT_MATRIX_APPROX((-x).at("O"), Vector3d(-2,-2,-2), 1e-20);
 }
 
-TEST(VectorComposite, size)
-{
-    VectorComposite x;
-
-    x.emplace("P", Vector2d(1,1));
-    x.emplace("O", Vector3d(2,2,2));
-    x.emplace("V", Vector4d(3,3,3,3));
-
-    ASSERT_EQ(x.size("PO"), 5);
-    ASSERT_EQ(x.size("VO"), 7);
-    ASSERT_EQ(x.size("PVO"), 9);
-    ASSERT_EQ(x.size("OPV"), 9);
-}
-
 TEST(VectorComposite, stateVector)
 {
     VectorComposite x;