diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 839ee0f97c9694da18cff81b17e78ce853b7e934..373b20149b9202ca61fcfb1c1ea4d265e284f63f 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -44,8 +44,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma * \param _o_ptr StateBlock pointer to the orientation (default: nullptr) * **/ - LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr); - LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr); + LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr); virtual ~LandmarkBase(); virtual void remove(); virtual YAML::Node saveToYaml() const; @@ -96,6 +95,11 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma template<typename classType, typename... T> static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all); + /** \brief Creator for Factory<LandmarkBase, YAML::Node> + * Caution: This creator does not set the landmark's anchor frame and sensor. + * These need to be set afterwards. + */ + static LandmarkBasePtr create(const YAML::Node& _node); }; } diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 5bdfff041cc7a9bf8b2020b779736e61fac97da1..b4aa39530bc0e59dd71b62f552e124236ce77867 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -3,17 +3,19 @@ #include "core/factor/factor_base.h" #include "core/map/map_base.h" #include "core/state_block/state_block.h" +#include "core/state_block/state_quaternion.h" +#include "core/common/factory.h" #include "core/yaml/yaml_conversion.h" namespace wolf { unsigned int LandmarkBase::landmark_id_count_ = 0; - LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : - NodeBase("LANDMARK", _type), - map_ptr_(), - state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed. - landmark_id_(++landmark_id_count_) +LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : + NodeBase("LANDMARK", _type), + map_ptr_(), + state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed. + landmark_id_(++landmark_id_count_) { state_block_vec_[0] = _p_ptr; state_block_vec_[1] = _o_ptr; @@ -21,17 +23,6 @@ unsigned int LandmarkBase::landmark_id_count_ = 0; // std::cout << "constructed +L" << id() << std::endl; } - LandmarkBase::LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : - NodeBase("LANDMARK", _type), - map_ptr_(_ptr), - state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed. - landmark_id_(++landmark_id_count_) - { - state_block_vec_[0] = _p_ptr; - state_block_vec_[1] = _o_ptr; - - // std::cout << "constructed +L" << id() << std::endl; - } LandmarkBase::~LandmarkBase() { removeStateBlocks(); @@ -170,6 +161,7 @@ YAML::Node LandmarkBase::saveToYaml() const } return node; } + void LandmarkBase::link(MapBasePtr _map_ptr) { if(_map_ptr) @@ -192,4 +184,37 @@ FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr) return _fac_ptr; } +LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) +{ + unsigned int id = _node["id"] .as< unsigned int >(); + Eigen::VectorXs pos = _node["position"] .as< Eigen::VectorXs >(); + bool pos_fixed = _node["position fixed"] .as< bool >(); + std::string type = _node["type"] .as< std::string >(); + + StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed); + StateBlockPtr ori_sb = nullptr; + + if (!_node["orientation"].as<std::string>().empty()) + { + Eigen::VectorXs ori = _node["orientation"].as< Eigen::VectorXs >(); + bool ori_fixed = _node["orientation fixed"].as< bool >(); + + if (ori.size() == 4) + ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed); + else + ori_sb = std::make_shared<StateBlock>(ori, ori_fixed); + } + + LandmarkBasePtr lmk = std::make_shared<LandmarkBase>(type, pos_sb, ori_sb); + lmk->setId(id); + + return lmk; +} + +// Register landmark creator +namespace +{ +const bool WOLF_UNUSED registered_lmk_ahp = LandmarkFactory::get().registerCreator("BASE", LandmarkBase::create); +} + } // namespace wolf diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b35296c543a5526399714d0b23b2a8908a5a39ce..eafc4f5ae3984fab6c526e0d8454e66aa0ae3032 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -175,12 +175,10 @@ target_link_libraries(gtest_make_posdef ${PROJECT_NAME}) wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) target_link_libraries(gtest_param_prior ${PROJECT_NAME}) - # ProcessorFrameNearestNeighborFilter class test wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp) target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME}) - # ProcessorMotion in 2D wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp) target_link_libraries(gtest_odom_2D ${PROJECT_NAME})