diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9523adc1770ee01575bd360c47e722f6ab157c04..c94b33bfb04c9751c727f62bb16557cd1ef8f508 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
+MESSAGE(STATUS "Starting WOLF CMakeLists ...")
# Pre-requisites about cmake itself
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
@@ -63,7 +63,7 @@ ENDIF(NOT BUILD_DOC)
IF(PROFILING)
add_definitions(-DPROFILING)
- message("Compiling with profiling options...")
+ message(STATUS "Compiling with profiling options...")
ENDIF(PROFILING)
if(BUILD_TESTS)
@@ -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
@@ -188,6 +187,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/doc/main.dox b/doc/main.dox
index 62090c8648315200e0ba9b726d55048d74406425..efde8f91eab56973935fe8efa85d64e6dd558b48 100644
--- a/doc/main.dox
+++ b/doc/main.dox
@@ -1,258 +1,29 @@
// EDIT CAPS TEXTS
/*! \mainpage WOLF
-
- \section Description
- WOLF (Windowed Localization Frames) is a software library ...
-
- \section Main Main Features
+ WOLF (Windowed Localization Frames) is a C++ framework for state estimation based on factor graphs.
- - Uses Eigen3.2. as an algebra engine
- - Suited for Filtering and Optimization approaches
- - Allows different state spaces configurations
- - Minimizes read/write of state vectors and matrix to memory
-
- \section Contents
-
- - \link Installation Installation \endlink
- - \link how_to How To \endlink
- - \link Performance Performance \endlink
- - \link faq FAQ \endlink
- - \link todo To Do \endlink
- - \link other OTHER STUFF FTW \endlink
- .
-
-
- \section Authors
- Joan Solà (jsola (_at_) iri.upc.edu)
-
- Andreu Corominas (acorominas (_at_) iri.upc.edu)
-
- Faust Terrado (fterrado (_at_) iri.upc.edu)
+ This is the Doxygen documentation of the **WOLF core** library.
- Àngel Santamaria (asantamaria (_at_) iri.upc.edu)
-
- Joan Vallvé (jvallve (_at_) iri.upc.edu)
-
- \section License
-
- TO BE DISCUSSED :
- This package is licensed under a
- <a href="http://www.gnu.org/licenses/lgpl.html" target=â€_blankâ€>
- LGPL 3.0 License</a>.
-
- \section Disclaimer
-
- This 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/>.
-
- \section References References and External Documentation
+ If you are looking for one of the following:
- ... Nothing written here yet ...
-
-*/
-
-
-
-// HERE STARTS THE FUN (THE DIFFERENT PAGES)
-
-// TITLE
-// MINI PAGES MENU
-// CONTENT (SECTIONS...)
-
-// ADDING PAGES:
-// - add the new page in the menus of each page
-// - current page is bold "\b", not a \link!
-
-
-/*! \page Installation Installation
-\b Installation | \link how_to How To \endlink | \link Performance \endlink | \link faq FAQ \endlink | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-
-To install the WOLF library, please follow the steps below:
-
-\section Download
-
-Checkout the source code from the IRI public repository
-\code
-cd <your_developing_folder>
-svn checkout https://devel.iri.upc.edu/labrobotica/algorithms/wolf wolf
-\endcode
-
-\section Compilation
-
-After downloading, change directory to wolf/build and execute cmake,
-\code
-cd build
-cmake ..
-\endcode
-compile the shared library (wolf.so) and the example programs,
-\code
-make
-\endcode
-
-<!--
-The <em>cmake</em> only need to be executed once (make will automatically call
-<em>cmake</em> if you modify one of the <em>CMakeList.txt</em> files).
--->
-
-Optionally, if you want to generate this documentation in your local machine (uses doxygen default style type),
-\code
-make doc
-\endcode <!--OTHER-->
-<!--
-- To generate this documentation with IRI-DOC style type
-\code
-cd doc
-svn checkout https://devel.iri.upc.edu/labrobotica/tools/iri_doc
-cd ../build; rm -rf *; cmake ..; make; make doc
-\endcode
--->
-
-Please note that the files in the <em>build</em> directory are genetated by <em>cmake</em>
-and <em>make</em> and can be safely removed.
-After doing so you will need to call cmake manually again.
-
-\section Installing_subsec Installing
-
-In order to be able to use the library, it is necessary to copy it into the system.
-To do that, execute, from the build directory
-\code
-sudo make install
-\endcode
-
-as root and the shared libraries will be copied to <em>/usr/local/lib/iridrivers</em> directory
-and the header files will be copied to <em>/usr/local/include/iridrivers</em> dierctory. At
-this point, the library may be used by any user.
-
-To remove the library from the system, execute
-\code
-sudo make uninstall
-\endcode
-as root, and all the associated files will be removed from the system.
-OTHER
-
-\section Configuration
-
-The default build mode is DEBUG. That is, objects and executables
-include debug information.
-
-The RELEASE build mode optimizes for speed. To build in this mode
-execute
-\code
-cmake .. -DCMAKE_BUILD_TYPE=RELEASE
-\endcode
-The release mode will be kept until next time cmake is executed.
-
-*/
-
-
-
-/*! \page how_to How to use this library
-\link Installation \endlink | \b How \b To | \link Performance \endlink | \link faq FAQ \endlink | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-You can use only the functions provided by the library after \link Installation install this library \endlink.
-
-If you need to use this library in your code, you'll have to add these lines into your CMakeLists.txt file.
-
-- first it is necessary to locate if required libraries have been installed or not using the following command (TO DO)
-\code
-FIND_PACKAGE(wolf REQUIRED)
-\endcode
-- In the case that the packages are present, it is necessary to add the header files directory to
-the include directory path by using
-\code
-INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIR})
-\endcode
-- Finally, it is also necessary to link with the desired libraries by using the following command
-\code
-TARGET_LINK_LIBRARIES(<executable name> ${wolf_LIBRARY})
-\endcode
-.
-
-<!--
-All these steps are automatically done when the new project is created following the instructions
-in <A href="http://wikiri.upc.es/index.php/Create_a_new_development_project" target=â€_blankâ€>here</a>
--->
-
-From the programming point of view the following example illustrates how to use wolf:
-\code
-#include "state_pvq.h"
-int main()
-{
- VectorXd storage(20);
- unsigned int index = 0, bsize = 4;
-
- //set storage vector
- storage << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19;
-
- VectorXd vec_pvq(StatePVQ::SIZE_);
- vec_pvq << 20, 21, 22, 23, 24, 25, 26, 27, 28, 29;
-
- StatePVQ pvq(storage, index + bsize, vec_pvq);
- cout << "storage : " << storage.transpose() << endl;
- cout << "pvq state: " << pvq.x().transpose() << endl;
- cout << "pvq pos : " << pvq.p().transpose() << endl;
- cout << "pvq vel : " << pvq.v().transpose() << endl;
- cout << "pvq quat : " << pvq.q().coeffs().transpose() << endl;
-
- VectorXd vec_pvq2(StatePVQ::SIZE_);
- vec_pvq2 << 30, 31, 32, 33, 34, 35, 36, 37, 38, 39;
- pvq.x(vec_pvq2);
- cout << "storage : " << storage.transpose() << endl;
- cout << "pvq state: " << pvq.x().transpose() << endl;
-
-
- return 0;
-}
-\endcode
-
-For further code examples, please see the files at src/examples where there are some "main()" instances that make use of the library in different useful ways.
-
-*/
-
-/*! \page Performance Performance
-\link Installation \endlink | \link how_to How To \endlink | \b Performance | \link faq FAQ \endlink | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-The following plot shows the processing time spent to compute ...
-
- <!-- \image html "images/xxx.png" -->
-*/
-
-/*! \page faq Frequently Asked Questions
-\link Installation \endlink | \link how_to How To \endlink | \link Performance \endlink | \b FAQ | \link todo To Do \endlink | \link other OTHER STUFF FTW \endlink
-
-Do I need some extra libraries to compile WOLF ?
-- Yes, please take a look at the \link Installation \endlink page.
-
-*/
-
-
-// THIS IS SHOWN BEFORE THE TO DO AUTOMATIC PAGE
-
-/*! \page todo To Do
-\link Installation \endlink | \link how_to How To \endlink | \link Performance \endlink | \link faq FAQ \endlink| \b To \b Do | \link other OTHER STUFF FTW \endlink
-- some to do
-- another to do
-*/
-
-
-
-
-/*! \page other OTHER STUFF FTW
-\link Installation \endlink | \link how_to How To \endlink | \link Performance \endlink | \link faq FAQ \endlink | \link todo To Do \endlink | \b OTHER \b STUFF \b FTW
-
-HERE IS YOUR CONTENT FOR THE WIN
-
-
+ - full documentation
+ - installation instructions
+ - access to the repositories
+ - WOLF plugins
+ - ROS packages
+
+ please visit the main WOLF page at http://www.iri.upc.edu/wolf.
+
+ WOLF is copyright 2015--2022
+
+ - Joan SolÃ
+ - Joan Vallvé
+ - Joaquim Casals,
+ - Jérémie Deray,
+ - Médéric Fourmy,
+ - Dinesh Atchuthan,
+ - Andreu Corominas-Murtra
+
+ at IRI-CSIC-UPC.
*/
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/math/SE2.h b/include/core/math/SE2.h
index 4ce5ab465d6e87ec8e7cacf5af05c06f2a6db32c..95631a8a6fb82ac22595b64d3e1d5b4fe7849cb4 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -102,6 +102,38 @@ inline VectorComposite identity()
return v;
}
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
+ MatrixBase<D2>& ip, typename D2::Scalar& io)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ MatrixSizeCheck<2, 1>::check(ip);
+
+ ip = SO2::exp(-o) * -p;
+ io = -o;
+}
+
+template<typename D1, typename D2>
+inline void inverse(const MatrixBase<D1>& d,
+ MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<3, 1>::check(d);
+ MatrixSizeCheck<3, 1>::check(id);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & d( 0 ) );
+ Map<Matrix<typename D2::Scalar, 2, 1> > ip ( & id( 0 ) );
+
+ inverse(p, d(2), ip, id(2));
+}
+
+template<typename D>
+inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
+{
+ Matrix<typename D::Scalar, 3, 1> id;
+ inverse(d, id);
+ return id;
+}
+
template<class D1, class D2>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
@@ -408,7 +440,47 @@ inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau
return res;
}
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& p1, const typename D1::Scalar& o1,
+ const MatrixBase<D2>& p2, const typename D2::Scalar& o2,
+ MatrixBase<D3>& p12, typename D3::Scalar& o12)
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<2, 1>::check(p12);
+
+ p12 = SO2::exp(-o1) * ( p2 - p1 );
+ o12 = o2 - o1;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d12)
+{
+ MatrixSizeCheck<3, 1>::check(d1);
+ MatrixSizeCheck<3, 1>::check(d2);
+ MatrixSizeCheck<3, 1>::check(d12);
+
+ typedef typename D1::Scalar T;
+
+ Map<const Matrix<T, 2, 1> > p1 ( & d1(0) );
+ Map<const Matrix<T, 2, 1> > p2 ( & d2(0) );
+ Map<Matrix<T, 2, 1> > p12 ( & d12(0) );
+
+ between(p1, d1(2), p2, d2(2), p12, d12(2));
+}
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2 )
+{
+ MatrixSizeCheck<3, 1>::check(d1);
+ MatrixSizeCheck<3, 1>::check(d2);
+ Matrix<typename D1::Scalar, 3, 1> d12;
+ between(d1, d2, d12);
+ return d12;
+}
} // namespace SE2
} // namespacs wolf
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/CMakeLists.txt b/test/CMakeLists.txt
index a84e5c5f1c6704f57d33312f89901ef0875b1394..9479b901549a8777580ab7455507a4f9a83a9567 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -126,11 +126,16 @@ target_link_libraries(gtest_processor_motion ${PLUGIN_NAME})
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
+target_link_libraries(gtest_rotation ${PLUGIN_NAME})
# SE3 test
wolf_add_gtest(gtest_SE3 gtest_SE3.cpp)
target_link_libraries(gtest_SE3 ${PLUGIN_NAME})
+# SE2 test
+wolf_add_gtest(gtest_SE2 gtest_SE2.cpp)
+target_link_libraries(gtest_SE2 ${PLUGIN_NAME})
+
# SensorBase test
wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp)
target_link_libraries(gtest_sensor_base ${PLUGIN_NAME})
diff --git a/test/gtest_SE2.cpp b/test/gtest_SE2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..690a5f373e2259c8cc2268c898b04f28c7c8fdc4
--- /dev/null
+++ b/test/gtest_SE2.cpp
@@ -0,0 +1,259 @@
+//--------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--------
+
+
+#include "core/math/SE2.h"
+#include "core/utils/utils_gtest.h"
+
+using namespace Eigen;
+using namespace wolf;
+using namespace SE2;
+
+TEST(SE2, exp)
+{
+ for (auto i = 1; i<10; i++)
+ {
+ double o = Vector1d::Random()(0) * M_PI;
+ ASSERT_MATRIX_APPROX(SO2::exp(o), Rotation2Dd(o).matrix(), 1e-8);
+ }
+}
+
+TEST(SE2, invBlocks)
+{
+ Vector2d p, ip;
+ double o, io;
+
+ // zero
+ p = Vector2d::Zero();
+ o = 0;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(io, 0, 1e-8);
+
+ // zero angle
+ p = Vector2d::Random();
+ o = 0;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, -p, 1e-8);
+ ASSERT_NEAR(io, 0, 1e-8);
+
+ // zero translation
+ p = Vector2d::Zero();
+ o = Vector1d::Random()(0) * M_PI;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(io, -o, 1e-8);
+
+ // random
+ for (auto i = 0; i < 10; i++)
+ {
+ p = Vector2d::Random();
+ o = Vector1d::Random()(0) * M_PI;
+
+ SE2::inverse(p,o,ip,io);
+
+ ASSERT_MATRIX_APPROX(ip, Rotation2Dd(-o).matrix() * -p, 1e-8);
+ ASSERT_NEAR(io, -o, 1e-8);
+ }
+}
+
+TEST(SE2, invVector)
+{
+ Vector3d d, id;
+
+ // zero
+ d = Vector3d::Zero();
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id, Vector3d::Zero(), 1e-8);
+
+ // zero angle
+ d = Vector3d::Random();
+ d(2) = 0;
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id.head<2>(), -d.head<2>(), 1e-8);
+ ASSERT_NEAR(id(2), 0, 1e-8);
+
+ // zero translation
+ d = Vector3d::Zero();
+ d(2) = Vector1d::Random()(0) * M_PI;
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id.head<2>(), Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(id(2), -d(2), 1e-8);
+
+ // random
+ for (auto i = 0; i < 10; i++)
+ {
+ d = Vector3d::Random();
+ d(2) *= M_PI;
+
+ SE2::inverse(d,id);
+
+ ASSERT_MATRIX_APPROX(id.head<2>(), Rotation2Dd(-d(2)) * -d.head<2>(), 1e-8);
+ ASSERT_NEAR(id(2), -d(2), 1e-8);
+ }
+}
+
+TEST(SE2, betweenBlocks)
+{
+ Vector2d p1, p2, pd;
+ double o1, o2, od;
+
+ // both origin: relative pose zero
+ p1 = Vector2d::Zero();
+ p2 = p1;
+ o1 = 0;
+ o2 = o1;
+ between(p1, o1, p2, o2, pd, od);
+
+ ASSERT_MATRIX_APPROX(pd, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(od, 0, 1e-8);
+
+ // both same random pose: relative pose zero
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector2d::Random();
+ p2 = p1;
+ o1 = Vector1d::Random()(0) * M_PI;
+ o2 = o1;
+
+ between(p1, o1, p2, o2, pd, od);
+
+ ASSERT_MATRIX_APPROX(pd, Vector2d::Zero(), 1e-8);
+ ASSERT_NEAR(od, 0, 1e-8);
+ }
+
+ // random relative pose
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector2d::Random();
+ o1 = Vector1d::Random()(0) * M_PI;
+ Vector2d pd_gt = Vector2d::Random();
+ double od_gt = Vector1d::Random()(0) * M_PI;
+
+ p2 = p1 + Rotation2Dd(o1) * pd_gt;
+ o2 = o1 + od_gt;
+
+ between(p1, o1, p2, o2, pd, od);
+
+ ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+ ASSERT_NEAR(od, od_gt, 1e-8);
+ }
+}
+
+TEST(SE2, betweenVectors)
+{
+ Vector3d p1, p2, pd;
+
+ // both origin: relative pose zero
+ p1 = Vector3d::Zero();
+ p2 = p1;
+ between(p1, p2, pd);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+
+ // both same random pose: relative pose zero
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ p2 = p1;
+
+ between(p1, p2, pd);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+ }
+
+ // random relative pose
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ Vector3d pd_gt = Vector3d::Random();
+ pd_gt(2) *= M_PI;
+
+ p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
+ p2(2) = p1(2) + pd_gt(2);
+
+ between(p1, p2, pd);
+
+ ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+ }
+}
+
+TEST(SE2, betweenVectorsReturn)
+{
+ Vector3d p1, p2, pd;
+
+ // both origin: relative pose zero
+ p1 = Vector3d::Zero();
+ p2 = p1;
+ pd = between(p1, p2);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+
+ // both same random pose: relative pose zero
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ p2 = p1;
+
+ pd = between(p1, p2);
+
+ ASSERT_MATRIX_APPROX(pd, Vector3d::Zero(), 1e-8);
+ }
+
+ // random relative pose
+ for (auto i = 0; i < 10; i++)
+ {
+ p1 = Vector3d::Random();
+ p1(2) *= M_PI;
+ Vector3d pd_gt = Vector3d::Random();
+ pd_gt(2) *= M_PI;
+
+ p2.head<2>() = p1.head<2>() + Rotation2Dd(p1(2)) * pd_gt.head<2>();
+ p2(2) = p1(2) + pd_gt(2);
+
+ pd = between(p1, p2);
+
+ ASSERT_MATRIX_APPROX(pd, pd_gt, 1e-8);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
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