removed useless constructor and added save/load for LandmarkBase

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

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

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})