diff --git a/CMakeLists.txt b/CMakeLists.txt
index e73cf293f1341de7d55d71f1ed232b2958015aa8..55c694583426d7ae5178cfa8328836779a1f3d6e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -127,7 +127,6 @@ INCLUDE_DIRECTORIES("include") # In this same project
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-
# ============ HEADERS ============
SET(HDRS_CAPTURE
include/core/capture/capture_base.h
@@ -187,6 +186,7 @@ SET(HDRS_MATH
include/core/math/covariance.h
)
SET(HDRS_MAP
+ include/core/map/factory_map.h
include/core/map/map_base.h
)
SET(HDRS_PROBLEM
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index 55eb56a880cd3d407cf631ce7e052e135e9bacce..4260a8c8efb26928a6d2242edbd43830b4467875 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -17,6 +17,7 @@ config:
tree_manager:
type: "none"
+ plugin: "core"
solver:
max_num_iterations: 100
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 7a7f9eeae3b50cbe7d96cd841d99ae1f07b566d3..197a6bfb1b30d79a64376f9468b9481c0a4bc3e3 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -275,6 +275,9 @@ WOLF_LIST_TYPEDEFS(FactorBase);
// Map
WOLF_PTR_TYPEDEFS(MapBase);
+// - - Map params
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsMapBase);
+
// - Landmark
WOLF_PTR_TYPEDEFS(LandmarkBase);
WOLF_LIST_TYPEDEFS(LandmarkBase);
diff --git a/include/core/map/factory_map.h b/include/core/map/factory_map.h
new file mode 100644
index 0000000000000000000000000000000000000000..5772e4e8a1abab280f5e6ee096161e99be39d515
--- /dev/null
+++ b/include/core/map/factory_map.h
@@ -0,0 +1,218 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file factory_map.h
+ *
+ * Created on: Jul 25, 2016
+ * \author: jvallve
+ */
+
+#ifndef FACTORY_MAP_H_
+#define FACTORY_MAP_H_
+
+namespace wolf
+{
+class MapBase;
+struct ParamsMapBase;
+}
+
+// wolf
+#include "core/common/factory.h"
+#include "core/utils/params_server.h"
+
+namespace wolf
+{
+
+/** \brief Map factory class
+ *
+ * This factory can create objects of classes deriving from MapBase.
+ *
+ * Specific object creation is invoked by `create(TYPE, params ... )`, and the TYPE of map is identified with a string.
+ * Currently, the following map types are implemented,
+ * - "MapGrid2dGravity" in plugin 'imu'
+ *
+ * among others.
+ *
+ * Find general Factory documentation in class Factory:
+ * - Access the factory
+ * - Register/unregister creators
+ * - Invoke object creation
+ *
+ * This documentation shows you how to use the FactoryMap specifically:
+ * - Write map creators.
+ * - Create maps
+ *
+ * #### Write map creators
+ * Map creators have the following API:
+ *
+ * \code
+ * static MapBasePtr create(ParamsMapBasePtr _params_map);
+ * \endcode
+ *
+ * They follow the general implementation shown below:
+ *
+ * \code
+ * static MapBasePtr create(ParamsMapBasePtr _params_map)
+ * {
+ * // cast map parameters to good type --- example: ParamsMapGrid
+ * auto params_ptr = std::static_pointer_cast<ParamsMapGrid>(_params_map);
+ *
+ * // Do create the Map object --- example: MapGrid
+ * return map_ptr = std::make_shared<MapGrid>(params_ptr);
+ * }
+ * \endcode
+ *
+ * #### Creating maps
+ * Note: Prior to invoking the creation of a map of a particular type,
+ * you must register the creator for this type into the factory.
+ *
+ * To create e.g. a MapGrid, you type:
+ *
+ * \code
+ * auto grid_ptr = FactoryMap::create("MapGrid", params_grid);
+ * \endcode
+ *
+ * #### See also
+ * - FactoryParamsMap: to create parameter structs deriving from ParamsMapBase directly from YAML files.
+ *
+ * #### Example 1: writing a MapGrid creator
+ * Here is an example of MapGrid::create() extracted from map_grid.cpp:
+ *
+ * \code
+ * static MapBasePtr create(const std::string& _unique_name, Eigen::VectorXd& _extrinsics_pq, ParamsMapBasePtr _intrinsics)
+ * {
+ * // check extrinsics vector
+ * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
+ *
+ * // cast instrinsics to good type
+ * auto intrinsics_ptr = std::static_pointer_cast<ParamsMapGrid>(_intrinsics);
+ *
+ * // Do create the MapGrid object, and complete its setup
+ * auto sen_ptr = std::make_shared<MapGrid>(_extrinsics_pq, intrinsics_ptr);
+ *
+ * sen_ptr->setName(_unique_name);
+ *
+ * return sen_ptr;
+ * }
+ * \endcode
+ *
+ * #### Example 2: registering a map creator into the factory
+ * Registration can be done manually or automatically. It involves the call to static functions.
+ * It is advisable to put these calls within unnamed namespaces.
+ *
+ * - __Manual registration__: you control registration at application level.
+ * Put the code either at global scope (you must define a dummy variable for this),
+ * \code
+ * namespace {
+ * const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
+ * }
+ * main () { ... }
+ * \endcode
+ * or inside your main(), where a direct call is possible:
+ * \code
+ * main () {
+ * FactoryMap::registerCreator("MapGrid", MapGrid::create);
+ * ...
+ * }
+ * \endcode
+ *
+ * - __Automatic registration__: registration is performed at library level.
+ * Put the code at the last line of the map_xxx.cpp file,
+ * \code
+ * namespace {
+ * const bool registered_grid = FactoryMap::registerCreator("MapGrid", MapGrid::create);
+ * }
+ * \endcode
+ * Automatic registration is recommended in wolf, and implemented in the classes shipped with it.
+ * You are free to comment out these lines and place them wherever you consider it more convenient for your design.
+ *
+ * #### Example 2: creating maps
+ * We finally provide the necessary steps to create a map of class MapGrid in our application:
+ *
+ * \code
+ * #include "core/map/factory_map.h"
+ * #include "core/map/map_grid.h" // provides MapGrid
+ *
+ * // Note: MapGrid::create() is already registered, automatically.
+ *
+ * using namespace wolf;
+ * int main() {
+ *
+ * // To create a grid, provide:
+ * // a type = "MapGrid" and
+ * // a pointer to the intrinsics struct:
+ *
+ * // Create a pointer to the struct of map parameters stored in a YAML file ( see FactoryParamsMap )
+ * ParamsMapGridPtr params_1 =
+ * FactoryParamsMap::create("ParamsMapGrid",
+ * grid_1.yaml);
+ *
+ * MapBasePtr grid_1_ptr =
+ * FactoryMap::create ( "MapGrid" ,
+ * params_1 );
+ *
+ * return 0;
+ * }
+ * \endcode
+ *
+ */
+typedef Factory<MapBase,
+ const ParamsMapBasePtr> FactoryMap;
+
+template<>
+inline std::string FactoryMap::getClass() const
+{
+ return "FactoryMap";
+}
+
+// ParamsMap factory
+struct ParamsMapBase;
+typedef Factory<ParamsMapBase,
+ const std::string&> FactoryParamsMap;
+template<>
+inline std::string FactoryParamsMap::getClass() const
+{
+ return "FactoryParamsMap";
+}
+
+#define WOLF_REGISTER_MAP(MapType) \
+ namespace{ const bool WOLF_UNUSED MapType##Registered = \
+ FactoryMap::registerCreator(#MapType, MapType::create); } \
+
+
+typedef Factory<MapBase,
+ const ParamsServer&> AutoConfFactoryMap;
+
+template<>
+inline std::string AutoConfFactoryMap::getClass() const
+{
+ return "AutoConfFactoryMap";
+}
+
+#define WOLF_REGISTER_MAP_AUTO(MapType) \
+ namespace{ const bool WOLF_UNUSED MapType##AutoConfRegistered = \
+ AutoConfFactoryMap::registerCreator(#MapType, MapType::create); } \
+
+
+} /* namespace wolf */
+
+#endif /* SENSOR_FACTORY_H_ */
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 13c870269665e595145b28d8ceca2a44aa0dac92..797d2bd3892e339c96a8c40e591eb56d39818ee5 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -32,11 +32,65 @@ class LandmarkBase;
//Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
+#include "core/common/params_base.h"
//std includes
namespace wolf {
+/*
+ * Macro for defining Autoconf map creator.
+ *
+ * Place a call to this macro inside your class declaration (in the map_class.h file),
+ * preferably just after the constructors.
+ *
+ * In order to use this macro, the derived map class, MapClass,
+ * must have a constructor available with the API:
+ *
+ * MapClass(const ParamsMapClassPtr _params);
+ *
+ * We recommend writing one of such constructors in your derived maps.
+ */
+#define WOLF_MAP_CREATE(MapClass, ParamsMapClass) \
+static \
+MapBasePtr create(const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsMapClass>(_server); \
+ \
+ return std::make_shared<MapClass>(params); \
+} \
+ \
+static \
+MapBasePtr create(const ParamsMapBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsMapClass>(_params); \
+ \
+ return std::make_shared<MapClass>(params); \
+} \
+
+
+/** \brief base struct for map parameters
+ *
+ * Derive from this struct to create structs of map parameters.
+ */
+struct ParamsMapBase: public ParamsBase
+{
+ std::string prefix = "map";
+
+ ParamsMapBase(const ParamsServer& _param_server) :
+ ParamsBase("map", _param_server)
+ {
+
+ };
+
+ ~ParamsMapBase() override = default;
+
+ std::string print() const override
+ {
+ return "";
+ }
+};
+
//class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
@@ -47,6 +101,9 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
MapBase();
+ MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
+ WOLF_MAP_CREATE(MapBase, ParamsMapBase);
+
~MapBase() override;
protected:
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 14f8588b7d01715a6ffb3dcd2e8be8d119aff78d..7405ae8320deb339ed6efdfe7319b4df8ec14cff 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -89,11 +89,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();
@@ -343,6 +343,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");
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index f736ce180aa565b43ec5a68d1ec061c551f33916..8194c87d95dc05bb20b5f736afdb704a11a75560 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -23,7 +23,7 @@
// wolf
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/common/factory.h"
+//#include "core/common/factory.h"
// YAML
#include <yaml-cpp/yaml.h>
@@ -43,6 +43,12 @@ MapBase::MapBase() :
// std::cout << "constructed M"<< std::endl;
}
+MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
+ NodeBase("MAP", _type)
+{
+// std::cout << "constructed M"<< std::endl;
+}
+
MapBase::~MapBase()
{
// std::cout << "destructed -M" << std::endl;
@@ -148,3 +154,12 @@ bool MapBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool _v
return _log.is_consistent_;
}
} // namespace wolf
+
+
+// Register in the FactorySensor
+#include "core/map/factory_map.h"
+namespace wolf {
+WOLF_REGISTER_MAP(MapBase);
+WOLF_REGISTER_MAP_AUTO(MapBase);
+} // namespace wolf
+
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f2fcb9096396bfd5d3913e861bd91c7a4bc1c6c2..a3981d2f9e1a8e2b08a25f290ee2e7cf60ffd6be 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -24,7 +24,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"
@@ -32,8 +35,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"
@@ -61,11 +62,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_(std::make_shared<MapBase>()),
+ map_ptr_(_map),
motion_provider_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
@@ -96,12 +97,13 @@ void Problem::setup()
{
hardware_ptr_ -> setProblem(shared_from_this());
trajectory_ptr_-> setProblem(shared_from_this());
- 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();
}
@@ -117,17 +119,19 @@ 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);
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
auto loaders = std::vector<std::shared_ptr<Loader>>();
std::string plugins_path;
- try{
+ try
+ {
plugins_path = _server.getParam<std::string>("plugins_path");
}
- catch(MissingValueException& e){
+ catch (MissingValueException& e)
+ {
WOLF_WARN(e.what());
WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
plugins_path="/usr/local/lib/";
@@ -145,9 +149,11 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// load Packages for subscribers and publishers
std::string packages_path;
- try {
+ try
+ {
packages_path = _server.getParam<std::string>("packages_path");
- } catch (MissingValueException& e) {
+ }
+ catch (MissingValueException& e) {
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
@@ -168,9 +174,12 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// load raw libs
std::vector<std::string> raw_libs;
- try {
+ try
+ {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
- } catch (MissingValueException& e) {
+ }
+ catch (MissingValueException& e)
+ {
WOLF_TRACE("No raw libraries to load...");
raw_libs = std::vector<std::string>();
}
@@ -196,11 +205,48 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
_server);
+ // Map (optional)
+ std::string map_type, map_plugin;
+ try
+ {
+ map_type = _server.getParam<std::string>("map/type");
+ map_plugin = _server.getParam<std::string>("map/plugin");
+ }
+ catch (MissingValueException& e)
+ {
+ WOLF_TRACE("No map/type and/or map/plugin specified. Emplacing the default map: MapBase.");
+ map_type = "MapBase";
+ map_plugin = "core";
+ }
+ WOLF_TRACE("Map Type: ", map_type, " in plugin ", map_plugin);
+ if (map_plugin != "core" and map_plugin != "wolf")
+ {
+ std::string plugin = plugins_path + "libwolf" + map_plugin + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
+ auto map = AutoConfFactoryMap::create(map_type, _server);
+ map->setProblem(problem);
+ problem->setMap(map);
+
// Tree manager
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
if (tree_manager_type != "None" and tree_manager_type != "none")
+ {
+ std::string tm_plugin = _server.getParam<std::string>("problem/tree_manager/plugin");
+ if (tm_plugin != "core" and tm_plugin != "wolf")
+ {
+ std::string plugin = plugins_path + "libwolf" + tm_plugin + lib_extension;
+ WOLF_TRACE("Loading plugin " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
+ l->load();
+ loaders.push_back(l);
+ }
problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
+ }
// Set problem prior -- first keyframe
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
@@ -215,9 +261,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
problem->setPriorOptions(prior_mode,
_server.getParam<VectorComposite>("problem/prior/state"),
_server.getParam<VectorComposite>("problem/prior/sigma"));
-
-
-
}
else
{
@@ -990,6 +1033,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_;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d5fa0054e26e64664dba42b3d0fc66a717bd8709..d86942de20f525f0b0995e2a5b43e18750311697 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -512,11 +512,35 @@ TEST(Problem, check)
ASSERT_TRUE(problem->check(true, std::cout));
}
+TEST(Problem, autoSetupMap)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->check(true, std::cout));
+}
+
+TEST(Problem, autoSetupNoMap)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup_no_map.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->check(true, std::cout));
+}
+
TEST(Problem, getState)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- auto parser = ParserYaml("test/yaml/params_problem_odom_3d.yaml", wolf_root);
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
auto server = ParamsServer(parser.getParams());
auto P = Problem::autoSetup(server);
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c61eab58146a9a5917c1130a14e234afa08a01b6
--- /dev/null
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -0,0 +1,49 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6c5ed47c2efc2afc3ba960b075e60cea73b58a73
--- /dev/null
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -0,0 +1,46 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index fa43fecb397dff295b683cfcd6a282adac61cc44..59aec3c70cee0e147e38dbfb9408c8879bc203e2 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -17,6 +17,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerDummy"
+ plugin: "core"
toy_param: 0
sensors:
-
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index f37e31459d9a883aca9eb12898aa5ac285e63210..419125468ba5155eba8f0f75c972a5d52b5dbcef 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -15,7 +15,7 @@ config:
O: [0.31, 0.31, 0.31]
V: [0.31, 0.31, 0.31]
time_tolerance: 0.1
- tree_manager:
+ tree_manager:
type: "None"
sensors:
-
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 277810464d6f619ed342ce3706bec30d7ca8e5f9..61498d1b6c182a6ae95930047940c56bcc3ca4ae 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
+ plugin: "core"
n_frames: 3
n_fix_first_frames: 2
viral_remove_empty_parent: true
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index f22fdde12f065d17accb122ef7f8d1728ef6fb6c..db0a176fce7534934e9a10da0c5f84a5ae431517 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindow"
+ plugin: "core"
n_frames: 3
n_fix_first_frames: 0
viral_remove_empty_parent: false
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..2b6313f5b9702ab0f7c3dc0187dbde23ae944d00 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
+ plugin: "core"
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
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..609fb96f585545376756279956377ee6730dbc0d 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -13,6 +13,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
+ plugin: "core"
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2
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 0b030b80faa8a5f63dfa4c60aa19bcfd3c81a9b0..aed7a0c7e4da1313e261501c87e3748fb64cd2b5 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -15,6 +15,7 @@ config:
time_tolerance: 0.1
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
+ plugin: "core"
n_frames: 5
n_frames_recent: 3
rate_old_frames: 2