diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fb5b5b1092ee4251cf4531f7af1e6afa3245d70..ac07260c67f935f162754f71d9341a04cb3d7130 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 4dd04a0d01e799ca4b82a9e978559d67d3743936..5c138e018d5d5a9fc7df08fa6efa3675763f968a 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 fc62fc8068317a66ae97f47320d85b532eca672d..57c8f0da040893f2b2663fa61ac401320ed8c03c 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 ed381086584a3db172fb73912588d869e8a342fd..33cc7d029008d2ab6325a8eb6f9828419e090b56 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 adc03e088aaac33223d70f542858e3c3fe4ff9d6..0d2f18f2d984f1616c42168202abcab09740e556 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 82aff821af7aa7ef9a5dfa656fb1b8825e48e1e7..9980b4c6c08434b4fdb84eb3c596b3508e5d6bf7 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 00d8da23e2f4247f9bfa1a2adeb75b2a65fe9c01..9cc033cb01eca6b6bdd66c509bca5e5fbdbf6942 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 860ede1ff3bfd123e0f9dfee2115a6e3f275f5c8..96a8192f838dd57e46c06ce464e5ef68e4bcc77c 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 1e621b8f60142e876e5d12c44f70c7472d3f52ee..67dbaf4020696db2495618b5d291ec59acb8fa70 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 71308a397edb94da2f06b628b6838443495a15cd..8ea5e94a6fe1a1c09958354202ea7450c16c753b 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 588f735c9123d33c99ed71f02c185ae668c0d317..3217b1268ba4fee62ba65b80743d4f87650393af 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 9fd1d9a3ef8624b6400c2d80213d363a4207fd01..0000000000000000000000000000000000000000 --- 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 2e3bca6104f1572576855aa75408289b0f79da1b..0000000000000000000000000000000000000000 --- 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 812f39e24995176d0eed792564d687a974e32680..0000000000000000000000000000000000000000 --- 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 a79a36ac23fe70c75376dc8eb60e1be411276015..0000000000000000000000000000000000000000 --- 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 15dad4cd0b8264950cd3090b83c9c94c78241c19..0000000000000000000000000000000000000000 --- 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 0000000000000000000000000000000000000000..d042172be34d684157e91d392be82b7c9c58b539 --- /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 0000000000000000000000000000000000000000..2fa18b6e500200784924e35139e403bb20e7aee4 --- /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 c262f81cbab2b13bb462380e3cdc30abcb0cb48c..5b3a035caba29ee20cea97726c7de6fa4cd6b9d5 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 0000000000000000000000000000000000000000..8d584a1a04557e0ec146c4e8492dec88a5c27dd8 --- /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 0000000000000000000000000000000000000000..c7077e1a2491296a4970664013a313a4ec89bb31 --- /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 dfeae1945084531266c5ffd2019a17ec09e6c809..0000000000000000000000000000000000000000 --- 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 d19cf457c35f5fa45abe6d1a9c63883d5ffd891d..0000000000000000000000000000000000000000 --- 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 7ca80e229594d86d8c4067be0bfe7663ccf0147c..0000000000000000000000000000000000000000 --- 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 0208dd62c388c6a9856c122b5c05ebacec1445bd..fa3c402a986d975dc22b35a61b160641cd286389 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 bd9a2fe5f13657b8bafd79df7547645647d0e3f4..f17dc876982e93d86f1cf22612dc8def1d0cdb06 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 c4fd5ef64fe46b942c9ab088802005adac47e1c9..6fa2388c1290649784174e283cb21685a8635b84 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 d69eb4b4ead92ee67c549570e11d541c8bc09cab..65dd33ac78fa171d52b4370dd82c67c382f0daeb 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 b7e3568c608a36084b3d04ff95e6f4239a0e7dad..7a32e9908556445c5a437676861c21aed7a96d90 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 f77008e1bb528118d955789e15a914d329d804bc..0000000000000000000000000000000000000000 --- 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 7ed323ab07779dd76737eae55f4b5b8818b45922..0000000000000000000000000000000000000000 --- 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 5a415ca2332f50d1c94457448cdffd7a52bbbbf0..0000000000000000000000000000000000000000 --- 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 a61ad826553734812553b483b5f0178f06c0a7e5..7299924ed68ba9bcf93f1a191bfd3401a1062a0a 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 eaaaef6ff3378a6dfc5f0ad5e020e1b9cd7147bb..9d46f7b2a2707f9d40b39491682bcfbd65a17de8 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 5cc0ca76835e7f99d633d8015f4acae10260432e..0000000000000000000000000000000000000000 --- 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 1d97c2a135c188403da08b16c4cb2a16d23201a0..0000000000000000000000000000000000000000 --- 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 2fbf543f033f9a2efee6959e57d1d3347fca89fa..23cf399b4406b61fd2d8a9c65d44e7037b36ff77 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 08578a640eec7afda29a42b3aa84b76d28424a30..c7897da7108bd0001116bf1c497901ecaad5f7dd 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 0712b09ba6d49f1908e5b39cf5db53171bbdec6c..47cc6069c7bcb90dc4f1ea2307cef2d95b0622ff 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 4baf6e8b53885999c718b73280cc5bb6c8d83593..451824fc146931b03aac3fa5c67b8ab4877369b0 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 7f7c15e7f030cba20e63b15d5dbb48a8f8dec609..63bd829179a1b60e53c75f81623a56fa7b026b0e 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 38d865ffa5869234ccdc577ad30892057f3ffce5..b53b15b33437b85b7a48be9087e9312b40a42199 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 b8cbc3bc0e1c85fc76e55d8ca2128ae7031aec1f..b0a8e731bafc6083423b7ea33c201ebbcd109344 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 1ab3e464fd1f2692a15afb04f802118085943141..72e91fee29beb4e01777332d72371a5d338fd682 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 54d9a21b2e354a9c94b9acd3d84e2eed692180b4..e7e79f86c3e3a2c9b4956b0b5dedf9397902aca7 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 4f905a0169af8a6aafaea99064407f86074d60d4..e2d17acbcd62c7f9413328913aa7ceaceed874cf 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 f5d99215c98bcf4d1e6dc39a059cb6a1aa60cec4..d9d6a138683616bfe132dc66bc7235b6d5079485 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 fd252e8001af0a0c9864a234a5901a887ecb245a..9a75da95da299b090bd711162d4c88c0bc51e3d0 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 f994993b47580e4b7b32aa8218896d5f2eba19a8..6297ab1896f904b913d0c41a1d0dd19ce177bb39 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 389aa92d80a48faa12b323df97a9385a7764a7ba..73f873272b402fcf226b4ae3b2eaff746bd3072a 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 142e0059b8d23480f796a2df21ed2945ad0d9235..0000000000000000000000000000000000000000 --- 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 38cfe68d2c4a46e2877a7d7b150c4e359f86183e..0000000000000000000000000000000000000000 --- 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 bd9b495c830e56f17f11abbb5b2109d780f4e438..0000000000000000000000000000000000000000 --- 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 168d0d47aae32ac6952827abd63e7a0b6ed7827f..0000000000000000000000000000000000000000 --- 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 2033bb4d09b6617722c3c427d8e2294892c399d9..0000000000000000000000000000000000000000 --- 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 f5bfb80431edf9a9adb7862288b916ae147ba372..65d29541921b8684f90713081ff15ba73aff31b0 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 ad9f63af228032c76177a3ce43c6aa3816bf06e7..777f51e38f1d0b870e467bdda65a372de0b1bc2a 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 4d9b7a7cb58d44b5067b5f90711796ba5e48e9cf..0822e864899d7db55b6f4fe135da42ea676f1046 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 db6c4a2940bcf51e1b90cb7a53350b5bc68c8a9f..1f24d9b0325f1ab8237ce0003f2a91a0bb3b959f 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 95043fc52e652b7ebef4609a5d361a0eb9068e06..f69fece0398f4b9f9b4d651828a29abcf26c8ad3 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 d3dbb0d4631cf8d0dad287670ac12192a7e25999..af3c212dfc6b7e69c4aaa5050bc5abb1fa1b413e 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 5276c399284196b063651b8e7862f546f8b77ac4..9e5f3f5dfb028d5bbbc6e74d7082901a459441ab 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