New FactoryStateBlock

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

include/core/state_block/factory_state_block.h

+/*
+ * 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_ */

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_ */

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

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_ */

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_ */

src/state_block/state_block.cpp

@@ -55,3 +55,31 @@ 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);
+
+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); }
+}

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)

test/gtest_factory_state_block.cpp

+/*
+ * 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);
+}
+
+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), false);
+    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);
+}
+
+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_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();
+}