From 0cdf0e5280d7c814f98fd1f2f376ff570b6c4b7c Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 27 Jul 2021 23:00:23 +0200
Subject: [PATCH] wip
---
include/core/common/wolf.h | 3 +
include/core/map/factory_map.h | 200 +++++++++++++++++++++++++++++++++
include/core/map/map_base.h | 51 ++++++++-
src/map/map_base.cpp | 16 ++-
4 files changed, 265 insertions(+), 5 deletions(-)
create mode 100644 include/core/map/factory_map.h
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 97f9a50ff..e055a1bdc 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -254,6 +254,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 000000000..9034f2772
--- /dev/null
+++ b/include/core/map/factory_map.h
@@ -0,0 +1,200 @@
+/**
+ * \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 std::string&,
+ const Eigen::VectorXd&,
+ 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 std::string&,
+ 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 93663bdf7..622efcdf1 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -11,11 +11,58 @@ 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>("map", _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() override = default;
+ using ParamsBase::ParamsBase;
+ std::string print() const override
+ {
+ return "";
+ }
+};
+
//class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
@@ -25,7 +72,9 @@ 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);
+
~MapBase() override;
protected:
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 7f6db136f..5d5433fc4 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -1,8 +1,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>
@@ -16,8 +15,8 @@
namespace wolf {
-MapBase::MapBase() :
- NodeBase("MAP", "Base")
+MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
+ NodeBase("MAP", _type)
{
// std::cout << "constructed M"<< std::endl;
}
@@ -127,3 +126,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
+
--
GitLab