diff --git a/CMakeLists.txt b/CMakeLists.txt
index 88f24f03d87d969fcec144f9923fea9917d4fcf3..390e8bb8678f0a735674edc17b1d9167169f355d 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index c440584c0539625d93b4fa4bc5bbafaa1cad1461..a5cce9d4830dbe1879fe78755574f6fd14398132 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/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); \
+ }
}
diff --git a/include/core/state_block/state_block_derived.h b/include/core/state_block/state_block_derived.h
index 541b79a19453f34cedf13343de1caee2d2fca74b..5d1377520098ae65e5a43b361f48d6061266de37 100644
--- a/include/core/state_block/state_block_derived.h
+++ b/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
diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d9548374c7efbff7a3558c95970293810ecf2288
--- /dev/null
+++ b/src/state_block/state_block_derived.cpp
@@ -0,0 +1,80 @@
+
+#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
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 6332a69928a558cfdede9125a73fd7ab7f835338..6e0316e04d72249dbd6d7fd294d5294c6100e20b 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/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);