wip

include/core/map/factory_map.h

@@ -155,8 +155,6 @@ namespace wolf
  *
  */
 typedef Factory<MapBase,
-                const std::string&,
-                const Eigen::VectorXd&,
                 const ParamsMapBasePtr> FactoryMap;
 
 template<>
@@ -181,7 +179,6 @@ inline std::string FactoryParamsMap::getClass() const
 
 
 typedef Factory<MapBase,
-                const std::string&,
                 const ParamsServer&> AutoConfFactoryMap;
 
 template<>

include/core/problem/problem.h

@@ -329,6 +329,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;
+        void setMap(MapBasePtr);
         void loadMap(const std::string& _filename_dot_yaml);
         void saveMap(const std::string& _filename_dot_yaml, //
                      const std::string& _map_name = "Map automatically saved by Wolf");

src/problem/problem.cpp

@@ -3,7 +3,10 @@
 #include "core/hardware/hardware_base.h"
 #include "core/trajectory/trajectory_base.h"
 #include "core/map/map_base.h"
+#include "core/map/factory_map.h"
 #include "core/sensor/sensor_base.h"
+#include "core/sensor/factory_sensor.h"
+#include "core/processor/factory_processor.h"
 #include "core/processor/processor_motion.h"
 #include "core/processor/processor_tracker.h"
 #include "core/capture/capture_pose.h"
@@ -11,8 +14,6 @@
 #include "core/factor/factor_base.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
 #include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
@@ -44,7 +45,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
         tree_manager_(nullptr),
         hardware_ptr_(std::make_shared<HardwareBase>()),
         trajectory_ptr_(std::make_shared<TrajectoryBase>()),
-        map_ptr_(std::make_shared<MapBase>()),
+        map_ptr_(nullptr),
         processor_is_motion_map_(),
         frame_structure_(_frame_structure),
         prior_options_(std::make_shared<PriorOptions>())
@@ -75,6 +76,7 @@ void Problem::setup()
 {
     hardware_ptr_  -> setProblem(shared_from_this());
     trajectory_ptr_-> setProblem(shared_from_this());
+    map_ptr_ = std::make_shared<MapBase>(std::make_shared<ParamsMapBase>());
     map_ptr_       -> setProblem(shared_from_this());
 }
 
@@ -200,6 +202,13 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
                                  _server.getParam<VectorComposite>("problem/prior/state"));
     }
 
+    // Map
+    std::string map_type = _server.getParam<std::string>("problem/map/type");
+    WOLF_TRACE("Map Type: ", map_type);
+    auto map = AutoConfFactoryMap::create(map_type, _server);
+    map->setProblem(problem);
+    problem->setMap(map);
+
     // Done
     return problem;
 }
@@ -988,6 +997,13 @@ MapBasePtr Problem::getMap() const
     return map_ptr_;
 }
 
+void Problem::setMap(MapBasePtr _map_ptr)
+{
+    assert(map_ptr_ == nullptr && "map has already been set");
+
+    map_ptr_ = _map_ptr;
+}
+
 TrajectoryBasePtr Problem::getTrajectory() const
 {
     return trajectory_ptr_;