From e6b54e2279c55e5f7eabe6f3290ea72b2626ac11 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 25 Jul 2022 18:04:49 +0200
Subject: [PATCH] [skip ci] all gtests working
---
CMakeLists.txt | 16 -
include/core/frame/frame_base.h | 2 +-
include/core/landmark/landmark_base.h | 2 +-
include/core/problem/problem.h | 6 +-
include/core/processor/processor_base.h | 2 +-
include/core/sensor/factory_sensor.h | 49 +-
include/core/sensor/sensor_base.h | 2 +-
include/core/solver/factory_solver.h | 19 +-
include/core/solver/solver_manager.h | 27 +-
include/core/state_block/has_state_blocks.h | 39 +-
include/core/tree_manager/tree_manager_base.h | 2 +-
include/core/utils/converter.h | 412 ------------
include/core/utils/converter_utils.h | 64 --
include/core/utils/params_server.h | 76 ---
include/core/yaml/parser_yaml.h | 100 ---
include/core/yaml/yaml_conversion.h | 225 -------
schema/sensor/SensorPose2d.schema | 20 +
schema/sensor/SensorPose3d.schema | 20 +
schema/solver/SolverCeres.schema | 4 +-
.../TreeManagerSlidingWindow.schema | 25 +
.../TreeManagerSlidingWindowDualRate.schema | 13 +
scilab/corner_detector.sce | 237 -------
scilab/scan.txt | 1 -
scilab/test_ceres_odom_plot.sce | 80 ---
src/frame/frame_base.cpp | 48 +-
src/landmark/landmark_base.cpp | 4 +-
src/problem/problem.cpp | 37 +-
src/processor/processor_motion.cpp | 6 +-
src/state_block/has_state_blocks.cpp | 2 +-
src/utils/converter_utils.cpp | 153 -----
src/utils/params_server.cpp | 61 --
src/yaml/parser_yaml.cpp | 594 ------------------
test/CMakeLists.txt | 73 +--
test/gtest_odom_2d.cpp | 74 +--
test/gtest_param_server.cpp | 95 ---
test/gtest_parser_yaml.cpp | 84 ---
test/gtest_problem.cpp | 3 +-
...sor_and_factor_pose_3d_with_extrinsics.cpp | 54 +-
test/gtest_processor_diff_drive.cpp | 12 +-
test/gtest_processor_loop_closure.cpp | 4 +-
.../gtest_processor_tracker_feature_dummy.cpp | 2 +-
...gtest_processor_tracker_landmark_dummy.cpp | 2 +-
test/gtest_sensor_base.cpp | 4 +-
test/gtest_sensor_diff_drive.cpp | 73 +--
test/gtest_sensor_odom.cpp | 96 +--
test/gtest_sensor_pose.cpp | 137 +---
test/gtest_tree_manager.cpp | 2 +-
test/gtest_tree_manager_sliding_window.cpp | 86 +--
..._tree_manager_sliding_window_dual_rate.cpp | 111 +---
test/gtest_yaml_conversions.cpp | 33 +-
test/yaml/params1.yaml | 40 --
test/yaml/params2.yaml | 37 --
test/yaml/params3.1.yaml | 8 -
test/yaml/params3.yaml | 21 -
test/yaml/params_basic.yaml | 34 -
.../params_tree_manager_sliding_window1.yaml | 18 +-
.../params_tree_manager_sliding_window2.yaml | 18 +-
...ree_manager_sliding_window_dual_rate1.yaml | 56 +-
...ree_manager_sliding_window_dual_rate2.yaml | 54 +-
...ree_manager_sliding_window_dual_rate3.yaml | 18 +-
...ger_sliding_window_dual_rate_baseline.yaml | 18 +-
test/yaml/sensor_diff_drive.yaml | 1 +
62 files changed, 614 insertions(+), 3002 deletions(-)
delete mode 100644 include/core/utils/converter.h
delete mode 100644 include/core/utils/converter_utils.h
delete mode 100644 include/core/utils/params_server.h
delete mode 100644 include/core/yaml/parser_yaml.h
delete mode 100644 include/core/yaml/yaml_conversion.h
create mode 100644 schema/sensor/SensorPose2d.schema
create mode 100644 schema/sensor/SensorPose3d.schema
create mode 100644 schema/tree_manager/TreeManagerSlidingWindow.schema
create mode 100644 schema/tree_manager/TreeManagerSlidingWindowDualRate.schema
delete mode 100644 scilab/corner_detector.sce
delete mode 100644 scilab/scan.txt
delete mode 100644 scilab/test_ceres_odom_plot.sce
delete mode 100644 src/utils/converter_utils.cpp
delete mode 100644 src/utils/params_server.cpp
delete mode 100644 src/yaml/parser_yaml.cpp
delete mode 100644 test/gtest_param_server.cpp
delete mode 100644 test/gtest_parser_yaml.cpp
delete mode 100644 test/yaml/params1.yaml
delete mode 100644 test/yaml/params2.yaml
delete mode 100644 test/yaml/params3.1.yaml
delete mode 100644 test/yaml/params3.yaml
delete mode 100644 test/yaml/params_basic.yaml
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6fb5b5b10..ac07260c6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -232,19 +232,12 @@ SET(HDRS_TREE_MANAGER
)
SET(HDRS_UTILS
include/${PROJECT_NAME}/utils/check_log.h
- include/${PROJECT_NAME}/utils/converter.h
include/${PROJECT_NAME}/utils/eigen_assert.h
include/${PROJECT_NAME}/utils/graph_search.h
include/${PROJECT_NAME}/utils/loader.h
include/${PROJECT_NAME}/utils/logging.h
- # include/${PROJECT_NAME}/utils/params_server.h
include/${PROJECT_NAME}/utils/singleton.h
include/${PROJECT_NAME}/utils/utils_gtest.h
- include/${PROJECT_NAME}/utils/converter_utils.h
- )
-SET(HDRS_YAML
- include/${PROJECT_NAME}/yaml/parser_yaml.h
- # include/${PROJECT_NAME}/yaml/yaml_conversion.h
)
# ============ SOURCES ============
@@ -285,7 +278,6 @@ SET(SRCS_MAP
src/map/map_base.cpp
)
SET(SRCS_PROBLEM
- # src/problem/prior_problem.cpp
src/problem/problem.cpp
)
SET(SRCS_PROCESSOR
@@ -333,13 +325,8 @@ SET(SRCS_TREE_MANAGER
)
SET(SRCS_UTILS
src/utils/check_log.cpp
- src/utils/converter_utils.cpp
src/utils/graph_search.cpp
src/utils/loader.cpp
- # src/utils/params_server.cpp
- )
-SET(SRCS_YAML
- src/yaml/parser_yaml.cpp
)
# ============ OPTIONALS ============
@@ -385,7 +372,6 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_TRAJECTORY}
${SRCS_TREE_MANAGER}
${SRCS_UTILS}
- ${SRCS_YAML}
)
# Set compiler options
@@ -496,8 +482,6 @@ INSTALL(FILES ${HDRS_TREE_MANAGER}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
INSTALL(FILES ${HDRS_UTILS}
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/utils)
-INSTALL(FILES ${HDRS_YAML}
- DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/yaml)
INSTALL(FILES ${WOLF_CONFIG_DIR}/config.h
DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 4dd04a0d0..5c138e018 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -143,7 +143,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// States
public:
- bool getCovariance(Eigen::MatrixXd& _cov) const;
+ bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
// Wolf tree access ---------------------------------------------------
public:
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index fc62fc806..57c8f0da0 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -108,7 +108,7 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
void setId(unsigned int _id);
// State blocks
- bool getCovariance(Eigen::MatrixXd& _cov) const;
+ bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
public:
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index ed3810865..33cc7d029 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -335,9 +335,9 @@ class Problem : public std::enable_shared_from_this<Problem>
bool getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
- bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
- bool getLastFrameCovariance(Eigen::MatrixXd& _covariance) const;
- bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
+ bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
+ bool getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
+ bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index adc03e088..0d2f18f2d 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -73,7 +73,7 @@ static ProcessorBasePtr create(const std::string& _yaml_filepath,
\
if (not server.applySchema(#ProcessorClass)) \
{ \
- WOLF_ERROR(server.getLog().str()); \
+ WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
auto params = std::make_shared<ParamsProcessorClass>(server.getNode()); \
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 82aff821a..9980b4c6c 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -67,41 +67,55 @@ namespace wolf
* Sensor creators have the following API:
*
* \code
- * static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server);
- * static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const std::string& _yaml_filepath);
+ * static SensorBasePtr create(const YAML::Node& _server);
+ * static SensorBasePtr create(const std::string& _schema,
+ * const std::string& _yaml_filepath,
+ * std::vector<std::string>& _folders_schema);
* \endcode
*
* They follow the general implementations shown below:
*
* \code
- * static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server)
+ * static SensorBasePtr create(const YAML::Node& _server)
* {
* // Do create the Sensor Parameters --- example: ParamsSensorCamera
- * auto params = std::make_shared<ParamsSensorCamera>(_unique_name, _server);
+ * auto params = std::make_shared<ParamsSensorCamera>(_server);
+ *
+ * // Do create the Sensor States priors
+ * auto priors = SpecSensorComposite(_server["states"]);
*
* // Do create the Sensor object --- example: SensorCamera
- * auto sensor = std::make_shared<SensorCamera>(_unique_name, _dim, params, _server);
+ * auto sensor = std::make_shared<SensorCamera>(params, priors);
*
* return sensor;
* }
- * static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const std::string& _yaml_filepath)
+ * static SensorBasePtr create(const std::string& _schema,
+ * const std::string& _yaml_filepath,
+ * std::vector<std::string>& _folders_schema)
* {
* // parse the yaml file
- * auto parser = ParserYaml(_yaml_filepath, "", true);
+ * auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath);
+ *
*
- * // load the parameters
- * auto server = ParamsServer(parser.getParams());
+ * // Check that the yaml has all necessary inforamtion
+ * if (not server.applySchema(_schema))
+ * {
+ * throw std::runtime_error(server.getLog());
+ * }
*
* // Do create the Sensor Parameters --- example: ParamsSensorCamera
- * auto params = std::make_shared<ParamsSensorCamera>(_unique_name, server);
+ * auto params = std::make_shared<ParamsSensorCamera>(server.getNode());
*
* // Do create the Sensor object --- example: SensorCamera
- * auto sensor = std::make_shared<SensorCamera>(_unique_name, _dim, params, server);
+ * auto sensor = std::make_shared<SensorCamera>(params, priors);
*
* return sensor;
* }
* \endcode
*
+ *
+ *
+ *
* #### Creating sensors
* Note: Prior to invoking the creation of a sensor of a particular type,
* you must register the creator for this type into the factory.
@@ -109,17 +123,16 @@ namespace wolf
* To create e.g. a SensorCamera in 3D, you type:
*
* \code
- * auto camera_ptr = FactorySensor::create("SensorCamera", "Front-left camera", 3, param_server);
+ * auto camera_ptr = FactorySensor::create("SensorCamera", param_server);
* \endcode
*
* or:
*
* \code
- * auto camera_ptr = FactorySensorYaml::create("SensorCamera", "Front-left camera", 3, yaml_filepath);
+ * auto camera_ptr = FactorySensorYaml::create("SensorCamera", yaml_filepath);
* \endcode
*
- * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera
- * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS!
+ * where ABSOLUTELY ALL input parameters are important. DO NOT USE IT WITH DUMMY PARAMETERS!
*
* We RECOMMEND using the macro WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) to automatically add the sensor creator, provided in 'sensor_base.h'.
*
@@ -188,16 +201,12 @@ namespace wolf
*
* SensorBasePtr camera_1_ptr =
* FactorySensor::create ( "SensorCamera",
- * "Front-left camera" ,
- * 3 ,
* parameter_server );
*
- * // A second camera... with a different name!
+ * // A second camera... with a different name specified in the yaml file or the parameters!
*
* SensorBasePtr camera_2_ptr =
* FactorySensor::create( "SensorCamera" ,
- * "Front-right camera" ,
- * 3 ,
* yaml_filename );
*
* return 0;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 00d8da23e..9cc033cb0 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -76,7 +76,7 @@ static SensorBasePtr create(const std::string& _schema,
\
if (not server.applySchema(_schema)) \
{ \
- throw std::runtime_error(server.getLog().str()); \
+ throw std::runtime_error(server.getLog()); \
} \
auto params = std::make_shared<ParamsSensorClass>(server.getNode()); \
\
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index 860ede1ff..96a8192f8 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.h
@@ -50,8 +50,21 @@ inline std::string FactorySolver::getClass() const
return "FactorySolver";
}
-#define WOLF_REGISTER_SOLVER(SolverType) \
- namespace{ const bool WOLF_UNUSED SolverType##Registered = \
- wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
+typedef Factory<std::shared_ptr<SolverManager>,
+ const ProblemPtr&,
+ const std::string&,
+ const std::vector<std::string>&> FactorySolverYaml;
+
+template<>
+inline std::string FactorySolverYaml::getClass() const
+{
+ return "FactorySolverYaml";
+}
+
+#define WOLF_REGISTER_SOLVER(SolverType) \
+ namespace{ const bool WOLF_UNUSED SolverType##Registered = \
+ wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
+ namespace{ const bool WOLF_UNUSED SolverType##RegisteredYaml = \
+ wolf::FactorySolverYaml::registerCreator(#SolverType, SolverType::create); } \
} /* namespace wolf */
\ No newline at end of file
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 1e621b8f6..67dbaf402 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -44,13 +44,26 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
* SolverManagerClass(const ProblemPtr& wolf_problem,
* const ParamsSolverClassPtr _params);
*/
-#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \
-static SolverManagerPtr create(const ProblemPtr& _problem, \
- const YAML::Node& _input_node) \
-{ \
- auto params = std::make_shared<ParamsSolverClass>(_input_node); \
- return std::make_shared<SolverClass>(_problem, params); \
-} \
+#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \
+static SolverManagerPtr create(const ProblemPtr& _problem, \
+ const YAML::Node& _input_node) \
+{ \
+ auto params = std::make_shared<ParamsSolverClass>(_input_node); \
+ return std::make_shared<SolverClass>(_problem, params); \
+} \
+static SolverManagerPtr create(const ProblemPtr& _problem, \
+ const std::string& _yaml_filepath, \
+ const std::vector<std::string>& _folders_schema) \
+{ \
+ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
+ if (not server.applySchema(#SolverClass)) \
+ { \
+ WOLF_ERROR(server.getLog()); \
+ return nullptr; \
+ } \
+ auto params = std::make_shared<ParamsSolverClass>(server.getNode()); \
+ return std::make_shared<SolverClass>(_problem, params); \
+} \
struct ParamsSolver;
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 71308a397..8ea5e94a6 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -45,7 +45,8 @@ class HasStateBlocks
StateKeys getKeys() const { return keys_order_; }
SpecComposite getSpecs() const;
void appendToStructure(const char& _new_key){ keys_order_ += _new_key; }
- bool isInStructure(const char& _sb_key) const { return keys_order_.find(_sb_key) != std::string::npos; }
+ bool hasKey(const char& _sb_key) const { return keys_order_.find(_sb_key) != std::string::npos; }
+ bool hasKeys(const std::string& _keys) const;
std::unordered_map<char, StateBlockConstPtr> getStateBlockMap() const;
std::unordered_map<char, StateBlockPtr> getStateBlockMap();
@@ -381,31 +382,17 @@ inline unsigned int HasStateBlocks::getSize(const StateKeys& _keys) const
return size;
}
-// inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const
-// {
-// const auto& it = std::find_if(state_block_map_.begin(),
-// state_block_map_.end(),
-// [_sb](const std::pair<char, StateBlockConstPtr>& pair)
-// {
-// return pair.second == _sb;
-// }
-// );
-
-// return it;
-// }
-
-// inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
-// {
-// const auto& it = std::find_if(state_block_map_.begin(),
-// state_block_map_.end(),
-// [_sb](const std::pair<char, StateBlockPtr>& pair)
-// {
-// return pair.second == _sb;
-// }
-// );
-
-// return it;
-// }
+inline bool HasStateBlocks::hasKeys(const std::string& _keys) const
+{
+ for (auto key : _keys)
+ {
+ if (not hasKey(key))
+ {
+ return false;
+ }
+ }
+ return true;
+}
inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
{
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 588f735c9..3217b1268 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -57,7 +57,7 @@ static TreeManagerBasePtr create(const std::string& _yaml_filepath,
\
if (not server.applySchema(#TreeManagerClass)) \
{ \
- WOLF_ERROR(server.getLog().str()); \
+ WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
auto params = std::make_shared<ParamsTreeManagerClass>(server.getNode()); \
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
deleted file mode 100644
index 9fd1d9a3e..000000000
--- a/include/core/utils/converter.h
+++ /dev/null
@@ -1,412 +0,0 @@
-//--------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--------
-#pragma once
-
-// wolf
-#include "core/common/wolf.h"
-#include "core/utils/converter_utils.h"
-#include "core/state_block/vector_composite.h"
-#include "core/state_block/state_composite.h"
-
-// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-// std
-#include <regex>
-#include <iostream>
-#include <array>
-#include <vector>
-#include <stack>
-#include <list>
-#include <math.h>
-
-/**
- @file
- converter.h
- @brief This file implements a set of simple functions that deal with the correspondence between
- classes and their string representation. The YAML autosetup framework makes heavy use of this file.
- */
-
-//Note: In order to deal with string representations we make heavy use of regexps.
-// We use the standard C++11 regular expressions capabilities.
-
-/*
-** This file is structured essentially in two parts:
-** in the first part (which starts after the CONVERTERS ~~~~ STRING ----> TYPE line)
-** we have functions to convert from string to C++ class. For example:
- string v1 = "[3,4,5,6,7,8,9,10,11]";
- vector<int> v = converter<vector<int>>::convert(v1);
- This code snippet transforms the string v1 which represents an std::vector into an actual std::vector value.
- The second part (which starts after the TYPES ----> ToSTRING line) has the functions to
- transform from C++ classes to strings. For instance, if we wanted to convert from a C++ class
- to a string we would do something like this:
- vector<int> vect{ 10, 20, 30 };
- string str = converter<std::string>::convert(vect);
- //str == "[10,20,30]"
- */
-namespace wolf{
-
- //This definition is a bit of a "hack". The reason of redefining the pair class is to be able
- //to have two string representations of a pair, namely
- //"(·,·)" -> typical pair/tuple representation
- //"{·,·}" -> key-value pair representation used to represent maps.
- //This is purely an aesthetic reason and could be removed without problems.
-template<class A, class B>
-struct Wpair : std::pair<A,B>
-{
- Wpair(A first, B second): std::pair<A,B>(first, second)
- {
-
- }
-};
- //// CONVERTERS ~~~~ STRING ----> TYPE
-template<typename T>
-struct converter{
- static T convert(std::string val){
- throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: " + val);
- }
-};
-template<typename A>
-struct converter<utils::list<A>>{
- static utils::list<A> convert(std::string val){
- std::regex rgxP("\\[([^,]+)?(,[^,]+)*\\]");
- utils::list<A> result = utils::list<A>();
- if(std::regex_match(val, rgxP)) {
- // std::string aux = val.substr(1,val.size()-2);
- // auto l = utils::getMatches(aux, std::regex("([^,]+)"));
- auto l = utils::parseList(val);
- for(auto it : l){
- // WOLF_DEBUG("Asking to convert in list ", it);
- result.push_back(converter<A>::convert(it));
- }
- } else throw std::runtime_error("Invalid string format representing a list-like structure. Correct format is [(value)?(,value)*]. String provided: " + val);
- return result;
- }
- static std::string convert(utils::list<A> val){
- std::string aux = "";
- bool first = true;
- for(auto it : val){
- if(not first) aux += "," + converter<A>::convert(it);
- else{
- first = false;
- aux = converter<A>::convert(it);
- }
- }
- return "[" + aux + "]";
- }
-};
-template<>
-struct converter<int>{
- static int convert(std::string val){
- double res;
- if (modf(stod(val),&res) > 0)
- throw std::runtime_error("Invalid conversion to int: The number contains decimals: " + val);
- return res;
- }
-};
-template<>
-struct converter<unsigned int>{
- static unsigned int convert(std::string val){
- double res;
- if (modf(stod(val),&res) > 0)
- throw std::runtime_error("Invalid conversion to unsigned int: The number contains decimals: " + val);
- if (res < 0)
- throw std::runtime_error("Invalid conversion to unsigned int: The number is negative: " + val);
- return res;
- }
-};
-template<>
-struct converter<double>{
- static double convert(std::string val){
- return stod(val);
- }
-};
-template<>
-struct converter<bool>{
- static bool convert(std::string val){
- if(val == "true" or val=="True" or val=="TRUE") return true;
- else if (val == "false" or val=="False" or val=="FALSE") return false;
- else throw std::runtime_error("Invalid conversion to bool (Must be either \"true\" or \"false\"). String provided: " + val);
- }
-};
-template<>
-struct converter<char>{
- static char convert(std::string val){
- //Here we should check that val.length() == 1 and get val[0] into a char variable
- if(val.length() == 1) return val.at(0);
- else throw std::runtime_error("Invalid converstion to char. String too long. String provided: " + val);
- throw std::runtime_error("Invalid char conversion. String provided: " + val);
- }
-};
- //// TYPES ----> ToSTRING
-template<>
-struct converter<std::string>{
- static std::string convert(std::string val){
- return val;
- }
- template<typename T>
- static std::string convert(T val){
- // throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val);
- throw std::runtime_error("There is no general convert to string for arbitrary T !!!");
- }
- static std::string convert(int val){
- return std::to_string(val);
- }
- static std::string convert(double val){
- return std::to_string(val);
- }
- static std::string convert(char val){
- return std::to_string(val);
- }
- template<typename A>
- static std::string convert(utils::list<A> val){
- std::string result = "";
- for(auto it : val){
- result += "," + converter<std::string>::convert(it);
- }
- if(result.size() > 0) result = result.substr(1,result.size());
- return "[" + result + "]";
- }
- template <typename A> static std::string convert(std::list<A> val) {
- std::string result = "";
- for (auto it : val) {
- result += "," + converter<std::string>::convert(it);
- }
- if (result.size() > 0) result = result.substr(1, result.size());
- return "[" + result + "]";
- }
- template<typename A>
- static std::string convert(std::set<A> val){
- std::string result = "";
- for(auto it : val){
- result += "," + converter<std::string>::convert(it);
- }
- if(result.size() > 0) result = result.substr(1,result.size());
- return "[" + result + "]";
- }
- template<typename A, typename B>
- static std::string convert(std::pair<A,B> val){
- return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
- }
- //NEW GUY
- template<typename A, typename B>
- static std::string convert(Wpair<A,B> val){
- return "(" + converter<std::string>::convert(val.first) + "," + converter<std::string>::convert(val.second) + ")";
- }
- template<typename A, typename B>
- static std::string convert(std::map<A,B> val){
- std::string result = "";
- for(auto it : val){
- result += "," + converter<std::string>::convert(it);
- }
- if(result.size() > 0) result = result.substr(1,result.size());
- return "[" + result + "]";
- }
- template<typename A, typename B>
- static std::string convert(std::unordered_map<A,B> val){
- std::string result = "";
- for(auto it : val){
- result += "," + converter<std::string>::convert(it);
- }
- if(result.size() > 0) result = result.substr(1,result.size());
- return "[" + result + "]";
- }
- template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
- static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){
- std::string result = "";
- for(int i = 0; i < val.rows() ; ++i){
- for(int j = 0; j < val.cols(); ++j){
- result += "," + converter<std::string>::convert(val(i,j));
- }
- }
- if(result.size() > 0) result = result.substr(1,result.size());
- if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic){
- result = "[" + std::to_string(val.rows()) + "," + std::to_string(val.cols()) + "]," + result;
- }
- return "[" + result + "]";
- }
- //// WOLF DEFINED TYPES -----> TO STRING
- static std::string convert(VectorComposite val){
- //TODO
- // throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
- auto helper = std::unordered_map<char, Eigen::VectorXd>();
- for(const auto& pair: val)
- helper.insert(std::pair<char, Eigen::VectorXd>(pair.first, pair.second));
- return converter<std::string>::convert(helper);
- }
- static std::string convert(MatrixComposite val){
- //TODO
- // throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
- auto helper = std::unordered_map< char, std::unordered_map<char, Eigen::MatrixXd>>();
- for(const auto& pair: val)
- helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(pair.first, pair.second));
- return converter<std::string>::convert(helper);
- }
-};
- //// CONVERTERS ~~~~ TYPE ----> STRING
-template<typename A, typename B>
-struct converter<std::pair<A,B>>{
- static std::pair<A,B> convert(std::string val){
- std::regex rgxP("\\{([^\\{:]+):([^\\}]+)\\}");
- std::smatch matches;
- if(std::regex_match(val, matches, rgxP)) {
- return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
- } else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val);
- }
-};
- //NEW GUY
-template<typename A, typename B>
-struct converter<Wpair<A,B>>{
- static Wpair<A,B> convert(std::string val){
- std::regex rgxP("\\(([^\\(,]+),([^\\)]+)\\)");
- std::smatch matches;
- if(std::regex_match(val, matches, rgxP)) {
- return Wpair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
- } else throw std::runtime_error("Invalid string format representing a Wpair. Correct format is (first,second). String provided: " + val);
- }
-};
-// TODO: WARNING!! For some reason when trying to specialize converter to std::array
-// it defaults to the generic T type, thus causing an error!
-
-template<typename A, unsigned int N>
-struct converter<std::array<A, N>>{
- static std::array<A,N> convert(std::string val){
- // std::vector<std::string> aux = utils::splitter(val);
- auto aux = converter<utils::list<A>>::convert(val);
- std::array<A,N> rtn = std::array<A,N>();
- if(N != aux.size()) throw std::runtime_error("Error in trying to transform literal string to Array. Invalid argument size. Required size "
- + std::to_string(N) + "; provided size " + std::to_string(aux.size()));
- for (int i = 0; i < N; ++i) {
- rtn[i] = aux[i];
- }
- return rtn;
- }
-};
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>{
- typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> Matrix;
- static Matrix convert(std::string val){
- auto splittedValues = utils::splitMatrixStringRepresentation(val);
- auto dimensions = converter<std::vector<int>>::convert(splittedValues[0]);
- auto values = converter<std::vector<_Scalar>>::convert(splittedValues[1]);
- Matrix m = Matrix();
- if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic) {
- if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing a dynamic Eigen Matrix. Missing dimensions. String provided: " + val);
- m.resize(dimensions[0],dimensions[1]);
- }else if(_Rows == Eigen::Dynamic){
- int nrows = (int) values.size() / _Cols;
- m.resize(nrows, _Cols);
- }else if(_Cols == Eigen::Dynamic){
- int ncols = (int) values.size() / _Rows;
- m.resize(_Rows, ncols);
- }
- if(m.rows()*m.cols() != (int) values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
- + "the Eigen matrix is of dimensions "
- + std::to_string(m.rows()) + "x" + std::to_string(m.cols()));
- else{
- for (int i = 0; i < m.rows(); i++)
- for (int j = 0; j < m.cols(); j++)
- m(i, j) = values[(int)(i * m.cols() + j)];
- }
- return m;
- }
-};
-template<typename A>
-struct converter<std::map<std::string,A>>{
- static std::map<std::string,A> convert(std::string val){
- std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
- if(not std::regex_match(val, rgxM))
- throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
-
- auto v = utils::parseList(val);
- auto map = std::map<std::string, A>();
- for(auto it : v){
- auto p = converter<std::pair<std::string, A>>::convert(it);
- map.insert(std::pair<std::string, A>(p.first, p.second));
- }
- return map;
- }
-};
-template<typename A, typename B>
-struct converter<std::map<A,B>>{
- static std::map<A,B> convert(std::string val){
- std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
- if(not std::regex_match(val, rgxM))
- throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
-
- auto v = utils::parseList(val);
- auto map = std::map<A, B>();
- for(auto it : v){
- auto p = converter<std::pair<A,B>>::convert(it);
- map.insert(std::pair<A, B>(p.first, p.second));
- }
- return map;
- }
-};
-template<typename A, typename B>
-struct converter<std::unordered_map<A,B>>{
- static std::unordered_map<A,B> convert(std::string val){
- std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
- if(not std::regex_match(val, rgxM))
- throw std::runtime_error("Invalid string representation of an unordered Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
-
- auto v = utils::parseList(val);
- auto map = std::unordered_map<A, B>();
- for(auto it : v){
- auto p = converter<std::pair<A,B>>::convert(it);
- map.insert(std::pair<A, B>(p.first, p.second));
- }
- return map;
- }
-};
-//// FROM STRING -----> WOLF DEFINED TYPES
-template<>
-struct converter<VectorComposite>{
- static VectorComposite convert(std::string val){
- auto unordered_map = converter<std::unordered_map<char, Eigen::VectorXd>>::convert(val);
- // VectorComposite result = VectorComposite(unordered_map);
- // return result;
- auto helper = VectorComposite();
- for(auto const& it: unordered_map)
- {
- helper.insert(std::pair<char, Eigen::VectorXd>(it.first, it.second));
- }
- return helper;
- }
-};
-template<>
-struct converter<MatrixComposite>{
- static MatrixComposite convert(std::string val){
- auto unordered_map = converter<std::unordered_map<char,
- std::unordered_map<char, Eigen::MatrixXd>>>::convert(val);
- // VectorComposite result = VectorComposite(unordered_map);
- // return result;
- auto helper = MatrixComposite();
- for(auto const& it: unordered_map)
- {
- helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(it.first, it.second));
- }
- return helper;
- }
-};
-}
\ No newline at end of file
diff --git a/include/core/utils/converter_utils.h b/include/core/utils/converter_utils.h
deleted file mode 100644
index 2e3bca610..000000000
--- a/include/core/utils/converter_utils.h
+++ /dev/null
@@ -1,64 +0,0 @@
-//--------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--------
-#pragma once
-
-// wolf
-#include "core/common/wolf.h"
-
-// std
-#include <regex>
-namespace utils{
- //Typically we want to convert from/to list-type structures. In order to be general
- //we define a list type which is used throughout the converter. In this case this type
- //is implemented with std::vector
- template <typename A>
- using list = std::vector<A>;
- // template <typename A>
- // class toString<A>{};
- /** @Brief Splits a comma-separated string into its pieces
- * @param val is just the string of comma separated values
- * @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value
- */
- std::vector<std::string> splitter(std::string val);
- /** @Brief Returns all the substrings of @val that match @exp
- * @param val String to be matched
- * @param exp Regular expression
- * @return <b>{std::vector<std::string>}</b> Collection of matching substrings
- */
- std::vector<std::string> getMatches(std::string val, std::regex exp);
- /** @Brief Given a string representation of a matrix extracts the dimensions and the values
- * @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]"
- * @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
- */
- std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix);
- /** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation
- * @param val is just a dictionary-like string
- * @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i}
- */
- std::vector<std::string> pairSplitter(std::string val);
- /** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...]
- * @param str_map just a dictionary-like string
- * @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets)
- */
- std::string splitMapStringRepresentation(std::string str_map);
- std::vector<std::string> parseList(std::string val);
-}
\ No newline at end of file
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
deleted file mode 100644
index 812f39e24..000000000
--- a/include/core/utils/params_server.h
+++ /dev/null
@@ -1,76 +0,0 @@
-//--------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--------
-#pragma once
-
-#include "core/utils/converter.h"
-
-#include <map>
-#include <exception>
-
-namespace wolf{
-
-class MissingValueException : public std::runtime_error
-{
-public:
- MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
-};
-
-class ParamsServer
-{
- private:
-
- std::string global_prefix_;
- std::map<std::string, std::string> params_;
-
- public:
-
- ParamsServer(const std::string& _global_prefix = "");
- ParamsServer(std::map<std::string, std::string> _params, const std::string& _global_prefix = "");
- ~ParamsServer()
- {
- //
- }
-
- void print();
-
- bool hasParam(std::string _key) const;
-
- void addParam(std::string _key, std::string _value);
-
- void addParams(std::map<std::string, std::string> _params);
-
- template<typename T>
- T getParam(std::string _key) const
- {
- if(params_.count(_key))
- return converter<T>::convert(params_.at(_key));
- else
- throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
- }
-};
-
-inline bool ParamsServer::hasParam(std::string _key) const
-{
- return params_.count(_key);
-}
-
-}
\ No newline at end of file
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
deleted file mode 100644
index a79a36ac2..000000000
--- a/include/core/yaml/parser_yaml.h
+++ /dev/null
@@ -1,100 +0,0 @@
-//--------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--------
-#pragma once
-
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-
-#include "yaml-cpp/yaml.h"
-
-namespace wolf
-{
-class ParserYaml {
- struct ParamsInitSensor{
- std::string type_;
- std::string name_;
- std::string plugin_;
- YAML::Node n_;
- };
- struct ParamsInitProcessor{
- std::string type_;
- std::string name_;
- std::string name_assoc_sensor_;
- std::string plugin_;
- YAML::Node n_;
- };
- struct SubscriberManager{
- std::string package_;
- std::string type_;
- std::string topic_;
- std::string sensor_name_;
- YAML::Node n_;
- };
- struct PublisherManager{
- std::string package_;
- std::string type_;
- std::string topic_;
- std::string period_;
- YAML::Node n_;
- };
- std::map<std::string, std::string> params_;
- std::string active_name_;
- std::vector<ParamsInitSensor> paramsSens_;
- std::vector<ParamsInitProcessor> paramsProc_;
- std::stack<std::string> parsing_file_;
- std::string file_;
- std::string path_root_;
- std::vector<SubscriberManager> subscriber_managers_;
- std::vector<PublisherManager> publisher_managers_;
- YAML::Node problem;
- std::string generatePath(std::string);
- YAML::Node loadYaml(std::string);
- void insert_register(std::string, std::string);
-public:
- ParserYaml(std::string _file, bool _freely_parse = false);
- ~ParserYaml()
- {
- //
- }
- void parse_freely();
- std::map<std::string, std::string> getParams();
-
- private:
- void walkTree(std::string _file);
- void walkTree(std::string _file, std::vector<std::string>& _tags);
- void walkTree(std::string _file, std::vector<std::string>& _tags, std::string _hdr);
-/** @Brief Recursively walks the YAML tree while filling a map with the values parsed from the file
- * @param YAML node to be parsed
- * @param tags represents the path from the root of the YAML tree to the current node
- * @param hdr is the name of the current YAML node
- */
- void walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string _hdr);
- void updateActiveName(std::string _tag);
-/** @Brief Parse the sensors, processors, callbacks and files elements of the YAML file. We assume that these elements
- are defined at the top level of the YAML file.
- * @param file is the path to the YAML file
- * @param _n is the top YAML::Node of file*/
- void parseFirstLevel(YAML::Node _n, std::string _file);
- std::string tagsToString(std::vector<std::string>& _tags);
- void parse();
-};
-}
\ No newline at end of file
diff --git a/include/core/yaml/yaml_conversion.h b/include/core/yaml/yaml_conversion.h
deleted file mode 100644
index 15dad4cd0..000000000
--- a/include/core/yaml/yaml_conversion.h
+++ /dev/null
@@ -1,225 +0,0 @@
-//--------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--------
-#pragma once
-
-// Yaml
-#include <yaml-cpp/yaml.h>
-
-// Eigen
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-// stl
-#include <iostream>
-
-namespace YAML
-{
-
-/**\brief Bridge YAML to and from Eigen::Matrix< >.
- *
- */
-template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
-struct convert<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> >
-{
- /** \brief Encode a YAML Sequence from an Eigen::Matrix< >
- */
- static Node encode(const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
- {
- Node node; //(NodeType::Sequence);
-
- int nValues = matrix.rows() * matrix.cols();
-
- for (int i = 0; i < nValues; ++i)
- {
- node.push_back(matrix(i / matrix.cols(), i % matrix.cols()));
- }
-
- return node;
- }
-
- /** \brief Decode a YAML sequence into a ````Eigen::Matrix<typename _Scalar, int _Rows, int _Cols>````
- *
- * Two YAML formats are accepted:
- *
- * - For matrices where at least one dimension is not Dynamic:
- * We use a single sequence of matrix entries: the matrix dimensions are inferred
- * ````
- * [ v1, v2, v3, ...]
- * ````
- * - For all kinds of matrices. We use a sequence of two sequences.
- * The first sequence is a vector of dimensions; the second sequence contains the matrix entries
- * ````
- * [ [ rows, cols ], [v1, v2, v3, ...] ]
- * ````
- */
- static bool decode(const Node& node, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& matrix)
- {
- YAML::Node values;
-
- // ==========================================================================================
- if (node[0].Type() == NodeType::Sequence) // sizes given by YAML
- {
- if (node.size() == 2 && node[0].size() == 2 && node[1].Type() == NodeType::Sequence
- && node[1].size() == node[0][0].as<unsigned int>() * node[0][1].as<unsigned int>()) // YAML string is well formed
- {
- int rows = node[0][0].as<int>();
- int cols = node[0][1].as<int>();
- values = node[1];
- if (_Rows == Eigen::Dynamic && _Cols == Eigen::Dynamic) // full dynamic
- {
- matrix.resize(rows, cols);
- }
- else if (_Rows == Eigen::Dynamic) // rows dynamic
- {
- if (_Cols != cols)
- {
- std::cout << "Wrong number of cols" << std::endl;
- return false;
- }
- matrix.resize(rows, Eigen::NoChange);
- }
- else if (_Cols == Eigen::Dynamic) // cols dynamic
- {
- if (_Rows != rows)
- {
- std::cout << "Wrong number of rows" << std::endl;
- return false;
- }
- matrix.resize(Eigen::NoChange, cols);
- }
- else // fixed size
- {
- if (_Rows != rows || _Cols != cols)
- {
- std::cout << "Wrong size" << std::endl;
- return false;
- }
- }
- }
- else
- {
- std::cout << "Bad matrix specification" << std::endl;
- return false;
- }
- }
- else // sizes given by Matrix
- {
- // If full dynamic -> error
- if (_Rows == Eigen::Dynamic && _Cols == Eigen::Dynamic)
- {
- std::cout << "Bad yaml format. A full dynamic matrix requires [ [rows, cols], [... values ...] ]"
- << std::endl;
- return false;
- }
- values = node;
-
- // If one dimension is dynamic -> calculate and resize
-
- // _Rows is Dynamic
- if (_Rows == Eigen::Dynamic)
- {
- if (values.size() % _Cols != 0)
- {
- std::cout << "Input size of dynamic row matrix is not a multiple of fixed column size"
- << std::endl;
- return false;
- }
-
- int nDynRows = values.size() / _Cols;
- matrix.resize(nDynRows, Eigen::NoChange);
- }
-
- // _Cols is Dynamic
- if (_Cols == Eigen::Dynamic)
- {
- if (values.size() % _Rows != 0)
- {
- std::cout << "Input size of dynamic column matrix is not a multiple of fixed row size"
- << std::endl;
- return false;
- }
-
- int nDynCols = values.size() / _Rows;
- matrix.resize(Eigen::NoChange, nDynCols);
- }
- }
-
- // final check for good size
- if (values.size() != (unsigned int)(matrix.rows() * matrix.cols()))
- {
- std::cout << "Wrong input size" << std::endl;
- return false;
- }
- else // Fill the matrix
- {
- for (int i = 0; i < matrix.rows(); i++)
- for (int j = 0; j < matrix.cols(); j++)
- matrix(i, j) = values[(int)(i * matrix.cols() + j)].as<_Scalar>();
- }
- return true;
- }
-};
-
-/**\brief Bridge YAML <--> Eigen::Quaternion with real component last
- *
- * WARNING: Beware of Eigen constructor order!
- *
- * We use the x-y-z-w order, with the real part at the end. This is consistent with ROS Quaternion.msg,
- * which is good for compatibility against ROS messages and YAML configuration.
- *
- */
-template<typename _Scalar, int _Options>
-struct convert<Eigen::Quaternion<_Scalar, _Options> >
-{
- static Node encode(const Eigen::Quaternion<_Scalar, _Options>& quaternion)
- {
- Node node(NodeType::Sequence);
-
- node[0] = quaternion.x();
- node[1] = quaternion.y();
- node[2] = quaternion.z();
- node[3] = quaternion.w();
-
- return node;
- }
-
- static bool decode(const Node& node, Eigen::Quaternion<_Scalar, _Options>& quaternion)
- {
-
- int nSize = node.size(); // Sequence check is implicit
- if (nSize != 4)
- {
- std::cout << "Wrong quaternion input size!" << std::endl;
- return false;
- }
- else
- {
- quaternion.x() = node[0].as<_Scalar>();
- quaternion.y() = node[1].as<_Scalar>();
- quaternion.z() = node[2].as<_Scalar>();
- quaternion.w() = node[3].as<_Scalar>();
- }
- return true;
- }
-};
-
-} // namespace YAML
\ No newline at end of file
diff --git a/schema/sensor/SensorPose2d.schema b/schema/sensor/SensorPose2d.schema
new file mode 100644
index 000000000..d042172be
--- /dev/null
+++ b/schema/sensor/SensorPose2d.schema
@@ -0,0 +1,20 @@
+follow: SensorBase.schema
+
+std_p:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: standard deviation of the position measurement.
+
+std_o:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: standard deviation of the orientation measurement.
+
+states:
+ P:
+ follow: SpecStateSensorP2d.schema
+ O:
+ follow: SpecStateSensorO2d.schema
+
diff --git a/schema/sensor/SensorPose3d.schema b/schema/sensor/SensorPose3d.schema
new file mode 100644
index 000000000..2fa18b6e5
--- /dev/null
+++ b/schema/sensor/SensorPose3d.schema
@@ -0,0 +1,20 @@
+follow: SensorBase.schema
+
+std_p:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: standard deviation of the position measurement.
+
+std_o:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: standard deviation of the orientation measurement.
+
+states:
+ P:
+ follow: SpecStateSensorP3d.schema
+ O:
+ follow: SpecStateSensorO3d.schema
+
diff --git a/schema/solver/SolverCeres.schema b/schema/solver/SolverCeres.schema
index c262f81cb..5b3a035ca 100644
--- a/schema/solver/SolverCeres.schema
+++ b/schema/solver/SolverCeres.schema
@@ -31,12 +31,12 @@ gradient_tolerance:
type: double
yaml_type: scalar
doc: "Gradient tolerance. Convergence criterion. Typical value: 1e-8"
-num_threads:
+n_threads:
mandatory: true
type: unsigned int
options: [1, 2, 3, 4]
yaml_type: scalar
- doc: Amount of hreads used by ceres.
+ doc: Amount of threads used by ceres.
use_nonmonotonic_steps:
mandatory: false
default: false
diff --git a/schema/tree_manager/TreeManagerSlidingWindow.schema b/schema/tree_manager/TreeManagerSlidingWindow.schema
new file mode 100644
index 000000000..8d584a1a0
--- /dev/null
+++ b/schema/tree_manager/TreeManagerSlidingWindow.schema
@@ -0,0 +1,25 @@
+follow: TreeManagerBase.schema
+
+type:
+ mandatory: true
+ type: string
+ yaml_type: scalar
+ doc: Type of the TreeManager. To keep all frames, use "none".
+
+n_frames:
+ mandatory: true
+ type: unsigned int
+ yaml_type: scalar
+ doc: Total number of frames of the sliding window.
+
+n_fix_first_frames:
+ mandatory: true
+ type: unsigned int
+ yaml_type: scalar
+ doc: Amount of frames fixed at the begining of the sliding window.
+
+viral_remove_empty_parent:
+ mandatory: true
+ type: bool
+ yaml_type: scalar
+ doc: If the other wolf nodes (landmarks) have to be removed when removing frames. Otherwise, they will remain alive but unconstrained.
diff --git a/schema/tree_manager/TreeManagerSlidingWindowDualRate.schema b/schema/tree_manager/TreeManagerSlidingWindowDualRate.schema
new file mode 100644
index 000000000..c7077e1a2
--- /dev/null
+++ b/schema/tree_manager/TreeManagerSlidingWindowDualRate.schema
@@ -0,0 +1,13 @@
+follow: TreeManagerSlidingWindow.schema
+
+n_frames_recent:
+ mandatory: true
+ type: unsigned int
+ yaml_type: scalar
+ doc: Amount of frames kept in the recent part of the sliding window.
+
+rate_old_frames:
+ mandatory: true
+ type: unsigned int
+ yaml_type: scalar
+ doc: Rate of keyframes that are kept from the recent part to the sparse part of the sliding window. One of each 'rate_old_frames' will be kept.
\ No newline at end of file
diff --git a/scilab/corner_detector.sce b/scilab/corner_detector.sce
deleted file mode 100644
index dfeae1945..000000000
--- a/scilab/corner_detector.sce
+++ /dev/null
@@ -1,237 +0,0 @@
-//info about 2d homogeneous lines and points: http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
-
-// clear all
-xdel(winsid());
-clear;
-
-//scan params:
-Ns = 720; //scan rays
-aperture = %pi; //scan aperture [rad]
-azimuth_step = aperture/Ns;
-
-//User Tunning params
-Nw = 8; //window size
-theta_th = %pi/8;
-theta_max = 0.3;
-K = 3; //How many std_dev are tolerated to count that a point is supporting a line
-r_stdev = 0.01; //ranging std dev
-max_beam_dist = 5; //max distance in amount of beams to consider concatenation of two lines
-max_dist = 0.2; //max distance to a corner from the ends of both lines to take it
-range_jump_th = 0.5; //threshold of distance to detect jumps in ranges
-max_points_line = 1000; //max amount of beams of a line
-
-//init
-points = [];
-result_lines = [];
-line_indexes = [];
-corners = [];
-corners_jumps = [];
-jumps = [];
-
-//scan ranges
-ranges = read(fullpath('scan.txt'),-1,Ns);
-//ranges = [];
-
-//invent a set of points + noise
-//points = [1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43;
-// 7 6 5 4 3 2 1 2 3 4 5 6 7 8 9 10 9 8 7 6 5 4 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 7.4 7.3 7.2 7.1 7 6.9 6.8 6.7 6.6 6.5 6.4];
-for i=1:Ns
- points = [points [ranges(i)*cos(aperture/2 - azimuth_step*i); ranges(i)*sin(aperture/2 - azimuth_step*i)]];
- // Store range jumps
- if i>1 & abs(ranges(i)-ranges(i-1)) > range_jump_th then
- jumps = [jumps i];
- end
-end
-
-points = points + rand(points,"normal")*r_stdev;
-
-//Main loop. Runs over a sliding window of Nw points
-i_jump = 1;
-for i = Nw:size(points,2)
-
- //set the window indexes
- i_from = i-Nw+1;
- i_to = i;
- points_w = points(:,i_from:i_to);
-
- //update the jump to be checked
- while i_jump < size(jumps,2) & i_from > jumps(i_jump)
- i_jump = i_jump+1;
- end
-
- //check if there is a jump inside the window (if it is the case, do not fit a line)
- if jumps(i_jump) > i_from & jumps(i_jump) <= i_to then
- continue;
- end
-
- //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij
- a_00 = sum( points_w(1,:).^2 );
- a_01 = sum( points_w(1,:).*points_w(2,:) );
- a_02 = sum( points_w(1,:) );
- a_10 = a_01;
- a_11 = sum( points_w(2,:).^2 );
- a_12 = sum( points_w(2,:) );
- a_20 = a_02;
- a_21 = a_12;
- a_22 = Nw;
- A1 = [a_00 a_01 a_02; a_10 a_11 a_12; a_20 a_21 a_22; 0 0 1];
-
- a_00 = sum( points_w(1,:).^2 );
- a_01 = sum( points_w(1,:).*points_w(2,:) );
- a_02 = sum( points_w(1,:) );
- a_10 = a_01;
- a_11 = sum( points_w(2,:).^2 );
- a_12 = sum( points_w(2,:) );
- A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-
- //solve
- line1 = pinv(A1)*[zeros(3,1);1];
- line = inv(A)*[0; 0; 1];
- //disp(line1-line);
-
- //compute error
- err = 0;
- for j=1:Nw
- err = err + abs(line'*[points_w(:,j);1])/sqrt(line(1)^2+line(2)^2);
- end
- err = err/Nw;
- //disp("error: "); disp(err);
-
- //if error below stdev, add line to result set
- if err < K*r_stdev then
- result_lines = [result_lines [line;points_w(:,1);points_w(:,$)]];
- line_indexes = [line_indexes [i_from; i_to]]; //ray index where the segment ends
- end
-end
-
-//line concatenation
-j = 1;
-while (j < size(result_lines,2))
- // beams between last of current line and first of next line
- if (line_indexes(1,j+1)-line_indexes(2,j)) > max_beam_dist then
- j=j+1;
- else
- //compute angle diff between consecutive segments
- cos_theta = result_lines(1:2,j)'*result_lines(1:2,j+1) / ( norm(result_lines(1:2,j)) * norm(result_lines(1:2,j+1)) );
- theta = abs(acos(cos_theta));
-
- //if angle diff lower than threshold, concatenate
- if theta < theta_max then
-
- //set the new window
- i_from = line_indexes(1,j);
- i_to = line_indexes(2,j+1);
- points_w = points(:,i_from:i_to);
-
- //Found the best fitting line over the window. Build the system: Ax=0. Matrix A = a_ij
- a_00 = sum( points_w(1,:).^2 );
- a_01 = sum( points_w(1,:).*points_w(2,:) );
- a_02 = sum( points_w(1,:) );
- a_10 = a_01;
- a_11 = sum( points_w(2,:).^2 );
- a_12 = sum( points_w(2,:) );
- A = [a_00 a_01 a_02; a_10 a_11 a_12; 0 0 1];
-
- //solve
- line = inv(A)*[0; 0; 1];
-
- //compute error
- err = 0;
- for k=1:Nw
- err = err + abs(line'*[points_w(:,k);1])/sqrt(line(1)^2+line(2)^2);
- end
- err = err/Nw;
-
- //if error below stdev, change line to concatenation and erase the next line
- if err < K*r_stdev then
- result_lines(:,j) = [line;points_w(:,1);points_w(:,$)];
- line_indexes(:,j) = [i_from; i_to];
- result_lines = [result_lines(:,1:j) result_lines(:,j+2:$)];
- line_indexes = [line_indexes(:,1:j) line_indexes(:,j+2:$)];
- if (i_to-i_from)>max_points_line then
- j=j+1;
- end
- else
- j=j+1;
- end
- else
- j=j+1;
- end
- end
-end
-
-//corner detection
-for i = 1:(size(result_lines,2)-1)
- for j = i+1:size(result_lines,2)
- // beams between last of current line and first of next line
- if (line_indexes(1,j)-line_indexes(2,i)) > max_beam_dist then
- break;
- end
- //compute angle diff between consecutive segments
- cos_theta = result_lines(1:2,i)'*result_lines(1:2,j) / ( norm(result_lines(1:2,i)) * norm(result_lines(1:2,j)) );
- theta = abs(acos(cos_theta));
-
- //if angle diff greater than threshold && indexes are less than Nw, we decide corner
- if theta > theta_th then
- //Corner found! Compute "sub-pixel" corner location as the intersection of two lines
- corner = cross(result_lines(1:3,i),result_lines(1:3,j));
- corner = corner./corner(3);//norlamlize homogeneous point
- // Check if the corner is close to both lines ends
- if (norm(corner(1:2)-points(:,line_indexes(2,i))) < max_dist & norm(corner(1:2)-points(:,line_indexes(1,j))) < max_dist) then
- corners = [corners corner];
- end
- //display
- //disp("theta: "); disp(theta);
- //disp("index:" ); disp(line_indexes(i)-Nw+1);//line_indexes(i) indicates the end point of the segment
- end
- end
-end
-
-// corner detection from jumps
-for i=1:size(jumps,2)
- if ranges(jumps(i)) < ranges(jumps(i)-1) then
- corners_jumps = [corners_jumps points(:,jumps(i))];
- else
- corners_jumps = [corners_jumps points(:,jumps(i)-1)];
- end
-end
-
-//Set figure
-fig1 = figure(0);
-fig1.background = 8;
-
-//plot points
-plot(points(1,:),points(2,:),"g.");
-
-//axis settings
-ah = gca();
-ah.isoview = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.grid = [0.1,0.1,0.1];
-ah.grid_position = "background";
-
-
-//plot lines
-for i=1:size(result_lines,2)
- m = -result_lines(1,i)/result_lines(2,i);
- xc = -result_lines(3,i)/result_lines(2,i);
- point1 = [result_lines(4,i) m*result_lines(4,i)+xc];
- point2 = [result_lines(6,i) m*result_lines(6,i)+xc];
- xpoly([point1(1) point2(1)],[point1(2) point2(2)]);
-end
-
-//plot corners
-plot(corners(1,:),corners(2,:),"ro");
-plot(corners_jumps(1,:),corners_jumps(2,:),"bo");
-
-disp(corners_jumps');
-disp(corners');
-
-//plot jumps
-//for i=1:size(jumps,2)
-// plot(ranges(jumps(i))*cos(aperture/2 - azimuth_step*jumps(i)), ranges(jumps(i))*sin(aperture/2 - azimuth_step*jumps(i)),"bx");
-//end
-
-
diff --git a/scilab/scan.txt b/scilab/scan.txt
deleted file mode 100644
index d19cf457c..000000000
--- a/scilab/scan.txt
+++ /dev/null
@@ -1 +0,0 @@
-2.78886,2.78289,2.7832,2.78367,2.7843,2.78508,2.78603,2.78713,2.78839,2.78981,2.79139,2.78669,2.78855,2.79057,2.79274,2.79507,2.79098,2.79359,2.79635,2.79927,2.79566,2.79886,2.80221,2.79895,2.80258,2.80638,2.80345,2.80753,2.81176,2.80917,2.81368,2.81132,2.81611,2.81396,2.81903,2.73664,2.72743,2.71782,2.70883,2.69945,2.69067,2.6815,2.67294,2.66398,2.65562,2.64736,2.6387,2.63064,2.62218,2.61431,2.60605,2.59837,2.59078,2.5828,2.57539,2.56759,2.56036,2.55321,2.54568,2.53871,2.53182,2.52455,2.51782,2.51118,2.50415,2.49767,2.49127,2.48495,2.47824,2.47207,2.46597,2.4595,2.45355,2.44768,2.44188,2.4357,2.43005,2.42446,2.41894,2.4135,2.40768,2.40236,2.39712,2.39195,2.3864,2.38135,2.37637,2.37146,2.36661,2.36182,2.35666,2.352,2.3474,2.34286,2.34186,2.36289,2.41051,2.43311,2.45628,2.48003,2.50439,2.52937,2.555,2.61224,2.63993,2.66837,2.69757,2.72758,2.75841,2.79011,2.82269,2.85621,2.8907,2.96659,3.0042,3.04295,3.08289,3.12406,3.16652,3.21033,3.22693,3.23444,3.24207,3.24982,3.2577,3.26571,3.2855,3.29383,3.30229,3.31088,3.3196,3.32846,3.33745,3.34658,3.35584,3.36524,3.37478,3.38445,3.39427,3.40423,3.41434,3.42459,3.43499,3.44553,3.45623,3.46707,3.47807,3.48923,3.50053,3.512,3.52362,3.53541,3.54736,3.55947,3.57175,3.5842,3.59681,3.6096,3.62256,3.63569,3.64901,3.6625,3.67617,3.69003,3.70407,3.7183,3.73272,3.74733,3.76214,3.7931,3.80843,3.82397,3.83972,3.85567,3.87183,3.88822,3.90481,3.92163,3.93867,3.95593,3.97343,3.97347,3.99127,4.00931,4.02758,4.0461,4.06486,4.08387,4.10314,4.12266,4.14244,4.16248,4.20237,4.22313,4.24418,4.26551,4.28712,4.30903,4.33122,4.35372,4.37652,4.39963,4.42304,4.44678,4.47084,4.49523,4.51994,4.545,4.57041,4.59615,4.62225,4.64871,4.67554,4.70274,4.73032,4.75828,4.78663,4.81538,4.84452,4.8741,4.90409,4.9345,4.96534,4.99662,5.02836,5.06053,5.09317,5.12628,5.15987,5.19395,5.22853,5.2636,5.2992,5.33532,5.37197,5.44106,5.47923,5.51796,5.55729,5.59721,5.63773,5.67888,5.72066,5.76307,5.80615,5.8499,5.89432,5.93945,6.02374,6.07085,6.11872,6.16734,6.21676,6.26698,6.318,6.36987,6.42258,6.47618,6.5758,6.6319,6.68894,6.74694,6.80593,6.86593,6.92698,7.04022,7.10425,7.1694,7.23572,7.30322,7.37192,7.49928,7.57149,7.64505,7.58372,7.51951,7.45681,7.32129,7.32938,7.34276,7.35632,7.36877,7.38272,7.39687,7.41124,7.4258,7.43923,7.4542,7.46937,7.48477,7.49904,7.51485,7.53087,7.54579,7.56225,7.57894,7.59587,7.61164,7.62902,8.37389,8.39194,8.41173,8.43177,8.45059,8.47118,8.49202,8.51164,8.53305,8.55319,8.57516,8.59739,8.61839,8.64122,8.6628,8.6862,8.70837,8.73237,8.7567,8.77979,8.80472,8.82843,8.85402,8.87835,8.9046,8.9296,8.9565,8.98218,9.0082,9.03616,9.06287,9.09152,9.11895,9.14834,9.17652,9.20503,9.23557,9.26487,9.29454,9.32626,9.35674,9.38762,9.42055,9.45226,9.4844,9.51695,9.55162,9.58503,9.61893,9.65324,4.38929,4.38536,4.36058,4.3365,4.3131,4.29036,4.26827,4.24682,4.22598,4.20576,4.18612,4.1944,4.17582,4.15708,4.13859,4.12032,4.10229,4.08449,4.06691,4.04955,4.03241,4.01549,3.99916,3.98265,3.96634,3.95024,3.93434,3.91901,3.90349,3.88817,3.87304,3.85845,3.84368,3.82909,3.81504,3.80081,3.78674,3.7732,3.75948,3.74591,3.73287,3.71963,3.7069,3.69398,3.68156,3.66894,3.65647,3.6445,3.63233,3.62065,3.60876,3.59736,3.58576,3.58265,3.61553,3.62696,3.63867,3.67347,3.68596,3.72229,3.7356,3.77355,3.78772,3.80219,3.84244,3.85785,3.89993,3.9163,3.93303,3.97774,3.99551,4.01367,4.06121,4.0805,4.10019,4.15081,4.17174,4.19309,13.8062,13.7714,13.7384,13.7075,13.8936,13.9735,14.0549,14.1382,14.3407,15.8017,15.9009,16.002,16.1054,16.3519,16.462,16.5744,16.6893,16.9594,17.0819,17.2072,17.3352,17.4661,17.6,8.14878,8.1334,8.11823,8.10324,8.08848,8.07391,8.0588,8.04465,8.03069,8.01693,8.00338,7.99,7.97684,7.96312,7.95032,7.93773,7.92533,7.91309,7.90106,7.88922,7.87755,7.86606,7.85476,7.84289,7.83195,7.82116,7.81058,7.80017,7.78993,7.77984,7.76995,7.76021,7.75066,7.74128,7.73204,7.76034,7.99805,8.11853,8.24311,8.37202,12.3718,12.3587,12.346,12.3336,12.3213,12.3094,12.2976,12.2862,12.275,12.264,12.2534,12.2429,12.2327,12.2228,12.213,12.2036,12.1944,12.1854,12.1766,12.3577,12.667,16.7608,16.7501,16.7398,16.7297,16.7201,16.7106,16.7015,16.6929,16.6844,16.9488,20.069,20.0619,20.0552,20.0489,20.043,20.0374,20.0323,20.0275,20.0231,20.0191,20.0155,20.0122,20.0094,20.0069,20.0048,20.0031,20.0018,20.0008,20.0002,20.0001,20.0002,20.0008,20.0018,20.0031,20.0048,20.0069,20.0094,20.0122,20.0155,20.0191,20.0231,20.0275,20.0323,20.0374,20.043,20.0489,20.0552,20.0619,20.069,20.0764,20.0843,20.0926,20.1012,20.1102,20.1196,20.1294,20.1397,20.1502,20.1612,20.1726,20.1844,20.1966,20.2092,20.2222,20.2356,20.2494,20.2636,20.2782,20.2932,20.3086,20.3244,20.3407,20.3573,20.3744,20.3919,20.4098,20.4281,20.4469,20.466,20.4856,20.5057,20.5261,20.547,20.5684,20.5901,20.6123,20.635,20.6581,20.6816,20.7056,20.73,20.7549,20.7802,20.806,20.8323,20.859,20.8862,20.9139,20.942,20.9706,20.9997,21.0293,21.0594,21.0899,21.1209,21.1525,21.1845,21.217,21.2501,21.2836,21.3177,21.3522,21.3873,21.423,21.4591,21.4958,21.533,21.5707,21.6091,21.6479,21.6873,21.7273,21.7678,21.8089,21.8505,21.8928,21.9356,21.979,22.023,22.0676,22.1128,22.1586,22.2051,22.2521,22.2998,22.3481,22.397,22.4466,22.4968,22.5477,22.5992,22.6515,22.7043,22.7579,22.8122,22.8671,22.9228,22.9792,23.0363,23.0941,23.1526,23.2119,23.2719,23.3327,23.3943,23.4566,23.5197,23.5836,23.6483,23.7138,23.7802,23.8473,23.9153,23.9842,24.0539,24.1244,24.1959,24.2682,24.3414,24.4156,24.4906,24.5666,24.6435,24.7214,24.8003,24.8801,24.9609,25.0428,25.1256,25.2095,25.2944,25.3804,25.4675,25.5556,25.6449,25.7353,25.8268,25.9194,26.0132,26.1082,26.2044,26.3018,26.4004,26.5003,26.6015,26.7039,26.8077,26.9127,27.0191,27.1269,27.2361,27.3466,27.4586,27.572,27.6869,27.8033,27.9213,28.0407,28.1617
\ No newline at end of file
diff --git a/scilab/test_ceres_odom_plot.sce b/scilab/test_ceres_odom_plot.sce
deleted file mode 100644
index 7ca80e229..000000000
--- a/scilab/test_ceres_odom_plot.sce
+++ /dev/null
@@ -1,80 +0,0 @@
-//plot log data from ceres test
-
-// clear all
-xdel(winsid());
-clear;
-
-// CERES ODOM BATCH
-//load log file
-data = read('~/Desktop/log_file_2.txt',-1,14);
-
-//plot
-fig1 = figure(0);
-fig1.background = 8;
-plot(data(2:$,10),data(2:$,11),"g.");
-plot(data(2:$,1),data(2:$,2),"b-");
-plot(data(2:$,4),data(2:$,5),"r-");
-plot(data(2:$,13),data(2:$,14),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_ODOM_BATCH - Time: ",string(data(1,1))," s"]));
-ah.title.font_size = 4;
-
-// MANAGER - THETA
-//load log file
-data2 = read('~/Desktop/log_file_2.txt',-1,15);
-data2L = read('~/Desktop/landmarks_file_2.txt',-1,2);
-
-disp(data2L);
-
-//plot
-fig2 = figure(1);
-fig2.background = 8;
-//plot(data2(2:$,13),data2(2:$,14),"g.");
-plot(data2(2:$,1),data2(2:$,2),"b-");
-plot(data2(2:$,4),data2(2:$,5),"r-");
-plot(data2(2:$,10),data2(2:$,11),"c--");
-
-plot(data2L(1:$,1),data2L(1:$,2),"k.");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$Optimization$";"$Ground\ Truth$";"$ODOM$";"$Landmarks$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_MANAGER: Theta - Time: ",string(data2(1,1))," s"]));
-ah.title.font_size = 4;
-
-// MANAGER - COMPLEX ANGLE
-//load log file
-data3 = read('~/Desktop/log_file_3.txt',-1,15);
-
-//plot
-fig3 = figure(2);
-fig3.background = 8;
-plot(data3(2:$,13),data3(2:$,14),"g.");
-plot(data3(2:$,1),data3(2:$,2),"b-");
-plot(data3(2:$,4),data3(2:$,5),"r-");
-plot(data3(2:$,10),data3(2:$,11),"c--");
-
-ah = gca();
-ah.auto_scale = "on";
-ah.x_label.text = "$x [m]$";
-ah.x_label.font_size = 4;
-ah.y_label.text = "$y [m]$";
-ah.y_label.font_size = 4;
-lh =legend(["$GPS$";"$Optimization$";"$Ground\ Truth$";"$ODOM$"],4);
-lh.font_size = 3;
-title(strcat(["CERES_MANAGER: Complex Angle - Time: ",string(data3(1,1))," s"]));
-ah.title.font_size = 4;
-
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 0208dd62c..fa3c402a9 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -33,50 +33,6 @@ namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
-// FrameBase::FrameBase(const TimeStamp& _ts,
-// const StateKeys& _frame_structure,
-// const VectorComposite& _state) :
-// NodeBase("FRAME", "FrameBase"),
-// HasStateBlocks(_frame_structure),
-// trajectory_ptr_(),
-// frame_id_(++frame_id_count_),
-// time_stamp_(_ts)
-// {
-// assert(_state.has(_frame_structure) && "_state does not include all keys of _frame_structure");
-
-// for (auto key : getKeys())
-// {
-// StateBlockPtr sb = FactoryStateBlock::create(std::string(1,key), _state.at(key), false); // false = non fixed
-// addStateBlock(key, sb, getProblem());
-// }
-// }
-
-
-// FrameBase::FrameBase(const TimeStamp& _ts,
-// const StateKeys& _frame_structure,
-// const std::list<VectorXd>& _vectors) :
-// NodeBase("FRAME", "FrameBase"),
-// HasStateBlocks(_frame_structure),
-// trajectory_ptr_(),
-// frame_id_(++frame_id_count_),
-// time_stamp_(_ts)
-// {
-// assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
-
-// auto vec_it = _vectors.begin();
-// for (const auto key : _frame_structure)
-// {
-// const auto& vec = *vec_it;
-
-// StateBlockPtr sb = FactoryStateBlock::create(std::string(1,key), vec, false); // false = non fixed
-
-// addStateBlock(key, sb, getProblem());
-
-// vec_it++;
-// }
-// }
-
-
FrameBase::FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
@@ -174,9 +130,9 @@ void FrameBase::link(ProblemPtr _prb)
this->link(_prb->getTrajectory());
}
-bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
+bool FrameBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
{
- return getProblem()->getFrameCovariance(shared_from_this(), _cov);
+ return getProblem()->getFrameCovariance(shared_from_this(), _keys, _cov);
}
FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index bd9a2fe5f..f17dc8769 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -100,9 +100,9 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
+bool LandmarkBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const
{
- return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
+ return getProblem()->getLandmarkCovariance(shared_from_this(), _keys, _cov);
}
YAML::Node LandmarkBase::toYaml() const
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c4fd5ef64..6fa2388c1 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -151,14 +151,14 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
WOLF_INFO("Applying schema");
if (not server.applySchema("Problem.schema"))
{
- WOLF_ERROR(server.getLog().str());
+ WOLF_ERROR(server.getLog());
return nullptr;
}
// validate against schema of specific dimension
bool is2D = server.getNode()["problem"]["dimension"].as<int>() == 2;
if (not server.applySchema(is2D ? "Problem2d.schema" : "Problem3d.schema"))
{
- WOLF_ERROR(server.getLog().str(), "\nNode:\n", server.getNode());
+ WOLF_ERROR(server.getLog(), "\nNode:\n", server.getNode());
return nullptr;
}
WOLF_INFO("schema applied");
@@ -1004,21 +1004,28 @@ bool Problem::getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _co
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const
+bool Problem::getFrameCovariance(FrameBaseConstPtr _frame, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
{
+ if (not _frame->hasKeys(_keys))
+ {
+ return false;
+ }
+
bool success(true);
// resizing
- SizeEigen sz = _frame_ptr->getLocalSize();
+ SizeEigen sz = _frame->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
- for (auto sb_i : _frame_ptr->getStateBlockVec())
+ for (auto key_i : _keys)
{
+ auto sb_i = _frame->getStateBlock(key_i);
j = 0;
- for (auto sb_j : _frame_ptr->getStateBlockVec())
+ for (auto key_j : _keys)
{
+ auto sb_j = _frame->getStateBlock(key_j);
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
}
@@ -1028,27 +1035,33 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd&
return success;
}
-bool Problem::getLastFrameCovariance(Eigen::MatrixXd& cov) const
+bool Problem::getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& cov) const
{
auto frm = getLastFrame();
- return getFrameCovariance(frm, cov);
+ return getFrameCovariance(frm, _keys, cov);
}
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const
+bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const
{
+ if (not _landmark->hasKeys(_keys))
+ {
+ return false;
+ }
bool success(true);
// resizing
- SizeEigen sz = _landmark_ptr->getLocalSize();
+ SizeEigen sz = _landmark->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
- for (auto sb_i : _landmark_ptr->getStateBlockVec())
+ for (auto key_i : _keys)
{
+ auto sb_i = _landmark->getStateBlock(key_i);
j = 0;
- for (auto sb_j : _landmark_ptr->getStateBlockVec())
+ for (auto key_j : _keys)
{
+ auto sb_j = _landmark->getStateBlock(key_j);
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d69eb4b4e..65dd33ac7 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -274,7 +274,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = getState(timestamp_from_callback);
for (auto pair_key_vec : proc_state)
- if (!keyframe_from_callback->isInStructure(pair_key_vec.first))
+ if (!keyframe_from_callback->hasKey(pair_key_vec.first))
keyframe_from_callback->addStateBlock(pair_key_vec.first,
FactoryStateBlock::create(std::string(1, pair_key_vec.first),
pair_key_vec.second,
@@ -365,7 +365,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = this->getState(timestamp_from_callback);
for (auto pair_key_vector : proc_state)
- if (!keyframe_from_callback->isInStructure(pair_key_vector.first))
+ if (!keyframe_from_callback->hasKey(pair_key_vector.first))
keyframe_from_callback->addStateBlock(pair_key_vector.first,
FactoryStateBlock::create(std::string(1, pair_key_vector.first),
pair_key_vector.second,
@@ -418,7 +418,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getFrame()->setTimeStamp( ts );
VectorComposite state_propa = getState( ts );
for (auto & pair_key_vec : state_propa)
- if (!last_ptr_->getFrame()->isInStructure(pair_key_vec.first))
+ if (!last_ptr_->getFrame()->hasKey(pair_key_vec.first))
last_ptr_->getFrame()->addStateBlock(pair_key_vec.first,
FactoryStateBlock::create(std::string(1, pair_key_vec.first),
pair_key_vec.second,
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index b7e3568c6..7a32e9908 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -111,7 +111,7 @@ StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBloc
"Trying to add a state block with an existing type! Use setStateBlock instead.");
state_block_map_.emplace(_sb_key, _sb);
- if (not isInStructure(_sb_key))
+ if (not hasKey(_sb_key))
appendToStructure(_sb_key);
// conditionally register to problem
diff --git a/src/utils/converter_utils.cpp b/src/utils/converter_utils.cpp
deleted file mode 100644
index f77008e1b..000000000
--- a/src/utils/converter_utils.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-//--------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/utils/converter_utils.h"
-
-// Eigen
-#include <array>
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/Geometry>
-
-// STD
-#include <iostream>
-#include <list>
-#include <stack>
-#include <vector>
-
-namespace utils {
-std::vector<std::string> splitter(std::string val) {
- std::vector<std::string> cont = std::vector<std::string>();
- std::stringstream ss(val);
- std::string token;
- while (std::getline(ss, token, ',')) {
- cont.push_back(token);
- }
- return cont;
-}
-std::vector<std::string> getMatches(std::string val, std::regex exp) {
- std::smatch res;
- auto v = std::vector<std::string>();
- std::string::const_iterator searchStart(val.cbegin());
- while (std::regex_search(searchStart, val.cend(), res, exp)) {
- v.push_back(res[0]);
- searchStart = res.suffix().first;
- }
- return v;
-}
-std::array<std::string, 2> splitMatrixStringRepresentation(std::string matrix) {
- std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:-?[0-9]*(?:\\.[0-9]+)?,?)+)\\]");
- std::regex rgxStatic("\\[((?:(?:-?[0-9]*)(?:\\.[0-9]+)?,?)+)\\]");
- std::smatch matches;
- std::smatch matchesStatic;
- std::array<std::string, 2> values = {{"[]", "[]"}};
- if (std::regex_match(matrix, matches, rgx)) {
- values[0] = "[" + matches[1].str() + "]";
- values[1] = "[" + matches[2].str() + "]";
- } else if (std::regex_match(matrix, matchesStatic, rgxStatic)) {
- values[1] = "[" + matchesStatic[1].str() + "]";
- } else {
- throw std::runtime_error(
- "Invalid string representation of a Matrix. Correct format is "
- "[([num,num],)?(num(,num)*)?]. String provided: " +
- matrix);
- }
- return values;
-}
-std::vector<std::string> pairSplitter(std::string val) {
- std::regex exp("\\{[^\\{:]:.*?\\}");
- return getMatches(val, exp);
-}
-std::string splitMapStringRepresentation(std::string str_map) {
- std::smatch mmatches;
- std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
- std::string result = "";
- if (std::regex_match(str_map, mmatches, rgxM)) {
- result = mmatches[1].str();
- } else {
- throw std::runtime_error(
- "Invalid string representation of a Map. Correct format is "
- "[({id:value})?(,{id:value})*]. String provided: " +
- str_map);
- }
- return result;
-}
-std::vector<std::string> parseList(std::string val) {
- std::stack<char> limiters;
- std::stack<std::string> word_stack;
- std::string current_word;
- std::vector<std::string> words;
- std::vector<char> chars(val.begin(), val.end());
- for (const char ¤t : chars) {
- if (current == '[') {
- limiters.push(current);
- word_stack.push(current_word);
- current_word = "";
- } else if (current == ']') {
- if (limiters.empty())
- throw std::runtime_error("Unmatched delimiter");
- if (limiters.top() == '[') {
- if (limiters.size() > 1) {
- if (word_stack.empty())
- word_stack.push("");
- current_word = word_stack.top() + "[" + current_word + "]";
- word_stack.pop();
- } else if (limiters.size() == 1 and current_word != "")
- words.push_back(current_word);
- else
- current_word += current;
- limiters.pop();
- } else
- throw std::runtime_error("Unmatched delimiter");
- } else if (current == '{') {
- limiters.push(current);
- word_stack.push(current_word);
- current_word = "";
- } else if (current == '}') {
- if (limiters.top() == '{') {
- if (limiters.size() > 1) {
- if (word_stack.empty())
- word_stack.push("");
- current_word = word_stack.top() + "{" + current_word + "}";
- word_stack.pop();
- } else if (limiters.size() == 1)
- words.push_back(current_word);
- else
- current_word += current;
- limiters.pop();
- } else
- throw std::runtime_error("Unmatched delimiter");
- } else if (current == ',') {
- if (limiters.size() == 1 and current_word != "") {
- words.push_back(current_word);
- current_word = "";
- } else if (limiters.size() > 1)
- current_word += current;
- } else {
- if (limiters.empty())
- throw std::runtime_error("Found non-delimited text");
- current_word += current;
- }
- }
- if (not limiters.empty())
- throw std::runtime_error("Unclosed delimiter [] or {}");
- return words;
-}
-} // namespace utils
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
deleted file mode 100644
index 7ed323ab0..000000000
--- a/src/utils/params_server.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-//--------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/utils/params_server.h"
-
-namespace wolf
-{
-
-ParamsServer::ParamsServer(const std::string& _global_prefix) :
- ParamsServer::ParamsServer(std::map<std::string, std::string>{}, _global_prefix)
-{
-}
-ParamsServer::ParamsServer(std::map<std::string, std::string> _params, const std::string& _global_prefix) :
- global_prefix_(_global_prefix)
-{
- if (not global_prefix_.empty())
- {
- if (global_prefix_.front() == '/')
- global_prefix_.erase(0,1);
- if (global_prefix_.back() != '/')
- global_prefix_.push_back('/');
- }
- addParams(_params);
-}
-
-void ParamsServer::print()
-{
- for (auto it : params_)
- std::cout << it.first << "~~" << it.second << std::endl;
-}
-
-void ParamsServer::addParam(std::string _key, std::string _value)
-{
- params_.insert(std::pair<std::string, std::string>(global_prefix_ + _key, _value));
-}
-
-void ParamsServer::addParams(std::map<std::string, std::string> _params)
-{
- for (auto param_pair : _params)
- addParam(param_pair.first, param_pair.second);
-}
-
-}
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
deleted file mode 100644
index 5a415ca23..000000000
--- a/src/yaml/parser_yaml.cpp
+++ /dev/null
@@ -1,594 +0,0 @@
-//--------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/yaml/parser_yaml.h"
-
-#include <string>
-#include <vector>
-#include <list>
-#include <stack>
-#include <regex>
-#include <map>
-#include <iostream>
-#include <algorithm>
-#include <numeric>
-
-using namespace wolf;
-namespace {
- //====== START OF FORWARD DECLARATION ========
- std::string parseAtomicNode(YAML::Node);
- std::string fetchMapEntry(YAML::Node);
- std::string mapToString(std::map<std::string,std::string>);
- //====== END OF FORWARD DECLARATION ========
-
-/** @Brief Interprets a map as being atomic and thus parses it as a single entity. We assume that the map has as values only scalars and sequences.
- * @param n the node representing a map
- * @return std::map<std::string, std::string> populated with the key,value pairs in n
- */
-std::map<std::string, std::string> fetchAsMap(YAML::Node _n){
- assert(_n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node");
- auto m = std::map<std::string, std::string>();
- for(const auto& kv : _n){
- std::string key = kv.first.as<std::string>();
- switch (kv.second.Type()) {
- case YAML::NodeType::Scalar : {
- std::string value = kv.second.Scalar();
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- case YAML::NodeType::Sequence : {
- std::string aux = parseAtomicNode(kv.second);
- m.insert(std::pair<std::string,std::string>(key, aux));
- break;
- }
- case YAML::NodeType::Map : {
- std::string value = fetchMapEntry(kv.second);
- std::regex r("^\\$.*");
- if (std::regex_match(key, r)) key = key.substr(1,key.size()-1);
- m.insert(std::pair<std::string,std::string>(key, value));
- break;
- }
- default:
- assert(1 == 0 && "Unsupported node Type at fetchAsMap");
- break;
- }
- }
- return m;
-}
- std::string fetchMapEntry(YAML::Node _n){
- switch (_n.Type()) {
- case YAML::NodeType::Scalar: {
- return _n.Scalar();
- break;
- }
- case YAML::NodeType::Sequence: {
- return parseAtomicNode(_n);
- break;
- }
- case YAML::NodeType::Map: {
- return mapToString(fetchAsMap(_n));
- break;
- }
- default: {
- assert(1 == 0 && "Unsupported node Type at fetchMapEntry");
- return "";
- break;
- }
- }
- }
- /** @Brief Transforms a std::map<std::string,std::string> to its std::string representation [{k1:v1},{k2:v2},{k3:v3},...]
- * @param map_ just a std::map<std::string,std::string>
- * @return <b>{std::string}</b> [{k1:v1},{k2:v2},{k3:v3},...]
- */
- std::string mapToString(std::map<std::string,std::string> _map){
- std::string result = "";
- auto v = std::vector<std::string>();
- std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<std::string,std::string> p){return "{" + p.first + ":" + p.second + "}";});
- auto concat = [](std::string ac, std::string str)-> std::string {
- return ac + str + ",";
- };
- std::string aux = "";
- std::string accumulated = std::accumulate(v.begin(), v.end(), aux, concat);
- if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1);
- else accumulated = "";
- return "[" + accumulated + "]";
- }
- /** @Brief Generates a std::string representing a YAML sequence. The sequence is assumed to be scalar or at most be a sequence of sequences of scalars.
- * @param n a vector of YAML::Node that represents a YAML::Sequence
- * @return <b>{std::string}</b> representing the YAML sequence
- */
- std::string parseAtomicNode(YAML::Node _n){
- std::string aux = "";
- bool first = true;
- std::string separator = "";
- switch(_n.Type()){
- case YAML::NodeType::Scalar:
- return _n.Scalar();
- break;
- case YAML::NodeType::Sequence:
- for(auto it : _n){
- aux += separator + parseAtomicNode(it);
- if(first){
- separator = ",";
- first = false;
- }
- }
- return "[" + aux + "]";
- break;
- case YAML::NodeType::Map:
- return mapToString(fetchAsMap(_n));
- break;
- default:
- return "";
- break;
- }
- }
-
- /** @Brief checks if a node of the YAML tree is atomic. Only works if the nodes are of type
- * Scalar, Sequence or Map.
- * @param key is the key associated to the node n if n.Type() == YAML::NodeType::Map
- * @param n node to be test for atomicity
- */
- bool isAtomic(std::string _key, YAML::Node _n){
- assert(_n.Type() != YAML::NodeType::Undefined && _n.Type() != YAML::NodeType::Null && "Cannot determine atomicity of Undefined/Null node");
- std::regex r("^\\$.*");
- bool is_atomic = true;
- switch(_n.Type()){
- case YAML::NodeType::Scalar:
- return true;
- break;
- case YAML::NodeType::Sequence:
- for(auto it : _n) {
- switch(it.Type()){
- case YAML::NodeType::Map:
- for(const auto& kv : it){
- is_atomic = is_atomic and isAtomic(kv.first.as<std::string>(), it);
- }
- break;
- default:
- is_atomic = is_atomic and isAtomic("", it);
- break;
- }
- }
- return is_atomic;
- break;
- case YAML::NodeType::Map:
- is_atomic = std::regex_match(_key, r);
- return is_atomic;
- break;
- default:
- throw std::runtime_error("Cannot determine atomicity of node type " + std::to_string(_n.Type()));
- return false;
- break;
- }
- return false;
- }
-}
-ParserYaml::ParserYaml(std::string _file, bool freely_parse)
-{
- params_ = std::map<std::string, std::string>();
- active_name_ = "";
- paramsSens_ = std::vector<ParamsInitSensor>();
- paramsProc_ = std::vector<ParamsInitProcessor>();
- subscriber_managers_ = std::vector<SubscriberManager>();
- publisher_managers_ = std::vector<PublisherManager>();
- parsing_file_ = std::stack<std::string>();
- file_ = _file;
-
- std::size_t found = _file.find_last_of("/");
- path_root_ = _file.substr(0,found+1);
- file_ = _file.substr(found+1);
-
- if (not freely_parse)
- parse();
- else
- parse_freely();
-}
-
-std::string ParserYaml::generatePath(std::string _file)
-{
- std::regex r("^/.*");
- if (std::regex_match(_file, r))
- {
- return _file;
- }
- else
- {
- return path_root_ + _file;
- }
-}
-YAML::Node ParserYaml::loadYaml(std::string _file)
-{
- try
- {
- // std::cout << "Parsing " << generatePath(_file) << std::endl;
- WOLF_INFO("Parsing file: ", generatePath(_file));
- return YAML::LoadFile(generatePath(_file));
- }
- catch (YAML::BadFile& e)
- {
- throw std::runtime_error("Couldn't load file " + generatePath(_file) + ". Tried to open it from " +
- parsing_file_.top());
- }
-}
-std::string ParserYaml::tagsToString(std::vector<std::string>& _tags)
-{
- std::string hdr = "";
- for (auto it : _tags)
- {
- hdr = hdr + "/" + it;
- }
- return hdr;
-}
-void ParserYaml::walkTree(std::string _file)
-{
- YAML::Node n;
- n = loadYaml(generatePath(_file));
- parsing_file_.push(generatePath(_file));
- std::vector<std::string> hdrs = std::vector<std::string>();
- walkTreeR(n, hdrs, "");
- parsing_file_.pop();
-}
-void ParserYaml::walkTree(std::string _file, std::vector<std::string>& _tags)
-{
- YAML::Node n;
- n = loadYaml(generatePath(_file));
- parsing_file_.push(generatePath(_file));
- walkTreeR(n, _tags, "");
- parsing_file_.pop();
-}
-void ParserYaml::walkTree(std::string _file, std::vector<std::string>& _tags, std::string hdr)
-{
- YAML::Node n;
- n = loadYaml(generatePath(_file));
- parsing_file_.push(generatePath(_file));
- walkTreeR(n, _tags, hdr);
- parsing_file_.pop();
-}
-void ParserYaml::walkTreeR(YAML::Node _n, std::vector<std::string>& _tags, std::string hdr)
-{
- switch (_n.Type())
- {
- case YAML::NodeType::Scalar:
- {
- std::regex r("^@.*");
- if (std::regex_match(_n.Scalar(), r))
- {
- std::string str = _n.Scalar();
- walkTree(str.substr(1, str.size() - 1), _tags, hdr);
- }
- else
- {
- insert_register(hdr, _n.Scalar());
- }
- break;
- }
- case YAML::NodeType::Sequence:
- {
- if (isAtomic("", _n))
- {
- insert_register(hdr, parseAtomicNode(_n));
- }
- else
- {
- for (const auto& kv : _n)
- {
- walkTreeR(kv, _tags, hdr);
- }
- }
- break;
- }
- case YAML::NodeType::Map:
- {
- for (const auto& kv : _n)
- {
- if (isAtomic(kv.first.as<std::string>(), _n))
- {
- std::string key = kv.first.as<std::string>();
- // WOLF_DEBUG("KEY IN MAP ATOMIC ", hdr + "/" + key);
- key = key.substr(1, key.size() - 1);
- insert_register(hdr + "/" + key, parseAtomicNode(kv.second));
- }
- else
- {
- /*
- If key=="follow" then the parser will assume that the value is a path and will parse
- the (expected) yaml file at the specified path. Note that this does not increase the header depth.
- The following example shows how the header remains unafected:
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - follow: "@some_path" |
- - var: 1.2 |
- Resulting map:
- cov_det -> 1
- my_value-> 23
- var: 1.2
- Instead of:
- cov_det -> 1
- follow/my_value-> 23
- var: 1.2
- Which would result from the following yaml files
- @my_main_config | @some_path
- - cov_det: 1 | - my_value : 23
- - $follow: "@some_path" |
- - var: 1.2 |
- */
- std::string key = kv.first.as<std::string>();
- // WOLF_DEBUG("KEY IN MAP NON ATOMIC ", key);
- std::regex rr("follow");
- if (not std::regex_match(kv.first.as<std::string>(), rr))
- {
- _tags.push_back(kv.first.as<std::string>());
- if (_tags.size() == 2)
- updateActiveName(kv.first.as<std::string>());
- walkTreeR(kv.second, _tags, hdr + "/" + kv.first.as<std::string>());
- _tags.pop_back();
- if (_tags.size() == 1)
- updateActiveName("");
- }
- else
- {
- walkTree(kv.second.as<std::string>(), _tags, hdr);
- }
- }
- }
- break;
- }
- case YAML::NodeType::Null:
- break;
- default:
- assert(1 == 0 && "Unsupported node Type at walkTreeR.");
- break;
- }
-}
-void ParserYaml::updateActiveName(std::string _tag)
-{
- active_name_ = _tag;
-}
-/*
-** @brief This function is responsible for parsing the first level of the YAML file.
-** The first level here can be thought as the entry point of the YAML file. Since we impose a certain structure
-** this function is responsible for enforcing said structure.
-**
- */
-void ParserYaml::parseFirstLevel(YAML::Node _n, std::string _file)
-{
-
- YAML::Node n_config = _n["config"];
- // assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node");
- if (n_config.Type() != YAML::NodeType::Map)
- throw std::runtime_error("Could not find config node. Please make sure that your YAML file " +
- generatePath(_file) + " starts with 'config:'");
- if (n_config["problem"].Type() != YAML::NodeType::Map)
- throw std::runtime_error("Could not find problem node. Please make sure that the 'config' node in YAML file " +
- generatePath(_file) + " has a 'problem' entry");
- problem = n_config["problem"];
- std::vector<std::map<std::string, std::string>> map_container;
- try
- {
- for (const auto& kv : n_config["sensors"])
- {
- ParamsInitSensor pSensor = { kv["type"].Scalar(), kv["name"].Scalar(), kv["plugin"].Scalar(), kv };
- paramsSens_.push_back(pSensor);
- map_container.push_back(std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
- { "name", kv["name"].Scalar() },
- { "plugin", kv["plugin"].Scalar() } }));
- }
- insert_register("sensors", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- }
- catch (YAML::InvalidNode& e)
- {
- throw std::runtime_error("Error parsing sensors @" + generatePath(_file) +
- ". Please make sure that each sensor entry has 'type', 'name' and 'plugin' entries.");
- }
-
- try
- {
- for (const auto& kv : n_config["processors"])
- {
- ParamsInitProcessor pProc = {
- kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor_name"].Scalar(), kv["plugin"].Scalar(), kv
- };
- paramsProc_.push_back(pProc);
- map_container.push_back(
- std::map<std::string, std::string>({ { "type", kv["type"].Scalar() },
- { "name", kv["name"].Scalar() },
- { "plugin", kv["plugin"].Scalar() },
- { "sensor_name", kv["sensor_name"].Scalar() } }));
- }
- insert_register("processors", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- }
- catch (YAML::InvalidNode& e)
- {
- throw std::runtime_error("Error parsing processors @" + generatePath(_file) +
- ". Please make sure that each processor has 'type', 'name', 'plugin' and "
- "'sensor_name' entries.");
- }
- try
- {
- for (const auto& kv : n_config["ROS subscriber"])
- {
- SubscriberManager pSubscriberManager = {
- kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["sensor_name"].Scalar(), kv
- };
- subscriber_managers_.push_back(pSubscriberManager);
- map_container.push_back(
- std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
- { "type", kv["type"].Scalar() },
- { "topic", kv["topic"].Scalar() },
- { "sensor_name", kv["sensor_name"].Scalar() } }));
- }
- insert_register("ROS subscriber", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- }
- catch (YAML::InvalidNode& e)
- {
- throw std::runtime_error("Error parsing subscriber @" + generatePath(_file) +
- ". Please make sure that each manager has 'package', 'type', 'topic' and "
- "'sensor_name' entries.");
- }
-
- try
- {
- for (const auto& kv : n_config["ROS publisher"])
- {
- PublisherManager pPublisherManager = {
- kv["package"].Scalar(), kv["type"].Scalar(), kv["topic"].Scalar(), kv["period"].Scalar(), kv
- };
- publisher_managers_.push_back(pPublisherManager);
- map_container.push_back(std::map<std::string, std::string>({ { "package", kv["package"].Scalar() },
- { "type", kv["type"].Scalar() },
- { "topic", kv["topic"].Scalar() },
- { "period", kv["period"].Scalar() } }));
- }
- insert_register("ROS publisher", wolf::converter<std::string>::convert(map_container));
- map_container.clear();
- }
- catch (YAML::InvalidNode& e)
- {
- throw std::runtime_error("Error parsing publisher @" + generatePath(_file) +
- ". Please make sure that each manager has 'package', 'type', 'topic' and 'period' entries.");
- }
-}
-std::map<std::string, std::string> ParserYaml::getParams()
-{
- std::map<std::string, std::string> rtn = params_;
- return rtn;
-}
-void ParserYaml::parse()
-{
- parsing_file_.push(generatePath(file_));
-
- YAML::Node n;
- n = loadYaml(generatePath(file_));
-
- parseFirstLevel(n, file_);
-
-
- if (problem.Type() != YAML::NodeType::Undefined)
- {
- std::vector<std::string> tags = std::vector<std::string>();
- walkTreeR(problem, tags, "problem");
- }
- for (auto it : paramsSens_)
- {
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("sensor");
- walkTreeR(it.n_, tags, "sensor/" + it.name_);
- }
- for (auto it : paramsProc_)
- {
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("processor");
- walkTreeR(it.n_, tags, "processor/" + it.name_);
- }
- for (auto it : subscriber_managers_)
- {
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("ROS subscriber");
- walkTreeR(it.n_, tags, "ROS subscriber/" + it.type_ + " - " + it.topic_);
- }
- for (auto it : publisher_managers_)
- {
- std::vector<std::string> tags = std::vector<std::string>();
- tags.push_back("ROS publisher");
- walkTreeR(it.n_, tags, "ROS publisher/" + it.type_ + " - " + it.topic_);
- }
- std::set<std::string> plugins, packages_subscriber, packages_publisher;
- for (auto pair : params_)
- if (pair.first.find("plugin") != std::string::npos and pair.first != "plugins")
- plugins.insert(pair.second);
- for (const auto& it : subscriber_managers_)
- packages_subscriber.insert(it.package_);
- for (const auto& it : publisher_managers_)
- packages_publisher.insert(it.package_);
- insert_register("plugins", wolf::converter<std::string>::convert(plugins));
- insert_register("packages_subscriber", wolf::converter<std::string>::convert(packages_subscriber));
- insert_register("packages_publisher", wolf::converter<std::string>::convert(packages_publisher));
-
- // YAML::Node n;
- // n = loadYaml(generatePath(file_));
-
- if (n.Type() == YAML::NodeType::Map)
- {
- for (auto it : n)
- {
- auto node = it.second;
- std::vector<std::string> tags = std::vector<std::string>();
- if (it.first.as<std::string>() != "config")
- walkTreeR(node, tags, it.first.as<std::string>());
- else
- {
- for (auto itt : node)
- {
- std::string node_key = itt.first.as<std::string>();
- if (node_key != "problem" and node_key != "sensors" and node_key != "processors" and
- node_key != "ROS subscriber" and node_key != "ROS publisher")
- {
- std::regex rr("follow");
- if (not std::regex_match(node_key, rr))
- {
- walkTreeR(itt.second, tags, node_key);
- }
- else
- {
- walkTree(itt.second.as<std::string>(), tags, "");
- }
- }
- }
- }
- }
- }
- else
- {
- std::vector<std::string> tags = std::vector<std::string>();
- walkTreeR(n, tags, "");
- }
- parsing_file_.pop();
-}
-/*
-** @brief This function gives the ability to run the parser without enforcing the wolf YAML structure.
- */
-void ParserYaml::parse_freely()
-{
- parsing_file_.push(generatePath(file_));
- std::vector<std::string> tags = std::vector<std::string>();
- walkTreeR(loadYaml(file_), tags, "");
- parsing_file_.pop();
-}
-void ParserYaml::insert_register(std::string _key, std::string _value)
-{
- if (_key.substr(0, 1) == "/")
- _key = _key.substr(1, _key.size() - 1);
- auto inserted_it = params_.insert(std::pair<std::string, std::string>(_key, _value));
- if (not inserted_it.second)
- WOLF_WARN("Skipping key '",
- _key,
- "' with value '",
- _value,
- "'. There already exists the register: (",
- inserted_it.first->first,
- ",",
- inserted_it.first->second,
- ")");
-}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a61ad8265..7299924ed 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -36,9 +36,6 @@ wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
-# # Converter from String to various types used by the parameters server
-# wolf_add_gtest(gtest_converter gtest_converter.cpp)
-
# FactorBase class test
wolf_add_gtest(gtest_factor_base gtest_factor_base.cpp)
@@ -80,12 +77,6 @@ wolf_add_gtest(gtest_logging gtest_logging.cpp)
# MotionBuffer class test
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
-# # Parameters server
-# # wolf_add_gtest(gtest_param_server gtest_param_server.cpp)
-
-# # YAML parser
-# # wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp)
-
# Problem class test
wolf_add_gtest(gtest_problem gtest_problem.cpp)
target_link_libraries(gtest_problem PUBLIC dummy)
@@ -189,54 +180,54 @@ wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model
# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
-# # ProcessorLoopClosure class test
-# wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
+# ProcessorLoopClosure class test
+wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
-# # ProcessorMotion in 2d
-# wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
+# ProcessorMotion in 2d
+wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
# SpecStateSensor
wolf_add_gtest(gtest_prior_sensor gtest_prior_sensor.cpp)
-# # ProcessorOdom3d class test
-# wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
+# ProcessorOdom3d class test
+wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
-# # FactorPose3dWithExtrinsics class test
-# wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+# FactorPose3dWithExtrinsics class test
+wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
-# # ProcessorTrackerFeatureDummy class test
-# wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
-# target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC dummy)
+# ProcessorTrackerFeatureDummy class test
+wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
+target_link_libraries(gtest_processor_tracker_feature_dummy PUBLIC dummy)
-# # ProcessorTrackerLandmarkDummy class test
-# wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
-# target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)
+# ProcessorTrackerLandmarkDummy class test
+wolf_add_gtest(gtest_processor_tracker_landmark_dummy gtest_processor_tracker_landmark_dummy.cpp)
+target_link_libraries(gtest_processor_tracker_landmark_dummy PUBLIC dummy)
# Schema test
wolf_add_gtest(gtest_schema gtest_schema.cpp)
-# # SensorDiffDriveSelfcalib class test
-# wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
+# SensorDiffDriveSelfcalib class test
+wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
-# # SensorOdom class test
-# wolf_add_gtest(gtest_sensor_odom gtest_sensor_odom.cpp)
+# SensorOdom class test
+wolf_add_gtest(gtest_sensor_odom gtest_sensor_odom.cpp)
-# # SensorPose class test
-# wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
+# SensorPose class test
+wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
-# IF (Ceres_FOUND)
-# # SolverCeres test
-# wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
+IF (Ceres_FOUND)
+ # SolverCeres test
+ wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
-# # SolverCeresMultithread test
-# wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
-# ENDIF(Ceres_FOUND)
+ # SolverCeresMultithread test
+ wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
+ENDIF(Ceres_FOUND)
-# # TreeManagerSlidingWindow class test
-# wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
+# TreeManagerSlidingWindow class test
+wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
-# # TreeManagerSlidingWindowDualRate class test
-# wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+# TreeManagerSlidingWindowDualRate class test
+wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
-# # yaml conversions
-# wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
+# yaml conversions
+wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index eaaaef6ff..9d46f7b2a 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -19,13 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file test_odom_2d.cpp
- *
- * Created on: Mar 15, 2016
- * \author: jsola
- */
-
#include "core/utils/utils_gtest.h"
// Classes under test
@@ -114,7 +107,7 @@ void show(const ProblemPtr& problem)
}
cout << " state: \n" << F->getStateVector("PO").transpose() << endl;
Eigen::MatrixXd cov;
- problem->getFrameCovariance(F,cov);
+ problem->getFrameCovariance(F, "PO", cov);
cout << " covariance: \n" << cov << endl;
}
}
@@ -138,8 +131,8 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
TimeStamp t0(0.0), t = t0;
double dt = .01;
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ VectorComposite x0("PO", Vector3d(0,0,0), {2,1});
+ VectorComposite s0("PO", Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), {2,1});
Vector3d delta (2,0,0);
Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
@@ -147,18 +140,21 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
SolverCeres solver(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0);
+ SpecComposite prior;
+ prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", s0.at('P')));
+ prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", s0.at('O')));
+ FrameBasePtr F0 = Pr->setPrior(prior, t0);
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(t, Vector3d::Zero());
+ FrameBasePtr F1 = Pr->emplaceFrame(t, "PO", Vector3d::Zero());
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "CaptureOdom2d", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "FeatureOdom2d", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorRelativePose2d>(f1, f1, F0, nullptr, false, TOP_MOTION);
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(t, Vector3d::Zero());
+ FrameBasePtr F2 = Pr->emplaceFrame(t, "PO", Vector3d::Zero());
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "CaptureOdom2d", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "FeatureOdom2d", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorRelativePose2d>(f2, f2, F1, nullptr, false, TOP_MOTION);
@@ -185,9 +181,9 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
// get covariances
MatrixXd P0_solver, P1_solver, P2_solver;
- ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver));
- ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
- ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F0, "PO", P0_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F1, "PO", P1_solver));
+ ASSERT_TRUE(Pr->getFrameCovariance(F2, "PO", P2_solver));
ASSERT_POSE2d_APPROX(F0->getStateVector("PO"), Vector3d(0,0,0), 1e-6);
auto s0_vector = s0.vector("PO");
@@ -208,9 +204,9 @@ TEST(Odom2d, VoteForKfAndSolve)
double dt = .01;
// Origin frame:
// Vector3d x0(0,0,0);
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
+ VectorComposite x0("PO", Vector3d(0,0,0), {2,1});
// Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
- VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ VectorComposite s0("PO", Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), {2,1});
// motion data
VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
@@ -218,7 +214,9 @@ TEST(Odom2d, VoteForKfAndSolve)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d",
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml",
+ {wolf_root});
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->voting_active = true;
params->dist_traveled = 100;
@@ -228,8 +226,7 @@ TEST(Odom2d, VoteForKfAndSolve)
params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.00;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
- ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(prc_base);
+ ProcessorOdom2dPtr processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, params);
// NOTE: We integrate and create KFs as follows:
@@ -240,7 +237,10 @@ TEST(Odom2d, VoteForKfAndSolve)
SolverCeres solver(problem);
// Origin Key Frame
- auto KF = problem->setPriorFactor(x0, s0, t);
+ SpecComposite prior;
+ prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", s0.at('P')));
+ prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", s0.at('O')));
+ FrameBasePtr KF = problem->setPrior(prior, t0);
processor_odom2d->setOrigin(KF);
solver.solve(SolverManager::ReportVerbosity::BRIEF);
@@ -329,7 +329,7 @@ TEST(Odom2d, VoteForKfAndSolve)
{
auto F = F_pair.second;
ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n] , 1e-6);
- ASSERT_TRUE (F->getCovariance(computed_cov));
+ ASSERT_TRUE (F->getCovariance("PO", computed_cov));
ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n] , 1e-6);
n += 3;
}
@@ -346,8 +346,8 @@ TEST(Odom2d, KF_callback)
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
- VectorComposite x0_cov(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
+ VectorComposite x0("PO", Vector3d(0,0,0), {2,1});
+ VectorComposite x0_cov("PO", Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), {2,1});
VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
@@ -362,7 +362,9 @@ TEST(Odom2d, KF_callback)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("SensorOdom2d",
+ wolf_root + "/test/yaml/sensor_odom_2d.yaml",
+ {wolf_root});
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->dist_traveled = 100;
params->angle_turned = data(1)*2.5; // force KF at every third process()
@@ -371,15 +373,17 @@ TEST(Odom2d, KF_callback)
params->time_tolerance = dt/2;
params->unmeasured_perturbation_std = 0.000001;
Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
- ProcessorBasePtr prc_base = problem->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, params);
- ProcessorOdom2dPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2d>(prc_base);
- processor_odom2d->setTimeTolerance(dt/2);
+ ProcessorOdom2dPtr processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, params);
+ // processor_odom2d->setTimeTolerance(dt/2);
// Ceres wrapper
SolverCeres solver(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0);
+ SpecComposite prior;
+ prior.emplace('P', SpecState("StatePoint2d", x0.at('P'), "factor", x0_cov.at('P')));
+ prior.emplace('O', SpecState("StateAngle", x0.at('O'), "factor", x0_cov.at('O')));
+ FrameBasePtr keyframe_0 = problem->setPrior(prior, t0);
processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
@@ -441,7 +445,7 @@ TEST(Odom2d, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
Vector3d x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, x_split);
+ FrameBasePtr keyframe_2 = problem->emplaceFrame(t_split, "PO", x_split);
// there must be 2KF and one F
ASSERT_EQ(problem->getTrajectory()->size(), 2);
@@ -466,7 +470,7 @@ TEST(Odom2d, KF_callback)
ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO") , integrated_pose_vector[n_split], 1e-6);
MatrixXd computed_cov;
- ASSERT_TRUE(problem->getLastFrameCovariance(computed_cov));
+ ASSERT_TRUE(problem->getLastFrameCovariance("PO", computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
@@ -478,7 +482,7 @@ TEST(Odom2d, KF_callback)
problem->print(4,1,1,1);
x_split = processor_odom2d->getState(t_split).vector("PO");
- FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
+ FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, "PO", x_split);
ASSERT_TRUE(problem->check(0));
processor_odom2d->keyFrameCallback(keyframe_1);
@@ -504,13 +508,13 @@ TEST(Odom2d, KF_callback)
// check the split KF
MatrixXd KF1_cov;
- ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, "PO", KF1_cov));
ASSERT_POSE2d_APPROX(keyframe_1->getStateVector("PO"), integrated_pose_vector[m_split], 1e-6);
ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
MatrixXd KF2_cov;
- ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
+ ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, "PO", KF2_cov));
ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO"), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
deleted file mode 100644
index 5cc0ca768..000000000
--- a/test/gtest_param_server.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-//--------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/utils/utils_gtest.h"
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.h"
-#include "core/utils/params_server.h"
-
-using namespace std;
-using namespace wolf;
-
-std::string wolf_root = _WOLF_ROOT_DIR;
-
-TEST(ParamsServer, Default)
-{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-}
-
-TEST(ParamsServer, getParamsOk)
-{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- EXPECT_EQ(server.getParam<int>("int_1"), -3);
- EXPECT_EQ(server.getParam<int>("int_2"), 0);
- EXPECT_EQ(server.getParam<int>("int_3"), 6);
-
- EXPECT_EQ(server.getParam<unsigned int>("uint_1"), 2);
- EXPECT_EQ(server.getParam<unsigned int>("uint_2"), 0);
- EXPECT_EQ(server.getParam<unsigned int>("uint_3"), 6);
-
- EXPECT_EQ(server.getParam<double>("double_1"), 3.6);
- EXPECT_EQ(server.getParam<double>("double_2"), -3);
- EXPECT_EQ(server.getParam<double>("double_3"), 3.6);
-
- EXPECT_EQ(server.getParam<std::string>("string_1"), std::string("wolf"));
- EXPECT_EQ(server.getParam<std::string>("string_2"), std::string("Wolf"));
-
- EXPECT_EQ(server.getParam<bool>("bool_1"), true);
- EXPECT_EQ(server.getParam<bool>("bool_2"), true);
- EXPECT_EQ(server.getParam<bool>("bool_3"), true);
- EXPECT_EQ(server.getParam<bool>("bool_4"), true);
- EXPECT_EQ(server.getParam<bool>("bool_5"), false);
- EXPECT_EQ(server.getParam<bool>("bool_6"), false);
- EXPECT_EQ(server.getParam<bool>("bool_7"), false);
- EXPECT_EQ(server.getParam<bool>("bool_8"), false);
-}
-
-TEST(ParamsServer, getParamsWrong)
-{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- EXPECT_ANY_THROW({ server.getParam<double>("should_not_exist"); });
-
- EXPECT_ANY_THROW({ server.getParam<int>("int_wrong_1"); });
- EXPECT_ANY_THROW({ server.getParam<int>("int_wrong_2"); });
- EXPECT_ANY_THROW({ server.getParam<int>("int_wrong_3"); });
-
- EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_1"); });
- EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_2"); });
- EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_3"); });
- EXPECT_ANY_THROW({ server.getParam<unsigned int>("uint_wrong_4"); });
-
- EXPECT_ANY_THROW({ server.getParam<double>("double_wrong_1"); });
- EXPECT_ANY_THROW({ server.getParam<double>("double_wrong_2"); });
-
- EXPECT_ANY_THROW({ server.getParam<bool>("bool_wrong_2"); });
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
deleted file mode 100644
index 1d97c2a13..000000000
--- a/test/gtest_parser_yaml.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-//--------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/utils/utils_gtest.h"
-#include "core/utils/converter.h"
-#include "core/common/wolf.h"
-#include "core/yaml/parser_yaml.h"
-
-using namespace std;
-using namespace wolf;
-
-std::string wolf_root = _WOLF_ROOT_DIR;
-std::string sensor_prefix = "sensor/";
-std::string processor_prefix = "processor/";
-
-TEST(ParserYaml, RegularParse)
-{
- auto parser = ParserYaml(wolf_root + "/test/yaml/params1.yaml");
- auto params = parser.getParams();
- // for(auto it : params)
- // cout << it.first << " %% " << it.second << endl;
- EXPECT_EQ(params[sensor_prefix + "odom/intrinsic/k_rot_to_rot"], "0.1");
- EXPECT_EQ(params[processor_prefix + "processor1/sensor_name"], "odom");
-}
-TEST(ParserYaml, ParseMap)
-{
- auto parser = ParserYaml(wolf_root + "/test/yaml/params2.yaml");
- auto params = parser.getParams();
- // for(auto it : params)
- // cout << it.first << " %% " << it.second << endl;
- EXPECT_EQ(params[processor_prefix + "processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]");
- // EXPECT_EQ(params["processor1/$mymap/k1"], "v1");
-}
-TEST(ParserYaml, FollowFile)
-{
- auto parser = ParserYaml(wolf_root + "/test/yaml/params1.yaml");
- auto params = parser.getParams();
- EXPECT_EQ(params[processor_prefix + "my_proc_test/max_buff_length"], "100");
- EXPECT_EQ(params[processor_prefix + "my_proc_test/voting_active"], "false");
-}
-TEST(ParserYaml, FollowOdom3d)
-{
- auto parser = ParserYaml(wolf_root + "/test/yaml/params1.yaml");
- auto params = parser.getParams();
- EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_buff_length"], "10");
- EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_time_span"], "0.2");
-}
-TEST(ParserYaml, JumpFile)
-{
- auto parser = ParserYaml(wolf_root + "/test/yaml/params3.yaml");
- auto params = parser.getParams();
- EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/max_buff_length"], "100");
- EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/voting_active"], "false");
-}
-TEST(ParserYaml, ProblemConfig)
-{
- auto parser = ParserYaml(wolf_root + "/test/yaml/params2.yaml");
- auto params = parser.getParams();
- EXPECT_EQ(params["problem/frame_structure"], "POV");
- EXPECT_EQ(params["problem/dimension"], "2");
-}
-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 2fbf543f0..23cf399b4 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -30,7 +30,6 @@
#include "dummy/processor_tracker_feature_dummy.h"
#include "core/solver/solver_manager.h"
#include "dummy/solver_manager_dummy.h"
-#include "core/yaml/parser_yaml.h"
#include "core/sensor/sensor_diff_drive.h"
#include "core/processor/processor_diff_drive.h"
#include "core/capture/capture_diff_drive.h"
@@ -414,7 +413,7 @@ TEST(Problem, Covariances)
// get covariance
Eigen::MatrixXd Cov;
- ASSERT_TRUE(P->getFrameCovariance(F, Cov));
+ ASSERT_TRUE(P->getFrameCovariance(F, "PO", Cov));
ASSERT_EQ(Cov.cols() , 6);
ASSERT_EQ(Cov.rows() , 6);
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index 08578a640..c7897da71 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_factor_pose_with_extrinsics.cpp
- *
- * Created on: Feb 19, 2020
- * \author: mfourmy
- */
#include "core/utils/utils_gtest.h"
#include "core/ceres_wrapper/solver_ceres.h"
@@ -36,11 +30,9 @@
#include "core/capture/capture_odom_3d.h"
#include "core/factor/factor_pose_3d_with_extrinsics.h"
-
using namespace Eigen;
using namespace wolf;
-
class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
{
/**
@@ -127,16 +119,17 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
// pose sensor and proc (to get extrinsics in the prb)
auto params_sp = std::make_shared<ParamsSensorPose>();
+ params_sp->name = "pose sensor";
params_sp->std_p = 1;
params_sp->std_o = 1;
-
- sensor_pose_ = problem_->installSensor("SensorPose", "pose", params_sp, SpecSensorComposite{{'P',SpecStateSensor("P",b_p_bm_)},
- {'O',SpecStateSensor("O",b_q_m_.coeffs())}});
+ sensor_pose_ = SensorBase::emplace<SensorPose3d>(problem_->getHardware(),
+ params_sp,
+ SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",b_p_bm_,"initial_guess")},
+ {'O',SpecStateSensor("StateQuaternion",b_q_m_.coeffs(),"initial_guess")}});
auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->name = "pose processor";
params_proc->time_tolerance = 0.5;
- auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
- // somehow by default the sensor extrinsics is fixed
- sensor_pose_->unfixExtrinsics();
+ auto proc_pose = ProcessorBase::emplace<ProcessorPose>(sensor_pose_, params_proc);
}
void TearDown() override{};
@@ -150,9 +143,9 @@ class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsB
{
FactorPose3dWithExtrinsicsBase_Test::SetUp();
// Two frames
- KF1_ = problem_->emplaceFrame(1, pose1_);
- KF2_ = problem_->emplaceFrame(2, pose2_);
- KF3_ = problem_->emplaceFrame(3, pose3_);
+ KF1_ = problem_->emplaceFrame(1, "PO", pose1_);
+ KF2_ = problem_->emplaceFrame(2, "PO", pose2_);
+ KF3_ = problem_->emplaceFrame(3, "PO", pose3_);
///////////////////
// Create factor graph
@@ -194,9 +187,9 @@ class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : publi
{
FactorPose3dWithExtrinsicsBase_Test::SetUp();
// Two frames
- KF1_ = problem_->emplaceFrame(1, pose1_);
- KF2_ = problem_->emplaceFrame(2, pose2_);
- KF3_ = problem_->emplaceFrame(3, pose3_);
+ KF1_ = problem_->emplaceFrame(1, "PO", pose1_);
+ KF2_ = problem_->emplaceFrame(2, "PO", pose2_);
+ KF3_ = problem_->emplaceFrame(3, "PO", pose3_);
problem_->keyFrameCallback(KF1_, nullptr);
problem_->keyFrameCallback(KF2_, nullptr);
problem_->keyFrameCallback(KF3_, nullptr);
@@ -235,9 +228,9 @@ class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorP
{
FactorPose3dWithExtrinsicsBase_Test::SetUp();
// Two frames
- KF1_ = problem_->emplaceFrame(1, pose1_);
- KF2_ = problem_->emplaceFrame(2, pose2_);
- KF3_ = problem_->emplaceFrame(3, pose3_);
+ KF1_ = problem_->emplaceFrame(1, "PO", pose1_);
+ KF2_ = problem_->emplaceFrame(2, "PO", pose2_);
+ KF3_ = problem_->emplaceFrame(3, "PO", pose3_);
Matrix6d cov6d = sensor_pose_->computeNoiseCov(VectorXd(0));
///////////////////
@@ -295,11 +288,10 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
- SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
ASSERT_POSE3d_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
ASSERT_POSE3d_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_QUATERNION_VECTOR_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
@@ -313,11 +305,10 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
- SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
ASSERT_POSE3d_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
ASSERT_POSE3d_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_QUATERNION_VECTOR_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
@@ -332,11 +323,10 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve)
problem_->perturb();
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
- SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
ASSERT_POSE3d_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
ASSERT_POSE3d_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_QUATERNION_VECTOR_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_MATRIX_APPROX(sensor_pose_->getP()->getState(), b_p_bm_, 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sensor_pose_->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 0712b09ba..47cc6069c 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/**
- * \file gtest_processor_diff_drive.cpp
- *
- * Created on: Jul 22, 2019
- * \author: jsola
- */
#include "core/processor/processor_diff_drive.h"
#include "core/sensor/sensor_diff_drive.h"
@@ -145,9 +139,9 @@ class ProcessorDiffDriveTest : public testing::Test
params_sen->name = "cool sensor";
params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, // default "fix", not dynamic
- {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}, // default "fix", not dynamic
- {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!!
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, // default "fix", not dynamic
+ {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}, // default "fix", not dynamic
+ {'I',SpecStateSensor("StateParams3", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!!
sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), params_sen, priors);
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 4baf6e8b5..451824fc1 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -49,7 +49,7 @@ class ProcessorLoopClosureTest : public testing::Test
virtual void SetUp()
{
// install sensor
- sensor = problem->installSensor("SensorOdom","my_sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ sensor = problem->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
// install processor
ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
@@ -60,7 +60,7 @@ class ProcessorLoopClosureTest : public testing::Test
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
{
// new frame
- return problem->emplaceFrame(ts, x);
+ return problem->emplaceFrame(ts, "PO", x);
}
CaptureBasePtr emplaceCapture(FrameBasePtr frame)
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 7f7c15e7f..63bd82917 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -105,7 +105,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ sensor = problem->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
// Install processor
params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 38d865ffa..b53b15b33 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -126,7 +126,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ sensor = problem->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
// Install processor
params = std::make_shared<ParamsProcessorTrackerLandmarkDummy>();
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index b8cbc3bc0..b0a8e731b 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -393,7 +393,7 @@ TEST(SensorBase, factory)
WOLF_INFO("Creating sensor from ", name, ".yaml");
auto valid = server.applySchema("SensorDummy"+toString(dim)+"d");
- WOLF_WARN_COND(not valid and not wrong, server.getLog().str());
+ WOLF_WARN_COND(not valid and not wrong, server.getLog());
WOLF_WARN_COND(valid and wrong, "Schema didn't detect any problem in a wrong yaml.");
// CORRECT YAML
@@ -441,7 +441,7 @@ TEST(SensorBase, factory)
YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml");
auto valid = server.applySchema("SensorDummyPoia3d");
- WOLF_WARN_COND(not valid, server.getLog().str());
+ WOLF_WARN_COND(not valid, server.getLog());
ASSERT_TRUE(valid);
// create sensor
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 1ab3e464f..72e91fee2 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -45,28 +45,11 @@ TEST(SensorDiffDrive, constructor_priors)
{
auto param = std::make_shared<ParamsSensorDiffDrive>();
- SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
- {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateAngle", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateParams3", i_state)}}; //default "fix", not dynamic
- auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
-}
-
-TEST(SensorDiffDrive, constructor_server)
-{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_diff_drive.yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
- auto params = std::make_shared<ParamsSensorDiffDrive>("sensor_1", server);
- auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, params, server);
+ auto sen = std::make_shared<SensorDiffDrive>(param, priors);
ASSERT_NE(sen, nullptr);
@@ -79,25 +62,13 @@ TEST(SensorDiffDrive, constructor_server)
TEST(SensorDiffDrive, factory)
{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_diff_drive.yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+ yaml_schema_cpp::YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_diff_drive.yaml");
- auto sb = FactorySensor::create("SensorDiffDrive","sensor_1", 2, server);
+ auto valid = server.applySchema("SensorDiffDrive");
+ WOLF_WARN_COND(not valid, server.getLog());
+ ASSERT_TRUE(valid);
- SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
-}
-
-TEST(SensorDiffDrive, factory_yaml)
-{
- auto sb = FactorySensorYaml::create("SensorDiffDrive","sensor_1", 2, wolf_root + "/test/yaml/sensor_diff_drive.yaml");
+ auto sb = FactorySensor::create("SensorDiffDrive", server.getNode());
SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
@@ -110,15 +81,9 @@ TEST(SensorDiffDrive, factory_yaml)
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS);
}
-TEST(SensorDiffDrive, factory_priors)
+TEST(SensorDiffDrive, factory_yaml)
{
- auto param = std::make_shared<ParamsSensorDiffDrive>();
-
- SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
- {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
-
- auto sb = FactorySensorPriors::create("SensorDiffDrive","sensor_1", 2, param, priors);
+ auto sb = FactorySensorYaml::create("SensorDiffDrive", "SensorDiffDrive", wolf_root + "/test/yaml/sensor_diff_drive.yaml", {wolf_root});
SensorDiffDrivePtr sen = std::dynamic_pointer_cast<SensorDiffDrive>(sb);
@@ -137,11 +102,11 @@ TEST(SensorDiffDrive, getParams)
param->ticks_per_wheel_revolution = 400;
param->ticks_std_factor = 2;
- SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
- {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateAngle", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateParams3", i_state)}}; //default "fix", not dynamic
- auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
+ auto sen = std::make_shared<SensorDiffDrive>(param, priors);
ASSERT_NE(sen->getParams(), nullptr);
@@ -156,11 +121,11 @@ TEST(SensorDiffDrive, computeNoiseCov)
param->ticks_per_wheel_revolution = 400;
param->ticks_std_factor = 2;
- SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state)}, //default "fix", not dynamic
- {'I',SpecStateSensor("StateBlock", i_state)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateAngle", o_state)}, //default "fix", not dynamic
+ {'I',SpecStateSensor("StateParams3", i_state)}}; //default "fix", not dynamic
- auto sen = std::make_shared<SensorDiffDrive>("sensor_1", 2, param, priors);
+ auto sen = std::make_shared<SensorDiffDrive>(param, priors);
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(Eigen::Vector2d::Zero()), Eigen::Matrix2d::Zero(), Constants::EPS);
ASSERT_MATRIX_APPROX(sen->computeNoiseCov(Eigen::Vector2d::Ones()), Eigen::Matrix2d::Identity() * 4, Constants::EPS);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 54d9a21b2..e7e79f86c 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -23,10 +23,10 @@
#include "core/sensor/sensor_odom.h"
#include "core/sensor/factory_sensor.h"
#include "core/utils/utils_gtest.h"
-#include "core/yaml/parser_yaml.h"
using namespace wolf;
using namespace Eigen;
+using namespace yaml_schema_cpp;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -103,15 +103,16 @@ void checkSensor(SensorBasePtr S,
TEST(SensorOdom, makeshared_priors_fix_2D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_2D)}}));
+ auto S = std::make_shared<SensorOdom2d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateAngle", o_state_2D)}}));
// checks
checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
@@ -122,15 +123,16 @@ TEST(SensorOdom, makeshared_priors_fix_2D)
TEST(SensorOdom, makeshared_priors_fix_3D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_3D)}}));
+ auto S = std::make_shared<SensorOdom3d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -144,15 +146,16 @@ TEST(SensorOdom, makeshared_priors_fix_3D)
TEST(SensorOdom, makeshared_priors_initial_guess_2D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess")},
- {'O',SpecStateSensor("O", o_state_2D, "initial_guess")}}));
+ auto S = std::make_shared<SensorOdom2d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess")},
+ {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess")}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -166,15 +169,16 @@ TEST(SensorOdom, makeshared_priors_initial_guess_2D)
TEST(SensorOdom, makeshared_priors_initial_guess_3D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess")},
- {'O',SpecStateSensor("O", o_state_3D, "initial_guess")}}));
+ auto S = std::make_shared<SensorOdom3d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess")},
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess")}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -188,15 +192,16 @@ TEST(SensorOdom, makeshared_priors_initial_guess_3D)
TEST(SensorOdom, makeshared_priors_factor_2D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "factor", p_std_2D)},
- {'O',SpecStateSensor("O", o_state_2D, "factor", o_std_2D)}}));
+ auto S = std::make_shared<SensorOdom2d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "factor", p_std_2D)},
+ {'O',SpecStateSensor("StateAngle", o_state_2D, "factor", o_std_2D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -210,15 +215,16 @@ TEST(SensorOdom, makeshared_priors_factor_2D)
TEST(SensorOdom, makeshared_priors_factor_3D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "factor", p_std_3D)},
- {'O',SpecStateSensor("O", o_state_3D, "factor", o_std_3D)}}));
+ auto S = std::make_shared<SensorOdom3d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "factor", p_std_3D)},
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
@@ -232,15 +238,16 @@ TEST(SensorOdom, makeshared_priors_factor_3D)
TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_2D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess", vector0, true)},
- {'O',SpecStateSensor("O", o_state_2D, "initial_guess", vector0, true)}}));
+ auto S = std::make_shared<SensorOdom2d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true)},
+ {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -254,15 +261,16 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_2D)
TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_3D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess", vector0, true)},
- {'O',SpecStateSensor("O", o_state_3D, "initial_guess", vector0, true)}}));
+ auto S = std::make_shared<SensorOdom3d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true)},
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -276,15 +284,16 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_3D)
TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_2D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 2, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
- {'O',SpecStateSensor("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
+ auto S = std::make_shared<SensorOdom2d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint2d", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
+ {'O',SpecStateSensor("StateAngle", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -298,15 +307,16 @@ TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_2D)
TEST(SensorOdom, makeshared_priors_initial_guess_dynamic_drift_3D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = std::make_shared<SensorOdom>("sensor1", 3, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
- {'O',SpecStateSensor("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
+ auto S = std::make_shared<SensorOdom3d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
@@ -350,16 +360,19 @@ TEST(SensorOdom, factory)
(dynamic ? "_dynamic" : "") +
(drift ? "_drift" : "") +
(wrong ? "_wrong" : "");
- // Yaml parser
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+ // Yaml server
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml");
WOLF_INFO("Creating sensor from ", name, ".yaml");
+ auto valid = server.applySchema("SensorOdom"+toString(dim)+"d");
+ WOLF_WARN_COND(not valid and not wrong, server.getLog());
+ WOLF_WARN_COND(valid and wrong, "Schema didn't detect any problem in a wrong yaml.");
+
// CORRECT YAML
if (not wrong)
{
- auto S = FactorySensor::create("SensorOdom", "sensor_1", dim, server);
+ auto S = FactorySensor::create("SensorOdom"+toString(dim)+"d", server.getNode());
auto p_size = dim;
auto o_size = dim == 2 ? 1 : 4;
@@ -378,7 +391,7 @@ TEST(SensorOdom, factory)
// INCORRECT YAML
else
{
- ASSERT_THROW(FactorySensor::create("SensorOdom", "sensor_1", dim, server),std::runtime_error);
+ ASSERT_THROW(FactorySensor::create("SensorOdom"+toString(dim)+"d", server.getNode()),std::runtime_error);
}
}
}
@@ -426,7 +439,7 @@ TEST(SensorOdom, factory_yaml)
// CORRECT YAML
if (not wrong)
{
- auto S = FactorySensorYaml::create("SensorOdom", "sensor_1", dim, yaml_filepath);
+ auto S = FactorySensorYaml::create("SensorOdom"+toString(dim)+"d", "SensorOdom"+toString(dim)+"d", yaml_filepath, {wolf_root});
auto p_size = dim;
auto o_size = dim == 2 ? 1 : 4;
@@ -445,7 +458,7 @@ TEST(SensorOdom, factory_yaml)
// INCORRECT YAML
else
{
- ASSERT_THROW(FactorySensorYaml::create("SensorOdom", "sensor_1", dim, yaml_filepath),std::runtime_error);
+ ASSERT_THROW(FactorySensorYaml::create("SensorOdom"+toString(dim)+"d", "SensorOdom"+toString(dim)+"d", yaml_filepath, {wolf_root}),std::runtime_error);
}
}
}
@@ -456,15 +469,16 @@ TEST(SensorOdom, factory_yaml)
TEST(SensorOdom, compute_noise_cov_3D)
{
auto params = std::make_shared<ParamsSensorOdom>();
+ params->name = "sensor_1";
params->k_disp_to_disp = k_disp_to_disp;
params->k_disp_to_rot = k_disp_to_rot;
params->k_rot_to_rot = k_rot_to_rot;
params->min_disp_var = min_disp_var;
params->min_rot_var = min_rot_var;
- auto S = FactorySensorPriors::create("SensorOdom","sensor1", 3, params,
- SpecSensorComposite({{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_3D)}}));
+ auto S = std::make_shared<SensorOdom3d>(params,
+ SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D)}}));
Vector6d disp_elements, rot_elements;
disp_elements << 1, 1, 1, 0, 0, 0;
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 4f905a016..e2d17acbc 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -34,6 +34,7 @@
using namespace wolf;
using namespace Eigen;
+using namespace yaml_schema_cpp;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -49,13 +50,14 @@ Matrix6d noise_cov_3D = (Vector6d() << std_p, std_p, std_p, std_o, std_o, std_o)
TEST(Pose, constructor_priors_2D)
{
auto param = std::make_shared<ParamsSensorPose>();
+ param->name = "a not so cool sensor";
param->std_p = std_p;
param->std_o = std_o;
- SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_2D)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", p_state_2D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateAngle", o_state_2D)}}; //default "fix", not dynamic
- auto sen = std::make_shared<SensorPose>("sensor_1", 2, param, priors);
+ auto sen = std::make_shared<SensorPose2d>(param, priors);
ASSERT_NE(sen, nullptr);
@@ -74,57 +76,14 @@ TEST(Pose, constructor_priors_2D)
TEST(Pose, constructor_priors_3D)
{
auto param = std::make_shared<ParamsSensorPose>();
+ param->name = "a not so cool sensor";
param->std_p = std_p;
param->std_o = std_o;
- SpecComposite priors{{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_3D)}}; //default "fix", not dynamic
+ SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d", p_state_3D)}, //default "fix", not dynamic
+ {'O',SpecStateSensor("StateQuaternion", o_state_3D)}}; //default "fix", not dynamic
- auto sen = std::make_shared<SensorPose>("sensor_1", 3, param, priors);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
-
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
-}
-
-TEST(Pose, constructor_server_2D)
-{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_2d.yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
- auto params = std::make_shared<ParamsSensorPose>("sensor_1", server);
- auto sen = std::make_shared<SensorPose>("sensor_1", 2, params, server);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
-
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
-}
-
-TEST(Pose, constructor_server_3D)
-{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_3d.yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
-
- auto params = std::make_shared<ParamsSensorPose>("sensor_1", server);
- auto sen = std::make_shared<SensorPose>("sensor_1", 3, params, server);
+ auto sen = std::make_shared<SensorPose3d>(param, priors);
ASSERT_NE(sen, nullptr);
@@ -142,12 +101,13 @@ TEST(Pose, constructor_server_3D)
TEST(Pose, factory_2D)
{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_2d.yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_pose_2d.yaml");
+
+ ASSERT_TRUE(server.applySchema("SensorPose2d"));
- auto sb = FactorySensor::create("SensorPose","sensor_1", 2, server);
+ auto sb = FactorySensor::create("SensorPose2d",server.getNode());
- SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+ SensorPose2dPtr sen = std::dynamic_pointer_cast<SensorPose2d>(sb);
ASSERT_NE(sen, nullptr);
@@ -165,12 +125,13 @@ TEST(Pose, factory_2D)
TEST(Pose, factory_3D)
{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_3d.yaml", true);
- ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_pose_3d.yaml");
- auto sb = FactorySensor::create("SensorPose","sensor_1", 3, server);
+ ASSERT_TRUE(server.applySchema("SensorPose3d"));
- SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+ auto sb = FactorySensor::create("SensorPose3d",server.getNode());
+
+ SensorPose3dPtr sen = std::dynamic_pointer_cast<SensorPose3d>(sb);
ASSERT_NE(sen, nullptr);
@@ -188,9 +149,9 @@ TEST(Pose, factory_3D)
TEST(Pose, factory_yaml_2D)
{
- auto sb = FactorySensorYaml::create("SensorPose","sensor_1", 2, wolf_root + "/test/yaml/sensor_pose_2d.yaml");
+ auto sb = FactorySensorYaml::create("SensorPose2d", "SensorPose2d", wolf_root + "/test/yaml/sensor_pose_2d.yaml", {wolf_root});
- SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+ SensorPose2dPtr sen = std::dynamic_pointer_cast<SensorPose2d>(sb);
ASSERT_NE(sen, nullptr);
@@ -208,67 +169,15 @@ TEST(Pose, factory_yaml_2D)
TEST(Pose, factory_yaml_3D)
{
- auto sb = FactorySensorYaml::create("SensorPose","sensor_1", 3, wolf_root + "/test/yaml/sensor_pose_3d.yaml");
-
- SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
-
- ASSERT_NE(sen, nullptr);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
-
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_3D, Constants::EPS)
-}
-
-TEST(Pose, factory_priors_2D)
-{
- auto param = std::make_shared<ParamsSensorPose>();
- param->std_p = std_p;
- param->std_o = std_o;
-
- SpecComposite priors{{'P',SpecStateSensor("P", p_state_2D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_2D)}}; //default "fix", not dynamic
+ auto sb = FactorySensorYaml::create("SensorPose3d", "SensorPose3d", wolf_root + "/test/yaml/sensor_pose_3d.yaml", {wolf_root});
- auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 2, param, priors);
-
- SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
+ SensorPose3dPtr sen = std::dynamic_pointer_cast<SensorPose3d>(sb);
ASSERT_NE(sen, nullptr);
ASSERT_TRUE(sen->getP()->isFixed());
ASSERT_TRUE(sen->getO()->isFixed());
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_2D, Constants::EPS);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_2D, Constants::EPS);
-
- ASSERT_EQ(sen->getStdP(), std_p);
- ASSERT_EQ(sen->getStdO(), std_o);
-
- ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov_2D, Constants::EPS)
-}
-
-TEST(Pose, factory_priors_3D)
-{
- auto param = std::make_shared<ParamsSensorPose>();
- param->std_p = std_p;
- param->std_o = std_o;
-
- SpecComposite priors{{'P',SpecStateSensor("P", p_state_3D)}, //default "fix", not dynamic
- {'O',SpecStateSensor("O", o_state_3D)}}; //default "fix", not dynamic
-
- auto sb = FactorySensorPriors::create("SensorPose","sensor_1", 3, param, priors);
-
- SensorPosePtr sen = std::dynamic_pointer_cast<SensorPose>(sb);
-
- ASSERT_TRUE(sen->getP()->isFixed());
- ASSERT_TRUE(sen->getO()->isFixed());
-
ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state_3D, Constants::EPS);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index f5d99215c..d9d6a1386 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -51,7 +51,7 @@ TEST(TreeManager, createNode)
auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root},
wolf_root + "/test/yaml/tree_manager_dummy.yaml");
- WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog().str());
+ WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog());
ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy"));
auto GM = TreeManagerDummy::create(yaml_server.getNode());
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index fd252e800..9a75da95d 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -25,13 +25,13 @@
#include "core/problem/problem.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_sliding_window.h"
-#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
#include "core/feature/feature_base.h"
#include "core/factor/factor_pose_3d.h"
using namespace wolf;
using namespace Eigen;
+using namespace yaml_schema_cpp;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -48,30 +48,15 @@ TEST(TreeManagerSlidingWindow, make_shared)
EXPECT_EQ(P->getTreeManager(), GM);
}
-TEST(TreeManagerSlidingWindow, createParams)
+TEST(TreeManagerSlidingWindow, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
-
- auto GM = TreeManagerSlidingWindow::create(ParamsGM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
-
- P->setTreeManager(GM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
-TEST(TreeManagerSlidingWindow, createParamServer)
-{
- ProblemPtr P = Problem::create("PO", 2);
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml");
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
+ ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow"));
- auto GM = TreeManagerSlidingWindow::create(server);
+ auto GM = TreeManagerSlidingWindow::create(server.getNode());
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
@@ -85,7 +70,7 @@ TEST(TreeManagerSlidingWindow, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerSlidingWindow::create(wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml");
+ auto GM = TreeManagerSlidingWindow::create(wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml", {wolf_root});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
@@ -99,40 +84,27 @@ TEST(TreeManager, Factory)
{
ProblemPtr P = Problem::create("PO", 2);
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
-
- auto GM = FactoryTreeManager::create("TreeManagerSlidingWindow",ParamsGM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
-
- P->setTreeManager(GM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- ASSERT_EQ(P->getTreeManager(), GM);
-}
-
-TEST(TreeManager, FactoryParamServer)
-{
- ProblemPtr P = Problem::create("PO", 2);
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml");
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
+ ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindow"));
- auto GM = FactoryTreeManagerServer::create("TreeManagerSlidingWindow", server);
+ auto GM = FactoryTreeManager::create("TreeManagerSlidingWindow",server.getNode());
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
P->setTreeManager(GM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
+ ASSERT_EQ(P->getTreeManager(), GM);
}
TEST(TreeManager, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindow", wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml");
+ auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindow",
+ wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml",
+ {wolf_root});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
@@ -144,10 +116,7 @@ TEST(TreeManager, FactoryYaml)
TEST(TreeManagerSlidingWindow, autoConf)
{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- ProblemPtr P = Problem::autoSetup(server);
+ ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml", {wolf_root});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
}
@@ -156,11 +125,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
{
// window size: 3
// first 2 frames fixed
-
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- ProblemPtr P = Problem::autoSetup(server);
+ ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml", {wolf_root});
P->applyPriorOptions(0);
// FRAME 1 ----------------------------------------------------------
@@ -172,7 +137,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", state);
P->keyFrameCallback(F2, nullptr);
// absolute factor
@@ -191,7 +156,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
EXPECT_FALSE(F2->isFixed());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", state);
P->keyFrameCallback(F3, nullptr);
// absolute factor
@@ -211,7 +176,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
EXPECT_FALSE(F3->isFixed());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", state);
P->keyFrameCallback(F4, nullptr);
// absolute factor
@@ -234,7 +199,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
EXPECT_FALSE(F4->isFixed());
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", state);
P->keyFrameCallback(F5, nullptr);
// absolute factor
@@ -266,10 +231,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window2.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- ProblemPtr P = Problem::autoSetup(server);
+ ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window2.yaml", {wolf_root});
P->applyPriorOptions(0);
// FRAME 1 (prior) ----------------------------------------------------------
@@ -281,7 +243,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", state);
P->keyFrameCallback(F2, nullptr);
// absolute factor
@@ -300,7 +262,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
EXPECT_FALSE(F2->isFixed());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", state);
P->keyFrameCallback(F3, nullptr);
// absolute factor
@@ -320,7 +282,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
EXPECT_FALSE(F3->isFixed());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", state);
P->keyFrameCallback(F4, nullptr);
// absolute factor
@@ -343,7 +305,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
EXPECT_FALSE(F4->isFixed());
// FRAME 5 ----------------------------------------------------------
- auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", state);
P->keyFrameCallback(F5, nullptr);
// absolute factor
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index f994993b4..6297ab189 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -25,7 +25,6 @@
#include "core/problem/problem.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
-#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
#include "core/capture/capture_odom_3d.h"
#include "core/feature/feature_base.h"
@@ -34,6 +33,7 @@
using namespace wolf;
using namespace Eigen;
+using namespace yaml_schema_cpp;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -50,30 +50,15 @@ TEST(TreeManagerSlidingWindowDualRate, make_shared)
EXPECT_EQ(P->getTreeManager(), GM);
}
-TEST(TreeManagerSlidingWindowDualRate, createParams)
+TEST(TreeManagerSlidingWindowDualRate, createNode)
{
ProblemPtr P = Problem::create("PO", 2);
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
-
- auto GM = TreeManagerSlidingWindowDualRate::create(ParamsGM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
-
- P->setTreeManager(GM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
-TEST(TreeManagerSlidingWindowDualRate, createParamServer)
-{
- ProblemPtr P = Problem::create("PO", 2);
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml");
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
+ ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindowDualRate"));
- auto GM = TreeManagerSlidingWindowDualRate::create(server);
+ auto GM = TreeManagerSlidingWindowDualRate::create(server.getNode());
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
@@ -87,7 +72,7 @@ TEST(TreeManagerSlidingWindowDualRate, createYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = TreeManagerSlidingWindowDualRate::create(wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml");
+ auto GM = TreeManagerSlidingWindowDualRate::create(wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml", {wolf_root});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
@@ -97,31 +82,15 @@ TEST(TreeManagerSlidingWindowDualRate, createYaml)
EXPECT_EQ(P->getTreeManager(), GM);
}
-TEST(TreeManagerSlidingWindowDualRate, FactoryParams)
+TEST(TreeManagerSlidingWindowDualRate, Factory)
{
ProblemPtr P = Problem::create("PO", 2);
- auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+ YamlServer server({wolf_root}, wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml");
- auto GM = FactoryTreeManager::create("TreeManagerSlidingWindowDualRate",ParamsGM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
-
- P->setTreeManager(GM);
-
- EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
- EXPECT_EQ(P->getTreeManager(), GM);
-}
-
-TEST(TreeManagerSlidingWindowDualRate, FactoryParamServer)
-{
- ProblemPtr P = Problem::create("PO", 2);
-
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- auto GM = FactoryTreeManagerServer::create("TreeManagerSlidingWindowDualRate",server);
+ ASSERT_TRUE(server.applySchema("TreeManagerSlidingWindowDualRate"));
+ auto GM = FactoryTreeManager::create("TreeManagerSlidingWindowDualRate",server.getNode());
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
P->setTreeManager(GM);
@@ -134,7 +103,9 @@ TEST(TreeManagerSlidingWindowDualRate, FactoryYaml)
{
ProblemPtr P = Problem::create("PO", 2);
- auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindowDualRate", wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml");
+ auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindowDualRate",
+ wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml",
+ {wolf_root});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
@@ -146,10 +117,7 @@ TEST(TreeManagerSlidingWindowDualRate, FactoryYaml)
TEST(TreeManagerSlidingWindowDualRate, autoConf)
{
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- ProblemPtr P = Problem::autoSetup(server);
+ ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", {wolf_root});
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr);
}
@@ -163,10 +131,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* rate_old_frames: 2
*/
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- ProblemPtr P = Problem::autoSetup(server);
+ ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", {wolf_root});
P->applyPriorOptions(0);
/* FRAME 1 ----------------------------------------------------------
@@ -202,7 +167,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* ( ) ( ) ( )(F1)(F2)
* fix fix
*/
- auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", state);
P->keyFrameCallback(F2, nullptr);
// absolute factor
@@ -237,7 +202,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* ( ) ( ) (F1)(F2)(F3)
* fix fix
*/
- auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", state);
P->keyFrameCallback(F3, nullptr);
// absolute factor
@@ -282,7 +247,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* ( ) (F1)(F2)(F3)(F4)
* fix fix
*/
- auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", state);
P->keyFrameCallback(F4, nullptr);
// absolute factor
@@ -336,7 +301,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* ( ) (F1) (F3)(F4)(F5)
* fix fix
*/
- auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", state);
P->keyFrameCallback(F5, nullptr);
// absolute factor
@@ -398,7 +363,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* (F1) (F3)(F4)(F5)(F6)
* fix fix
*/
- auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
+ auto F6 = P->emplaceFrame(TimeStamp(6), "PO", state);
P->keyFrameCallback(F6, nullptr);
// absolute factor
@@ -469,7 +434,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* (F1) (F3) (F5)(F6)(F7)
* fix fix
*/
- auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
+ auto F7 = P->emplaceFrame(TimeStamp(7), "PO", state);
P->keyFrameCallback(F7, nullptr);
// absolute factor
@@ -548,7 +513,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* (F3) (F5)(F6)(F7)(F8)
* fix fix
*/
- auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
+ auto F8 = P->emplaceFrame(TimeStamp(8), "PO", state);
P->keyFrameCallback(F8, nullptr);
// absolute factor
@@ -638,11 +603,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* n_frames_recent: 3
* rate_old_frames: 2
*/
-
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
-
- ProblemPtr P = Problem::autoSetup(server);
+ ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", {wolf_root});
P->applyPriorOptions(0);
/* FRAME 1 ----------------------------------------------------------
@@ -676,7 +637,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) ( ) ( )(F1)(F2)
*/
- auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
+ auto F2 = P->emplaceFrame(TimeStamp(2), "PO", state);
P->keyFrameCallback(F2, nullptr);
// absolute factor
@@ -710,7 +671,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) ( ) (F1)(F2)(F3)
*/
- auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
+ auto F3 = P->emplaceFrame(TimeStamp(3), "PO", state);
P->keyFrameCallback(F3, nullptr);
// absolute factor
@@ -753,7 +714,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) (F1)(F2)(F3)(F4)
*/
- auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
+ auto F4 = P->emplaceFrame(TimeStamp(4), "PO", state);
P->keyFrameCallback(F4, nullptr);
// absolute factor
@@ -805,7 +766,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* ( ) (F1) (F3)(F4)(F5)
*/
- auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
+ auto F5 = P->emplaceFrame(TimeStamp(5), "PO", state);
P->keyFrameCallback(F5, nullptr);
// absolute factor
@@ -865,7 +826,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* (F1) (F3)(F4)(F5)(F6)
*/
- auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
+ auto F6 = P->emplaceFrame(TimeStamp(6), "PO", state);
P->keyFrameCallback(F6, nullptr);
// absolute factor
@@ -934,7 +895,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* (F1) (F3) (F5)(F6)(F7)
*/
- auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
+ auto F7 = P->emplaceFrame(TimeStamp(7), "PO", state);
P->keyFrameCallback(F7, nullptr);
// absolute factor
@@ -1011,7 +972,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* Sliding window:
* (F3) (F5)(F6)(F7)(F8)
*/
- auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
+ auto F8 = P->emplaceFrame(TimeStamp(8), "PO", state);
P->keyFrameCallback(F8, nullptr);
// absolute factor
@@ -1095,16 +1056,12 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
{
// SLIDING WINDOW DUAL RATE
- ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml");
- ParamsServer server = ParamsServer(parser.getParams());
- ProblemPtr problem = Problem::autoSetup(server);
- SolverManagerPtr solver = FactorySolver::create("SolverCeres", problem, server);
+ ProblemPtr problem = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", {wolf_root});
+ SolverManagerPtr solver = FactorySolverYaml::create("SolverCeres", problem, wolf_root + "/test/yaml/solver.yaml", {wolf_root});
// BASELINE
- ParserYaml parser_bl = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml");
- ParamsServer server_bl = ParamsServer(parser_bl.getParams());
- ProblemPtr problem_bl = Problem::autoSetup(server_bl);
- SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server);
+ ProblemPtr problem_bl = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml", {wolf_root});
+ SolverManagerPtr solver_bl = FactorySolverYaml::create("SolverCeres", problem_bl, wolf_root + "/test/yaml/solver.yaml", {wolf_root});
// aux variables
Vector7d data;
diff --git a/test/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
index 389aa92d8..73f873272 100644
--- a/test/gtest_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -22,32 +22,35 @@
#include "core/common/wolf.h"
#include "core/utils/utils_gtest.h"
-// #include "core/yaml/yaml_conversion.h"
-#include <yaml-schema-cpp/yaml_conversion.h>
+#include <yaml-schema-cpp/yaml_conversion.hpp>
#include <yaml-cpp/yaml.h>
#include <eigen3/Eigen/Dense>
#include <iostream>
-//#include <fstream>
using namespace Eigen;
-using namespace wolf;
TEST(MapYaml, save_2d)
{
MatrixXd M23(2,3);
MatrixXd M33(3,3);
+ VectorXd v3(3);
M23 << 1, 2, 3, 4, 5, 6;
M33 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+ v3 << 1, 2, 3;
std::cout << "\nTrying different yaml specs for matrix..." << std::endl;
- YAML::Node mat_sized_23, mat_sized_33, mat_sized_bad, mat_23, mat_33, mat_bad;
+ YAML::Node mat_sized_23, mat_sized_33, mat_23, mat_33;
+ YAML::Node vec_sized_3, vec_3, vec_0;
mat_sized_23 = YAML::Load("[[2, 3] ,[1, 2, 3, 4, 5, 6] ]"); // insensitive to spacing
mat_sized_33 = YAML::Load("[[3, 3] ,[1, 2, 3, 4, 5, 6, 7, 8, 9]]"); // insensitive to spacing
+ vec_sized_3 = YAML::Load("[[3, 1] ,[1, 2, 3]]"); // insensitive to spacing
- mat_23 = YAML::Load("[1, 2, 3, 4, 5, 6]"); // insensitive to spacing
- mat_33 = YAML::Load("[1, 2, 3, 4, 5, 6, 7, 8, 9]"); // insensitive to spacing
+ mat_23 = YAML::Load("[1, 2, 3, 4, 5, 6]"); // insensitive to spacing
+ mat_33 = YAML::Load("[1, 2, 3, 4, 5, 6, 7, 8, 9]"); // insensitive to spacing
+ vec_3 = YAML::Load("[1, 2, 3]"); // insensitive to spacing
+ //vec_0 = YAML::Load("[]"); // insensitive to spacing
MatrixXd Mx = mat_sized_23.as<MatrixXd>();
std::cout << "Dyn-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << Mx << std::endl;
@@ -76,6 +79,22 @@ TEST(MapYaml, save_2d)
M3 = mat_33.as<Matrix3d>();
std::cout << "3-3 [1, 2, 3, 4, 5, 6, 7, 8, 9] = \n" << M3 << std::endl;
ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
+
+ VectorXd vx = vec_sized_3.as<VectorXd>();
+ std::cout << "Dyn [[3, 1] ,[1, 2, 3] ] = \n" << vx << std::endl;
+ ASSERT_MATRIX_APPROX(vx, v3, 1e-12);
+
+ Vector3d v3d = vec_sized_3.as<Vector3d>();
+ std::cout << "3 [[3, 1] ,[1, 2, 3] ] = \n" << v3d << std::endl;
+ ASSERT_MATRIX_APPROX(v3d, v3, 1e-12);
+
+ v3d = vec_3.as<Vector3d>();
+ std::cout << "3 [1, 2, 3] = \n" << v3d << std::endl;
+ ASSERT_MATRIX_APPROX(v3d, v3, 1e-12);
+
+ vx = vec_3.as<VectorXd>();
+ std::cout << "Dyn [1, 2, 3] = \n" << vx << std::endl;
+ ASSERT_MATRIX_APPROX(vx, v3, 1e-12);
}
int main(int argc, char **argv)
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
deleted file mode 100644
index 142e0059b..000000000
--- a/test/yaml/params1.yaml
+++ /dev/null
@@ -1,40 +0,0 @@
-problem:
- frame_structure: "POV"
- dimension: 3
-sensors:
- -
- type: "ODOM 2d"
- name: "odom"
- plugin: "core"
- intrinsic:
- k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
- extrinsic:
- pose: [1,2,3]
- -
- type: "RANGE BEARING"
- name: "rb"
- plugin: "core"
-processors:
- -
- type: "ODOM 2d"
- name: "processor1"
- sensor_name: "odom"
- plugin: "core"
- -
- type: "RANGE BEARING"
- name: "rb_processor"
- sensor_name: "rb"
- plugin: "core"
- -
- type: "ODOM 2d"
- name: "my_proc_test"
- sensor_name: "odom"
- plugin: "core"
- follow: "params3.1.yaml"
- -
- type: "ODOM 3d"
- name: "my_proc_odom3d"
- sensor_name: "odom"
- plugin: "core"
- follow: "processor_odom_3d.yaml"
diff --git a/test/yaml/params2.yaml b/test/yaml/params2.yaml
deleted file mode 100644
index 38cfe68d2..000000000
--- a/test/yaml/params2.yaml
+++ /dev/null
@@ -1,37 +0,0 @@
-problem:
- frame_structure: "POV"
- dimension: 2
-sensors:
- -
- type: "ODOM 2d"
- name: "odom"
- plugin: "core"
- intrinsic:
- k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
- extrinsic:
- pose: [1,2,3]
- -
- type: "RANGE BEARING"
- name: "rb"
- plugin: "core"
-processors:
- -
- type: "ODOM 2d"
- name: "processor1"
- sensor_name: "odom"
- plugin: "core"
- $mymap:
- k1: v1
- k2: v2
- k3: [v3,v4,v5]
- -
- type: "RANGE BEARING"
- name: "rb_processor"
- sensor_name: "rb"
- plugin: "core"
- -
- type: "ODOM 2d"
- name: "my_proc_test"
- sensor_name: "odom"
- plugin: "core"
diff --git a/test/yaml/params3.1.yaml b/test/yaml/params3.1.yaml
deleted file mode 100644
index bd9b495c8..000000000
--- a/test/yaml/params3.1.yaml
+++ /dev/null
@@ -1,8 +0,0 @@
-cov_det: 100
-unmeasured_perturbation_std: 100
-angle_turned: 100
-dist_traveled: 100
-max_buff_length: 100
-max_time_span: 100
-time_tolerance: 100
-voting_active: false
\ No newline at end of file
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
deleted file mode 100644
index 168d0d47a..000000000
--- a/test/yaml/params3.yaml
+++ /dev/null
@@ -1,21 +0,0 @@
-problem:
- frame_structure: "POV"
- dimension: 2
-sensors:
- -
- type: "ODOM 2d"
- name: "odom"
- plugin: "core"
- intrinsic:
- k_disp_to_disp: 0.1
- k_rot_to_rot: 0.1
- extrinsic:
- pose: [1,2,3]
-processors:
- -
- type: "ODOM 2d"
- name: "my_proc_test"
- plugin: "core"
- sensor_name: "odom"
- extern_params:
- follow: "params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params_basic.yaml b/test/yaml/params_basic.yaml
deleted file mode 100644
index 2033bb4d0..000000000
--- a/test/yaml/params_basic.yaml
+++ /dev/null
@@ -1,34 +0,0 @@
-problem:
- dim: 2
-
-int_1: -3
-int_2: 0
-int_3: "6"
-uint_1: 2
-uint_2: 0
-uint_3: "6"
-double_1: 3.6
-double_2: -3
-double_3: "3.6"
-string_1: wolf
-string_2: "Wolf"
-bool_1: true
-bool_2: True
-bool_3: TRUE
-bool_4: "true"
-bool_5: false
-bool_6: False
-bool_7: FALSE
-bool_8: "false"
-
-int_wrong_1: 2.3
-int_wrong_2: "wolf"
-int_wrong_3: true
-uint_wrong_1: -2
-uint_wrong_2: 3.5
-uint_wrong_3: "wolf"
-uint_wrong_4: true
-double_wrong_1: "wolf"
-double_wrong_2: true
-bool_wrong: 1
-
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index f5bfb8043..65d295419 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -2,19 +2,19 @@ problem:
frame_structure: "PO"
dimension: 3
first_frame:
- 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
+ P:
+ mode: factor
+ state: [0,0,0]
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ mode: factor
+ state: [0,0,0,1]
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
follow: "tree_manager_sliding_window1.yaml"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
states:
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index ad9f63af2..777f51e38 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -2,19 +2,19 @@ problem:
frame_structure: "PO"
dimension: 3
first_frame:
- 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
+ P:
+ mode: factor
+ state: [0,0,0]
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ mode: factor
+ state: [0,0,0,1]
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
follow: "tree_manager_sliding_window2.yaml"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
states:
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 4d9b7a7cb..0822e8648 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -2,13 +2,51 @@ problem:
frame_structure: "PO"
dimension: 3
first_frame:
- 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
+ P:
+ mode: factor
+ state: [0,0,0]
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ mode: factor
+ state: [0,0,0,1]
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
- follow: "tree_manager_sliding_window_dual_rate1.yaml"
\ No newline at end of file
+ follow: "tree_manager_sliding_window_dual_rate1.yaml"
+sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
+ 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
+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: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 10 # meters
+ angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_provider: true
+ state_provider_order: 1
\ No newline at end of file
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 db6c4a294..1f24d9b03 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -2,14 +2,14 @@ problem:
frame_structure: "PO"
dimension: 3
first_frame:
- 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
+ P:
+ mode: factor
+ state: [0,0,0]
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ mode: factor
+ state: [0,0,0,1]
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
plugin: "core"
@@ -18,3 +18,41 @@ problem:
rate_old_frames: 2
n_fix_first_frames: 0
viral_remove_empty_parent: false
+sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
+ 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
+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: 0.2 # seconds
+ max_buff_length: 10 # motion deltas
+ dist_traveled: 10 # meters
+ angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_provider: true
+ state_provider_order: 1
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 95043fc52..f69fece03 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -4,14 +4,14 @@ problem:
frame_structure: "PO"
dimension: 3
first_frame:
- 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
+ P:
+ mode: factor
+ state: [0,0,0]
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ mode: factor
+ state: [0,0,0,1]
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
type: "TreeManagerSlidingWindowDualRate"
plugin: "core"
@@ -22,7 +22,7 @@ problem:
viral_remove_empty_parent: true
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
states:
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 d3dbb0d46..af3c212df 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
@@ -4,19 +4,19 @@ problem:
frame_structure: "PO"
dimension: 3
first_frame:
- 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
+ P:
+ mode: factor
+ state: [0,0,0]
+ noise_std: [0.31, 0.31, 0.31]
+ O:
+ mode: factor
+ state: [0,0,0,1]
+ noise_std: [0.31, 0.31, 0.31]
tree_manager:
type: "None"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
states:
diff --git a/test/yaml/sensor_diff_drive.yaml b/test/yaml/sensor_diff_drive.yaml
index 5276c3992..9e5f3f5df 100644
--- a/test/yaml/sensor_diff_drive.yaml
+++ b/test/yaml/sensor_diff_drive.yaml
@@ -1,3 +1,4 @@
+name: a cool sensor diff drive
states:
P:
mode: fix
--
GitLab