diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3cfdcb44eff879f3ea3934c3a79fc9ec6c4f4fd5..75b2269f91251bf68673b37ba86962ee03878e1a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -209,6 +209,7 @@ SET(HDRS_FRAME
   include/core/frame/frame_base.h
   )
 SET(HDRS_STATE_BLOCK
+  include/core/state_block/factory_state_block.h
   include/core/state_block/has_state_blocks.h
   include/core/state_block/local_parametrization_angle.h
   include/core/state_block/local_parametrization_base.h
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index d97810fe3f6fb83f14ee2e802ea8f07b2049485f..1510362e65c0a457f8982eec5e8e5cc130798668 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -244,7 +244,7 @@ class Factory
         bool registerCreator(const std::string& _type, CreatorCallback createFn);
         bool unregisterCreator(const std::string& _type);
         TypeBasePtr create(const std::string& _type, TypeInput... _input);
-        std::string getClass();
+        std::string getClass() const;
 
     private:
         CallbackMap callbacks_;
@@ -301,7 +301,7 @@ inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
 }
 
 template<class TypeBase, typename... TypeInput>
-inline std::string Factory<TypeBase, TypeInput...>::getClass()
+inline std::string Factory<TypeBase, TypeInput...>::getClass() const
 {
     return "Factory<class TypeBase>";
 }
@@ -319,7 +319,7 @@ class LandmarkBase;
 typedef Factory<LandmarkBase,
         const YAML::Node&>  FactoryLandmark;
 template<>
-inline std::string FactoryLandmark::getClass()
+inline std::string FactoryLandmark::getClass() const
 {
     return "FactoryLandmark";
 }
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 738d3558a41124377dffa39731ed1c859d09f873..5fc2f8f14ccffc52653c6f43fa0c07fdb03be825 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -170,7 +170,7 @@ struct ParamsProcessorBase;
 typedef Factory<ParamsProcessorBase,
         const std::string&> FactoryParamsProcessor;
 template<>
-inline std::string FactoryParamsProcessor::getClass()
+inline std::string FactoryParamsProcessor::getClass() const
 {
     return "FactoryParamsProcessor";
 }
@@ -180,7 +180,7 @@ typedef Factory<ProcessorBase,
         const std::string&,
         const ParamsProcessorBasePtr> FactoryProcessor;
 template<>
-inline std::string FactoryProcessor::getClass()
+inline std::string FactoryProcessor::getClass() const
 {
   return "FactoryProcessor";
 }
@@ -193,7 +193,7 @@ typedef Factory<ProcessorBase,
         const std::string&,
         const ParamsServer&> AutoConfFactoryProcessor;
 template<>
-inline std::string AutoConfFactoryProcessor::getClass()
+inline std::string AutoConfFactoryProcessor::getClass() const
 {
     return "AutoConfFactoryProcessor";
 }
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 67ef5f597108f7ddc3bc264fb1c0cbc218bdba0a..ff0574aaadc6a52015a6c9e4f076a1ec64d968c0 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -212,9 +212,9 @@ namespace wolf
 // ParamsSensor factory
 struct ParamsSensorBase;
 typedef Factory<ParamsSensorBase,
-        const std::string&> FactoryParamsSensor;
+                const std::string&> FactoryParamsSensor;
 template<>
-inline std::string FactoryParamsSensor::getClass()
+inline std::string FactoryParamsSensor::getClass() const
 {
     return "FactoryParamsSensor";
 }
@@ -222,10 +222,11 @@ inline std::string FactoryParamsSensor::getClass()
 // Sensor factory
 typedef Factory<SensorBase,
                 const std::string&,
-                const Eigen::VectorXd&, const ParamsSensorBasePtr> FactorySensor;
+                const Eigen::VectorXd&,
+                const ParamsSensorBasePtr> FactorySensor;
 
 template<>
-inline std::string FactorySensor::getClass()
+inline std::string FactorySensor::getClass() const
 {
   return "FactorySensor";
 }
@@ -240,7 +241,7 @@ typedef Factory<SensorBase,
                 const ParamsServer&> AutoConfFactorySensor;
 
 template<>
-inline std::string AutoConfFactorySensor::getClass()
+inline std::string AutoConfFactorySensor::getClass() const
 {
   return "AutoConfFactorySensor";
 }
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index b055217828d887169f8e732138b43a4f2067c62a..e3cc58a4a0b6d269026312f9c946a490d1045923 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -214,7 +214,7 @@ typedef Factory<SolverManager,
                 const ParamsServer&> FactorySolver;
 
 template<>
-inline std::string FactorySolver::getClass()
+inline std::string FactorySolver::getClass() const
 {
   return "FactorySolver";
 }
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
new file mode 100644
index 0000000000000000000000000000000000000000..c353121b724011b993942d5a567166963a7440d0
--- /dev/null
+++ b/include/core/state_block/factory_state_block.h
@@ -0,0 +1,51 @@
+/*
+ * factory_state_block.h
+ *
+ *  Created on: Apr 27, 2020
+ *      Author: jsola
+ */
+
+#ifndef STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+#define STATE_BLOCK_FACTORY_STATE_BLOCK_H_
+
+#include "core/common/factory.h"
+#include "core/state_block/state_block.h"
+
+namespace wolf
+{
+
+// State blocks factory
+typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock;
+template<>
+inline std::string FactoryStateBlock::getClass() const
+{
+    return "FactoryStateBlock";
+}
+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);
+
+    if (creator_callback_it == callbacks_.end())
+        // not found: return StateBlock base
+        return std::make_shared<StateBlock>(_state, _fixed);
+
+    // Invoke the creation function
+    return (creator_callback_it->second)(_state, _fixed);
+}
+
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType)                                            \
+  namespace{ const bool WOLF_UNUSED StateBlockType##Registered =                            \
+     FactoryStateBlock::get().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); }              \
+
+
+
+
+}
+
+
+#endif /* STATE_BLOCK_FACTORY_STATE_BLOCK_H_ */
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 05a87f0f25bb2af1466da5a5a5b4608b64f2a883..8b714b64c8b3bd82699fab1d845887e2bd74e10c 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.h
@@ -20,6 +20,8 @@ class StateAngle : public StateBlock
         StateAngle(double _angle = 0.0, bool _fixed = false);
 
         virtual ~StateAngle();
+
+        static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 inline StateAngle::StateAngle(double _angle, bool _fixed) :
@@ -33,6 +35,12 @@ inline StateAngle::~StateAngle()
     //
 }
 
+inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<1,1>::check(_state);
+    return std::make_shared<StateAngle>(_state(0), _fixed);
+}
+
 } /* namespace wolf */
 
 #endif /* STATE_ANGLE_H_ */
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index ab3bb7147f3c81f57a3436216412274394acc04a..5d3aa1fc2e9694211d388592c8aa2605cee00824 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -157,6 +157,11 @@ public:
         /** \brief Remove this state_block from the problem
          **/
         //void removeFromProblem(ProblemPtr _problem_ptr);
+
+        /** \brief Factory creator
+         *
+         */
+        static StateBlockPtr create (const Eigen::VectorXd& _state, bool _fixed = false);
 };
 
 } // namespace wolf
@@ -292,6 +297,11 @@ inline double* StateBlock::getStateData()
     return state_.data();
 }
 
+inline StateBlockPtr StateBlock::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+    return std::make_shared<StateBlock>(_state, _fixed);
+}
+
 }// namespace wolf
 
 #endif
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index 4760a52b7faa2bf188b520534b8512722d209f63..f9a0c099841c025671bff1b51d50df86fccff51a 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.h
@@ -19,6 +19,8 @@ class StateHomogeneous3d : public StateBlock
         StateHomogeneous3d(bool _fixed = false);
         StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false);
         virtual ~StateHomogeneous3d();
+
+        static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed) :
@@ -40,6 +42,12 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
     // The local_param_ptr_ pointer is already removed by the base class
 }
 
+inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<4,1>::check(_state);
+    return std::make_shared<StateHomogeneous3d>(_state, _fixed);
+}
+
 } // namespace wolf
 
 #endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 3732201633ca04eeac5815b9948c8e9d7261f852..817d436aef72c9b128ce97dd8569f380a575ce1b 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -20,6 +20,8 @@ class StateQuaternion : public StateBlock
         StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
         StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
         virtual ~StateQuaternion();
+
+        static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
@@ -45,6 +47,12 @@ inline StateQuaternion::~StateQuaternion()
     // The local_param_ptr_ pointer is already removed by the base class
 }
 
+inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<4,1>::check(_state);
+    return std::make_shared<StateQuaternion>(_state, _fixed);
+}
+
 } // namespace wolf
 
 #endif /* SRC_STATE_QUATERNION_H_ */
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index 91f2b9531d91bdad952760a0670ceece4240e86a..a56dae93e138c0f99cc51f73884a50be9f252707 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -23,7 +23,7 @@ struct ParamsTreeManagerBase;
 typedef Factory<ParamsTreeManagerBase,
         const std::string&> FactoryParamsTreeManager;
 template<>
-inline std::string FactoryParamsTreeManager::getClass()
+inline std::string FactoryParamsTreeManager::getClass() const
 {
     return "FactoryParamsTreeManager";
 }
@@ -33,7 +33,7 @@ typedef Factory<TreeManagerBase,
         const std::string&,
         const ParamsTreeManagerBasePtr> FactoryTreeManager;
 template<>
-inline std::string FactoryTreeManager::getClass()
+inline std::string FactoryTreeManager::getClass() const
 {
   return "FactoryTreeManager";
 }
@@ -46,7 +46,7 @@ typedef Factory<TreeManagerBase,
         const std::string&,
         const ParamsServer&> AutoConfFactoryTreeManager;
 template<>
-inline std::string AutoConfFactoryTreeManager::getClass()
+inline std::string AutoConfFactoryTreeManager::getClass() const
 {
     return "AutoConfFactoryTreeManager";
 }
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 783a56a5e8cb52fe0ae79e2ad14ca4b50976a48c..030febd46ba50917d8681bd2ce3bb6ee8be55e23 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -55,3 +55,32 @@ void StateBlock::perturb(double amplitude)
 }
 
 }
+
+#include "core/state_block/factory_state_block.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/state_block/state_homogeneous_3d.h"
+
+namespace wolf{
+WOLF_REGISTER_STATEBLOCK(StateBlock);
+WOLF_REGISTER_STATEBLOCK(StateQuaternion);
+WOLF_REGISTER_STATEBLOCK(StateAngle);
+WOLF_REGISTER_STATEBLOCK(StateHomogeneous3d);
+WOLF_REGISTER_STATEBLOCK_WITH_KEY(H, StateHomogeneous3d);
+
+StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
+{
+    if (_state.size() == 1)
+        return StateAngle::create(_state, _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.");
+
+    return nullptr;
+}
+
+//WOLF_REGISTER_STATEBLOCK_WITH_KEY(O, wolf::create_orientation);
+namespace{ const bool __attribute__((used)) create_orientationRegisteredWithO =                   \
+     FactoryStateBlock::get().registerCreator("O", wolf::create_orientation); }
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index dc7cfae429f38b83a414229f78a70b01cc5ba53f..b0309585c0a4a1a16b2ce0582669e2e75cf41d68 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -47,6 +47,10 @@ add_library(dummy ${SRC_DUMMY})
 wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
 target_link_libraries(gtest_capture_base ${PROJECT_NAME})
 
+# Converter from String to various types used by the parameters server
+wolf_add_gtest(gtest_converter gtest_converter.cpp)
+target_link_libraries(gtest_converter ${PROJECT_NAME})
+
 # FactorBase class test
 wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
 target_link_libraries(gtest_factor_base ${PROJECT_NAME})
@@ -55,9 +59,9 @@ target_link_libraries(gtest_factor_base ${PROJECT_NAME})
 wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
 target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
 
-# Converter from String to various types used by the parameters server
-wolf_add_gtest(gtest_converter gtest_converter.cpp)
-target_link_libraries(gtest_converter ${PROJECT_NAME})
+# FactoryStateBlock factory test
+wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
+target_link_libraries(gtest_factory_state_block ${PROJECT_NAME})
 
 # FeatureBase classes test
 wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbded073c2e9a226c0f66b1feb34388bd5c06dfc
--- /dev/null
+++ b/test/gtest_factory_state_block.cpp
@@ -0,0 +1,157 @@
+/*
+ * gtest_factory_state_block.cpp
+ *
+ *  Created on: Apr 28, 2020
+ *      Author: jsola
+ */
+
+#include "core/common/wolf.h"
+#include "core/state_block/factory_state_block.h"
+#include "core/sensor/factory_sensor.h"
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+using namespace wolf;
+
+/*
+// You may use this to make some methods of Foo public
+WOLF_PTR_TYPEDEFS(FooPublic);
+class FooPublic : public Foo
+{
+    // You may use this to make some methods of Foo public
+}
+
+class TestInit : public testing::Test
+{
+    public:
+        // You may use this to initialize stuff
+        // Combine it with TEST_F(FooTest, testName) { }
+        void SetUp() override
+        {
+            // Init all you want here
+            // e.g. FooPublic foo;
+        }
+        void TearDown() override {} // you can delete this if unused
+};
+
+TEST_F(TestInit, testName)
+{
+    // Use class TestInit
+}
+*/
+
+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);
+
+    ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
+    ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5)   , sbv->getState(), 1e-20);
+    ASSERT_MATRIX_APPROX(Eigen::Vector1d(6)     , sbw->getState(), 1e-20);
+
+    ASSERT_FALSE(sbp->hasLocalParametrization());
+    ASSERT_FALSE(sbv->hasLocalParametrization());
+    ASSERT_FALSE(sbw->hasLocalParametrization());
+}
+
+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);
+
+    ASSERT_MATRIX_APPROX(Eigen::Vector3d(1,2,3) , sbp->getState(), 1e-20);
+    ASSERT_MATRIX_APPROX(Eigen::Vector2d(4,5)   , sbv->getState(), 1e-20);
+    ASSERT_MATRIX_APPROX(Eigen::Vector1d(6)     , sbw->getState(), 1e-20);
+
+    ASSERT_FALSE(sbp->isFixed());
+    ASSERT_TRUE (sbv->isFixed());
+    ASSERT_FALSE(sbw->isFixed());
+
+    ASSERT_FALSE(sbp->hasLocalParametrization());
+    ASSERT_FALSE(sbv->hasLocalParametrization());
+    ASSERT_FALSE(sbw->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Quaternion)
+{
+    auto sbq = FactoryStateBlock::get().create("StateQuaternion", Eigen::Vector4d(1,2,3,4), false);
+
+    ASSERT_EQ(sbq->getSize()     , 4);
+    ASSERT_EQ(sbq->getLocalSize(), 3);
+    ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Angle)
+{
+    auto sba = FactoryStateBlock::get().create("StateAngle", Eigen::Vector1d(1), false);
+
+    ASSERT_EQ(sba->getSize()     , 1);
+    ASSERT_EQ(sba->getLocalSize(), 1);
+    ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_Homogeneous3d)
+{
+    auto sbh = FactoryStateBlock::get().create("StateHomogeneous3d", Eigen::Vector4d(1,2,3,4), false);
+
+    ASSERT_EQ(sbh->getSize()     , 4);
+    ASSERT_EQ(sbh->getLocalSize(), 3);
+    ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_H)
+{
+    auto sbh = FactoryStateBlock::get().create("H", Eigen::Vector4d(1,2,3,4), false);
+
+    ASSERT_EQ(sbh->getSize()     , 4);
+    ASSERT_EQ(sbh->getLocalSize(), 3);
+    ASSERT_TRUE(sbh->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_quaternion)
+{
+    auto sbq = FactoryStateBlock::get().create("O", Eigen::Vector4d(1,2,3,4), false);
+
+    ASSERT_EQ(sbq->getSize()     , 4);
+    ASSERT_EQ(sbq->getLocalSize(), 3);
+    ASSERT_TRUE(sbq->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_angle)
+{
+    auto sba = FactoryStateBlock::get().create("O", Eigen::Vector1d(1), false);
+
+    ASSERT_EQ(sba->getSize()     , 1);
+    ASSERT_EQ(sba->getLocalSize(), 1);
+    ASSERT_TRUE(sba->hasLocalParametrization());
+}
+
+TEST(FactoryStateBlock, creator_O_is_wrong_size)
+{
+    ASSERT_THROW(auto sba = FactoryStateBlock::get().create("O", Eigen::Vector2d(1,2), false) , std::length_error);
+}
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+
+    // restrict to a group of tests only
+    //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+    // restrict to this test only
+    //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+    return RUN_ALL_TESTS();
+}