diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 622efcdf1b091eac66e44451a1054e8012372253..f981f498a6dd8b7aa30c9b638870785b44ecd04f 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -72,6 +72,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
LandmarkBasePtrList landmark_list_;
public:
+ MapBase();
MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
WOLF_MAP_CREATE(MapBase, ParamsMapBase);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 083e6a93242484b39d52af49af8b154ef0f3400a..7004a205cabee71ae225dbae91b9a047431e7a65 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -69,11 +69,11 @@ class Problem : public std::enable_shared_from_this<Problem>
PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
+ Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
public:
- static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
+ static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 5d5433fc4c45bb39fc554d805e0485a716c2956b..e263f9ad02d4f66417cbddf31ce8da280a1d3ec4 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -15,6 +15,12 @@
namespace wolf {
+MapBase::MapBase() :
+ NodeBase("MAP", "Base")
+{
+// std::cout << "constructed M"<< std::endl;
+}
+
MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
NodeBase("MAP", _type)
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f4cdc9dfbd931c2673bc9052681e6cca924a2386..8258db880b80a7def50a5dde2d6e623efacfbd87 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -41,11 +41,11 @@
namespace wolf
{
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
- map_ptr_(nullptr),
+ map_ptr_(_map),
processor_is_motion_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
@@ -76,13 +76,13 @@ 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());
+ if (map_ptr_)
+ map_ptr_ -> setProblem(shared_from_this());
}
-ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
+ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
{
- ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+ ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
p->setup();
return p->shared_from_this();
}
@@ -97,8 +97,8 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Problem structure and dimension
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
int dim = _server.getParam<int> ("problem/dimension");
- auto problem = Problem::create(frame_structure, dim);
- //
+ auto problem = Problem::create(frame_structure, dim, nullptr);
+
// cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
@@ -190,9 +190,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
_server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"),
_server.getParam<VectorComposite>("problem/prior/sigma"));
-
-
-
}
else
{
@@ -203,11 +200,16 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
// Map
- std::string map_type = _server.getParam<std::string>("problem/map/type");
+ std::string map_type = _server.getParam<std::string>("map/type");
WOLF_TRACE("Map Type: ", map_type);
auto map = AutoConfFactoryMap::create(map_type, _server);
+ WOLF_INFO("Map created");
+ WOLF_INFO_COND(problem->getMap() == nullptr, "Problem map is nullptr");
+ WOLF_INFO_COND(problem->getMap() != nullptr, "Problem map is OK");
map->setProblem(problem);
+ WOLF_INFO("Map problem set");
problem->setMap(map);
+ WOLF_INFO("Problem map set");
// Done
return problem;
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
index 6c5ed47c2efc2afc3ba960b075e60cea73b58a73..387924859ffebd1cec161a215dc2e572aeb585a6 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -13,6 +13,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index fa43fecb397dff295b683cfcd6a282adac61cc44..a27f60df2d6e64ecf2a8511d5923713c71daa2d9 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -18,6 +18,8 @@ config:
tree_manager:
type: "TreeManagerDummy"
toy_param: 0
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index f37e31459d9a883aca9eb12898aa5ac285e63210..10759a6ede49ee2990c0a9177987fc4ca5a8a3a6 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -17,6 +17,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 277810464d6f619ed342ce3706bec30d7ca8e5f9..d0c3d81dda9444acde8a83b851e31aca28b00d46 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -16,6 +16,8 @@ config:
n_frames: 3
n_fix_first_frames: 2
viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index f22fdde12f065d17accb122ef7f8d1728ef6fb6c..e7a7bc9b90c538a3d598e6610bce4a5c742086bc 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -16,6 +16,8 @@ config:
n_frames: 3
n_fix_first_frames: 0
viral_remove_empty_parent: false
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index a7f0f7434fb8a71c74e3aa3f15b8dc9f6ea7c067..299fa63e0698b3938033a3e52bf4847c80320d98 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -18,3 +18,5 @@ config:
rate_old_frames: 2
n_fix_first_frames: 2
viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
index cae3df67f036430481cd936ea31a9d2b4c0bca9a..928408fcb3ae6b1b2016a3fd53da0040f63044bd 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -18,3 +18,5 @@ config:
rate_old_frames: 2
n_fix_first_frames: 0
viral_remove_empty_parent: false
+ map:
+ type: "MapBase"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index 8f00f6499df2c96c9993bd6c486554579e7fbab9..9e64db064b95bbe2b55f1b2117ba33cfce364055 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -24,6 +24,8 @@ config:
rate_old_frames: 2
n_fix_first_frames: 2
viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index 114e865bdc3a86b6d0cddf42e0f5360b7b2d5928..16e421556cda457fa6b23564d5d4cdc4f024ef5d 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -18,6 +18,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"