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(); +}