Merge branch 'devel' into 336-processor-motion-composite-state-adaptation

demos/hello_wolf/capture_range_bearing.h

@@ -21,7 +21,7 @@ class CaptureRangeBearing : public CaptureBase
 {
     public:
         CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
-        virtual ~CaptureRangeBearing();
+        ~CaptureRangeBearing() override;
 
         const VectorXi&         getIds          ()          const;
         const int&              getId           (int _i)    const;

demos/hello_wolf/factor_range_bearing.h

@@ -45,12 +45,12 @@ class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2,
             //
         }
 
-        virtual ~FactorRangeBearing()
+        ~FactorRangeBearing() override
         {
             //
         }
 
-        virtual std::string getTopology() const override
+        std::string getTopology() const override
         {
             return "LMK";
         }

demos/hello_wolf/feature_range_bearing.h

@@ -19,7 +19,7 @@ class FeatureRangeBearing : public FeatureBase
 {
     public:
         FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
-        virtual ~FeatureRangeBearing();
+        ~FeatureRangeBearing() override;
 };
 
 } // namespace wolf

demos/hello_wolf/landmark_point_2d.h

@@ -19,7 +19,7 @@ class LandmarkPoint2d : public LandmarkBase
 {
     public:
         LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
-        virtual ~LandmarkPoint2d();
+        ~LandmarkPoint2d() override;
 };
 
 } /* namespace wolf */

demos/hello_wolf/processor_range_bearing.h

@@ -46,31 +46,31 @@ class ProcessorRangeBearing : public ProcessorBase
         typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
 
         ProcessorRangeBearing(ParamsProcessorBasePtr _params);
-        virtual ~ProcessorRangeBearing() {/* empty */}
-        virtual void configure(SensorBasePtr _sensor) override;
+        ~ProcessorRangeBearing() override {/* empty */}
+        void configure(SensorBasePtr _sensor) override;
 
         // Factory method for high level API
         WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
 
     protected:
         // Implementation of pure virtuals from ProcessorBase
-        virtual void processCapture     (CaptureBasePtr _capture) override;
-        virtual void processKeyFrame    (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
-        virtual bool triggerInCapture   (CaptureBasePtr) const override { return true;};
-        virtual bool triggerInKeyFrame  (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
-        virtual bool voteForKeyFrame    () const override {return false;}
+        void processCapture     (CaptureBasePtr _capture) override;
+        void processKeyFrame    (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+        bool triggerInCapture   (CaptureBasePtr) const override { return true;};
+        bool triggerInKeyFrame  (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+        bool voteForKeyFrame    () const override {return false;}
 
         /** \brief store key frame
         *
         * Returns true if the key frame should be stored
         */
-        virtual bool storeKeyFrame(FrameBasePtr) override;
+        bool storeKeyFrame(FrameBasePtr) override;
 
         /** \brief store capture
         *
         * Returns true if the capture should be stored
         */
-        virtual bool storeCapture(CaptureBasePtr) override;
+        bool storeCapture(CaptureBasePtr) override;
 
     private:
         // control variables

demos/hello_wolf/sensor_range_bearing.h

@@ -45,7 +45,7 @@ class SensorRangeBearing : public SensorBase
         SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
         SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
         WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
-        virtual ~SensorRangeBearing();
+        ~SensorRangeBearing() override;
 
 };
 

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/factor/factor_odom_2d_closeloop.h

@@ -35,7 +35,7 @@ class FactorOdom2dCloseloop : public FactorAutodiff<FactorOdom2dCloseloop, 3, 2,
 
         virtual ~FactorOdom2dCloseloop() = default;
 
-        virtual std::string getTopology() const override
+        std::string getTopology() const override
         {
             return std::string("LOOP");
         }

include/core/frame/frame_base.h

@@ -63,12 +63,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
                   StateBlockPtr _o_ptr = nullptr,
                   StateBlockPtr _v_ptr = nullptr);
 
-        FrameBase(const FrameType & _tp,
-                  const TimeStamp& _ts,
-                  const std::string _frame_structure,
-                  const SizeEigen _dim,
-                  const Eigen::VectorXd& _x);
-
         FrameBase(const FrameType & _tp,
                   const TimeStamp& _ts,
                   const std::string _frame_structure,

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);
 
@@ -89,65 +89,6 @@ FrameBase::FrameBase(const FrameType & _tp,
     }
 }
 
-FrameBase::FrameBase(const FrameType & _tp,
-                     const TimeStamp& _ts,
-                     const std::string _frame_structure,
-                     const SizeEigen _dim,
-                     const Eigen::VectorXd& _x) :
-           NodeBase("FRAME", "FrameBase"),
-           HasStateBlocks(_frame_structure),
-           trajectory_ptr_(),
-           frame_id_(++frame_id_count_),
-           type_(_tp),
-           time_stamp_(_ts)
-{
-    if(_frame_structure.compare("PO") == 0 and _dim == 2){
-        assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for PO 2D!");
-        StateBlockPtr p_ptr ( std::make_shared<StateBlock>    (_x.head    <2> ( ) ) );
-        StateBlockPtr o_ptr ( std::make_shared<StateAngle>    ((double) _x(2) ) );
-        addStateBlock("P", p_ptr);
-        addStateBlock("O", o_ptr);
-        this->setType("PO 2d");
-    }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
-        assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for PO 3D!");
-        StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> ( ) ) );
-        StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail    <4> ( ) ) );
-        addStateBlock("P", p_ptr);
-        addStateBlock("O", o_ptr);
-        this->setType("PO 3d");
-    }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
-        // auto _x = Eigen::VectorXd::Zero(10);
-        assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 3D!");
-        StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> ( ) ) );
-        StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
-        StateBlockPtr v_ptr ( std::make_shared<StateBlock>      (_x.tail    <3> ( ) ) );
-        addStateBlock("P", p_ptr);
-        addStateBlock("O", o_ptr);
-        addStateBlock("V", v_ptr);
-        this->setType("POV 3d");
-    }else if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
-        // auto _x = Eigen::VectorXd::Zero(10);
-        assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 3D!");
-        StateBlockPtr p_ptr ( std::make_shared<StateBlock>      (_x.head    <3> (  ) ) );
-        StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3 ) ) );
-        StateBlockPtr v_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (7 ) ) );
-        StateBlockPtr c_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (10) ) );
-        StateBlockPtr cd_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (13) ) );
-        StateBlockPtr Lc_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (16) ) );
-        addStateBlock("P", p_ptr);
-        addStateBlock("O", o_ptr);
-        addStateBlock("V", v_ptr);
-        addStateBlock("C", c_ptr);
-        addStateBlock("D", cd_ptr);
-        addStateBlock("L", Lc_ptr);
-        this->setType("POVCDL 3d");
-    }else{
-        std::cout << _frame_structure << " ^^ " << _dim << std::endl;
-        throw std::runtime_error("Unknown frame structure");
-    }
-
-
-}
 
 FrameBase::~FrameBase()
 {

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();
@@ -331,12 +331,35 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
                                    const SizeEigen _dim, //
                                    const Eigen::VectorXd& _frame_state)
 {
+    VectorComposite state;
+    SizeEigen index = 0;
+    SizeEigen size = 0;
+    for (const auto& ckey : _frame_structure)
+    {
+        const auto& key = string(1,ckey); // ckey is char
+
+        if (key == "O")
+        {
+            if (_dim == 2) size = 1;
+            else size = 4;
+        }
+        else
+            size = _dim;
+
+        assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small.");
+
+        state.emplace(key, _frame_state.segment(index, size));
+
+        index += size;
+    }
+
+    assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large.");
+
     auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_,
                                              _frame_key_type,
                                              _time_stamp,
                                              _frame_structure,
-                                             _dim,
-                                             _frame_state);
+                                             state);
     return frm;
 }
 
@@ -440,6 +463,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 +507,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;
 }