Add StatePoint, Vector and Params to the Factory

CMakeLists.txt

@@ -319,6 +319,7 @@ SET(SRCS_STATE_BLOCK
   src/state_block/local_parametrization_homogeneous.cpp
   src/state_block/local_parametrization_quaternion.cpp
   src/state_block/state_block.cpp
+  src/state_block/state_block_derived.cpp
   src/state_block/state_composite.cpp
   )
 SET(SRCS_TRAJECTORY

include/core/state_block/factory_state_block.h

@@ -149,17 +149,19 @@ inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen:
     return (creator_callback_it->second)(_state, _fixed);
 }
 
-#define WOLF_REGISTER_STATEBLOCK(StateBlockType)                                            \
-  namespace{ const bool WOLF_UNUSED StateBlockType##Registered =                            \
-     FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); }   \
-
-#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType)                              \
-  namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key =                   \
-     FactoryStateBlock::registerCreator(#Key, StateBlockType::create); }              \
-
-
-
+#define WOLF_REGISTER_STATEBLOCK(StateBlockType)                                                                      \
+    namespace                                                                                                         \
+    {                                                                                                                 \
+    const bool WOLF_UNUSED StateBlockType##Registered =                                                               \
+        FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create);                                  \
+    }
 
+#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType)                                                        \
+    namespace                                                                                                         \
+    {                                                                                                                 \
+    const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key =                                                      \
+        FactoryStateBlock::registerCreator(#Key, StateBlockType::create);                                             \
+    }
 }
 
 

include/core/state_block/state_block_derived.h

@@ -29,11 +29,11 @@ namespace wolf
 
 /**
  * @brief State block for general parameters
- * 
+ *
  * This state block cannot be transformed with a geometric frame transformation.
- * 
+ *
  * This state block cannot have a local parametrization
- * 
+ *
  * @tparam size the size of the state block's vector
  */
 template <size_t size>
@@ -45,6 +45,7 @@ class StateParams : public StateBlock
     {
         // non transformable! --> do nothing
     }
+    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 typedef StateParams<1>  StateParams1;
@@ -60,11 +61,11 @@ typedef StateParams<10> StateParams10;
 
 /**
  * @brief State block for general 2D points and positions
- * 
+ *
  * This state block can be transformed with a geometric frame transformation.
  * This is controlled at construction time by parameter _transformable,
  * or later via setTransformable(bool).
- * 
+ *
  * This state block cannot have a local parametrization
  */
 class StatePoint2d : public StateBlock
@@ -78,16 +79,17 @@ class StatePoint2d : public StateBlock
     {
         if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
     }
+    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 /**
  * @brief State block for general 2D vectors
- * 
+ *
  * This state block can be transformed with a geometric frame transformation.
  * This is controlled at construction time by parameter _transformable,
  * or later via setTransformable(bool).
  * Being a vector, the geometric frame transformation will rotate the vector.
- * 
+ *
  * This state block cannot have a local parametrization
  */
 class StateVector2d : public StateBlock
@@ -101,15 +103,16 @@ class StateVector2d : public StateBlock
     {
         if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
     }
+    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 /**
  * @brief State block for general 3D points and positions
- * 
+ *
  * This state block can be transformed with a geometric frame transformation.
  * This is controlled at construction time by parameter _transformable,
  * or later via setTransformable(bool).
- * 
+ *
  * This state block cannot have a local parametrization
  */
 class StatePoint3d : public StateBlock
@@ -124,16 +127,17 @@ class StatePoint3d : public StateBlock
         if (transformable_)
             setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
     }
+    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
 /**
  * @brief State block for general 3D vectors
- * 
+ *
  * This state block can be transformed with a geometric frame transformation.
  * This is controlled at construction time by parameter _transformable,
  * or later via setTransformable(bool).
  * Being a vector, the geometric frame transformation will rotate the vector.
- * 
+ *
  * This state block cannot have a local parametrization
  */
 class StateVector3d : public StateBlock
@@ -147,8 +151,10 @@ class StateVector3d : public StateBlock
     {
         if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
     }
+    static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
 };
 
+
 }  // namespace wolf
 
 #endif  // STATE_BLOCK_DERIVED_H_
\ No newline at end of file

src/state_block/state_block_derived.cpp

+
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/factory_state_block.h"
+
+namespace wolf
+{
+StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<2, 1>::check(_state);
+    return std::make_shared<StatePoint2d>(_state, _fixed);
+}
+
+StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<3, 1>::check(_state);
+    return std::make_shared<StatePoint3d>(_state, _fixed);
+}
+
+StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<2, 1>::check(_state);
+    return std::make_shared<StateVector2d>(_state, _fixed);
+}
+
+StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<3, 1>::check(_state);
+    return std::make_shared<StateVector3d>(_state, _fixed);
+}
+
+StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed)
+{
+    assert(_state.size() == 2 or _state.size() == 3 && "Provided _state must be size 2 or 3!");
+    if (_state.size() == 2)
+        return std::make_shared<StatePoint2d>(_state, _fixed);
+    else
+        return std::make_shared<StatePoint3d>(_state, _fixed);
+}
+
+StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed)
+{
+    assert(_state.size() == 2 or _state.size() == 3 && "Provided _state must be size 2 or 3!");
+    if (_state.size() == 2)
+        return std::make_shared<StateVector2d>(_state, _fixed);
+    else
+        return std::make_shared<StateVector3d>(_state, _fixed);
+}
+
+template <size_t size>
+StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fixed)
+{
+    MatrixSizeCheck<size, 1>::check(_state);
+    return std::make_shared<StateParams<size>>(_state, _fixed);
+}
+
+namespace
+{
+const bool __attribute__((used)) create_pointRegisteredWithP =
+    FactoryStateBlock::registerCreator("P", wolf::create_point);
+const bool __attribute__((used)) create_vectorRegisteredWithP =
+    FactoryStateBlock::registerCreator("V", wolf::create_vector);
+}  // namespace
+
+WOLF_REGISTER_STATEBLOCK(StatePoint2d);
+WOLF_REGISTER_STATEBLOCK(StateVector2d);
+WOLF_REGISTER_STATEBLOCK(StatePoint3d);
+WOLF_REGISTER_STATEBLOCK(StateVector3d);
+
+WOLF_REGISTER_STATEBLOCK(StateParams1);
+WOLF_REGISTER_STATEBLOCK(StateParams2);
+WOLF_REGISTER_STATEBLOCK(StateParams3);
+WOLF_REGISTER_STATEBLOCK(StateParams4);
+WOLF_REGISTER_STATEBLOCK(StateParams5);
+WOLF_REGISTER_STATEBLOCK(StateParams6);
+WOLF_REGISTER_STATEBLOCK(StateParams7);
+WOLF_REGISTER_STATEBLOCK(StateParams8);
+WOLF_REGISTER_STATEBLOCK(StateParams9);
+WOLF_REGISTER_STATEBLOCK(StateParams10);
+
+}  // namespace wolf
\ No newline at end of file

test/gtest_factory_state_block.cpp

@@ -31,7 +31,7 @@
 #include "core/common/wolf.h"
 #include "core/state_block/factory_state_block.h"
 #include "core/sensor/factory_sensor.h"
-
+#include "core/state_block/state_block_derived.h"
 
 
 using namespace wolf;
@@ -165,6 +165,109 @@ TEST(FactoryStateBlock, creator_O_is_wrong_size)
     ASSERT_THROW(auto sba = FactoryStateBlock::create("O", Eigen::Vector2d(1,2), false) , std::length_error);
 }
 
+TEST(FactoryStateBlock, creator_Point)
+{
+    auto sba = FactoryStateBlock::create("StatePoint2d", Eigen::Vector2d(1,2), false);
+
+    ASSERT_EQ(sba->getSize()     , 2);
+    ASSERT_EQ(sba->getLocalSize(), 2);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    sba = FactoryStateBlock::create("StatePoint3d", Eigen::Vector3d(1,2,3), false);
+
+    ASSERT_EQ(sba->getSize()     , 3);
+    ASSERT_EQ(sba->getLocalSize(), 3);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    // fails
+    // ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_P)
+{
+    auto sba = FactoryStateBlock::create("P", Eigen::Vector2d(1,2), false);
+
+    ASSERT_EQ(sba->getSize()     , 2);
+    ASSERT_EQ(sba->getLocalSize(), 2);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    sba = FactoryStateBlock::create("P", Eigen::Vector3d(1,2,3), false);
+
+    ASSERT_EQ(sba->getSize()     , 3);
+    ASSERT_EQ(sba->getLocalSize(), 3);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    // fails
+    // ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_Vector)
+{
+    auto sba = FactoryStateBlock::create("StateVector2d", Eigen::Vector2d(1,2), false);
+
+    ASSERT_EQ(sba->getSize()     , 2);
+    ASSERT_EQ(sba->getLocalSize(), 2);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    sba = FactoryStateBlock::create("StateVector3d", Eigen::Vector3d(1,2,3), false);
+
+    ASSERT_EQ(sba->getSize()     , 3);
+    ASSERT_EQ(sba->getLocalSize(), 3);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    // fails
+    // ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_V)
+{
+    auto sba = FactoryStateBlock::create("V", Eigen::Vector2d(1,2), false);
+
+    ASSERT_EQ(sba->getSize()     , 2);
+    ASSERT_EQ(sba->getLocalSize(), 2);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    sba = FactoryStateBlock::create("V", Eigen::Vector3d(1,2,3), false);
+
+    ASSERT_EQ(sba->getSize()     , 3);
+    ASSERT_EQ(sba->getLocalSize(), 3);
+    ASSERT_FALSE(sba->hasLocalParametrization());
+
+    // fails
+    // ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error);
+}
+
+TEST(FactoryStateBlock, creator_Params)
+{
+    auto sb1  = FactoryStateBlock::create("StateParams1", Eigen::Vector1d(1), false);
+    auto sb2  = FactoryStateBlock::create("StateParams2", Eigen::Vector2d(1, 2), false);
+    auto sb3  = FactoryStateBlock::create("StateParams3", Eigen::Vector3d(1, 2, 3), false);
+    auto sb4  = FactoryStateBlock::create("StateParams4", Eigen::Vector4d(1, 2, 3, 4), false);
+    auto sb5  = FactoryStateBlock::create("StateParams5", Eigen::Vector5d(1, 2, 3, 4, 5), false);
+    auto sb6  = FactoryStateBlock::create("StateParams6", Eigen::Vector6d(1, 2, 3, 4, 5, 6), false);
+    auto sb7  = FactoryStateBlock::create("StateParams7", Eigen::Vector7d(1, 2, 3, 4, 5, 6, 7), false);
+    auto sb8  = FactoryStateBlock::create("StateParams8", Eigen::Vector8d(1, 2, 3, 4, 5, 6, 7, 8), false);
+    auto sb9  = FactoryStateBlock::create("StateParams9", Eigen::Vector9d(1, 2, 3, 4, 5, 6, 7, 8, 9), false);
+    auto sb10 = FactoryStateBlock::create("StateParams10", Eigen::Vector10d(1, 2, 3, 4, 5, 6, 7, 8, 9, 10), false);
+
+    ASSERT_EQ(sb1->getSize(), 1);
+    ASSERT_EQ(sb2->getSize(), 2);
+    ASSERT_EQ(sb3->getSize(), 3);
+    ASSERT_EQ(sb4->getSize(), 4);
+    ASSERT_EQ(sb5->getSize(), 5);
+    ASSERT_EQ(sb6->getSize(), 6);
+    ASSERT_EQ(sb7->getSize(), 7);
+    ASSERT_EQ(sb8->getSize(), 8);
+    ASSERT_EQ(sb9->getSize(), 9);
+    ASSERT_EQ(sb10->getSize(), 10);
+
+    ASSERT_EQ(sb1->getLocalSize(), 1);
+    ASSERT_FALSE(sb1->hasLocalParametrization());
+
+    // fails to compile (static asserts)
+    // ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error);
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);