From 21b87f3d7c9e8c143dee8b97d53ce8a5f2f8e0b8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Wed, 13 Jul 2022 18:15:59 +0200 Subject: [PATCH] [skip ci] wip --- CMakeLists.txt | 3 + include/core/frame/frame_base.h | 51 ++-- include/core/math/SE2.h | 1 + include/core/math/SE3.h | 1 + include/core/problem/problem.h | 82 +++--- include/core/processor/motion_provider.h | 45 +-- include/core/sensor/spec_state_sensor.h | 2 +- include/core/state_block/composite.h | 116 ++++++++ include/core/state_block/has_state_blocks.h | 106 +++---- include/core/state_block/spec_composite.h | 85 ++---- include/core/state_block/state_block.h | 2 +- include/core/state_block/state_composite.h | 57 +--- include/core/state_block/vector_composite.h | 86 ++++++ include/core/trajectory/trajectory_base.h | 2 +- include/core/utils/converter.h | 1 + schema/processor/MotionProvider.schema | 6 +- src/frame/frame_base.cpp | 96 ++++--- src/problem/problem.cpp | 295 ++++++++++++-------- src/processor/motion_provider.cpp | 4 +- src/processor/processor_motion.cpp | 6 +- src/processor/processor_odom_2d.cpp | 2 +- src/sensor/sensor_base.cpp | 6 +- src/state_block/has_state_blocks.cpp | 40 ++- src/state_block/spec_composite.cpp | 9 +- src/state_block/state_composite.cpp | 152 ---------- src/state_block/vector_composite.cpp | 167 +++++++++++ test/gtest_motion_provider.cpp | 20 +- test/gtest_problem.cpp | 8 +- test/gtest_state_block.cpp | 2 +- test/gtest_state_composite.cpp | 66 ++--- 30 files changed, 912 insertions(+), 607 deletions(-) create mode 100644 include/core/state_block/composite.h create mode 100644 include/core/state_block/vector_composite.h create mode 100644 src/state_block/vector_composite.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 261e7816b..0b833cbbf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -204,6 +204,7 @@ SET(HDRS_SOLVER include/${PROJECT_NAME}/solver/factory_solver.h ) SET(HDRS_STATE_BLOCK + include/${PROJECT_NAME}/state_block/composite.h include/${PROJECT_NAME}/state_block/factory_state_block.h include/${PROJECT_NAME}/state_block/has_state_blocks.h include/${PROJECT_NAME}/state_block/local_parametrization_angle.h @@ -217,6 +218,7 @@ SET(HDRS_STATE_BLOCK include/${PROJECT_NAME}/state_block/state_composite.h include/${PROJECT_NAME}/state_block/state_homogeneous_3d.h include/${PROJECT_NAME}/state_block/state_quaternion.h + include/${PROJECT_NAME}/state_block/vector_composite.h ) SET(HDRS_TRAJECTORY include/${PROJECT_NAME}/trajectory/trajectory_base.h @@ -319,6 +321,7 @@ SET(SRCS_STATE_BLOCK src/state_block/state_block.cpp src/state_block/state_block_derived.cpp src/state_block/state_composite.cpp + src/state_block/vector_composite.cpp ) SET(SRCS_TRAJECTORY src/trajectory/trajectory_base.cpp diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 2a468a08b..1ff268871 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -70,36 +70,47 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); - /** \brief Constructor with type, time stamp and state pointer - * - * Constructor with type, time stamp and state pointer - * \param _ts is the time stamp associated to this frame, provided in seconds - * \param _frame_structure StateStructure (string) with the state keys - * \param _state VectorComposite containing the state of each key - **/ - FrameBase(const TimeStamp& _ts, - const StateStructure& _frame_structure, - const VectorComposite& _state); - - /** \brief Constructor with type, time stamp and state pointer + // /** \brief Constructor with type, time stamp and state pointer + // * + // * Constructor with type, time stamp and state pointer + // * \param _ts is the time stamp associated to this frame, provided in seconds + // * \param _frame_structure StateStructure (string) with the state keys + // * \param _state VectorComposite containing the state of each key + // **/ + // FrameBase(const TimeStamp& _ts, + // const StateStructure& _frame_structure, + // const VectorComposite& _state); + + // /** \brief Constructor with type, time stamp and state pointer + // * + // * Constructor with type, time stamp and state pointer + // * \param _ts is the time stamp associated to this frame, provided in seconds + // * \param _frame_structure StateStructure (string) with the state keys + // * \param _vectors std::list of VectorXd containing the state of each key + // **/ + // FrameBase(const TimeStamp& _ts, + // const StateStructure& _frame_structure, + // const std::list<VectorXd>& _vectors); + + /** \brief Constructor time stamp and specs composite * - * Constructor with type, time stamp and state pointer + * Constructor with time stamp and specs composite * \param _ts is the time stamp associated to this frame, provided in seconds - * \param _frame_structure StateStructure (string) with the state keys - * \param _vectors std::list of VectorXd containing the state of each key - **/ + * \param _frame_specs SpecComposite containing all information needed to create the state blocs. + **/ FrameBase(const TimeStamp& _ts, - const StateStructure& _frame_structure, - const std::list<VectorXd>& _vectors); + const SpecComposite& _frame_specs); /** \brief Constructor time stamp and specs composite * * Constructor with time stamp and specs composite * \param _ts is the time stamp associated to this frame, provided in seconds - * \param _frame_specs SpecComposite containing all information needed to create the state blocs. + * \param _frame_types TypeComposite containing the types of the state blocs. + * \param _frame_vectors VectorComposite containing the vectors of the the state blocs. **/ FrameBase(const TimeStamp& _ts, - const SpecComposite& _frame_specs); + const TypeComposite& _frame_types, + const VectorComposite& _frame_vectors); ~FrameBase() override; diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h index 32dbd2fc4..63d07cbee 100644 --- a/include/core/math/SE2.h +++ b/include/core/math/SE2.h @@ -23,6 +23,7 @@ #include "core/common/wolf.h" #include "core/math/rotations.h" +#include "core/state_block/vector_composite.h" #include "core/state_block/state_composite.h" #include <Eigen/Geometry> diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h index 701dfebf4..9ae6c2bf3 100644 --- a/include/core/math/SE3.h +++ b/include/core/math/SE3.h @@ -23,6 +23,7 @@ #include "core/common/wolf.h" #include "core/math/rotations.h" +#include "core/state_block/vector_composite.h" #include "core/state_block/state_composite.h" /* diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 3b2799e31..2e77bda2f 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -40,7 +40,7 @@ class Loader; #include "core/frame/frame_base.h" #include "core/state_block/state_block.h" #include "core/state_block/spec_composite.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" #include "core/processor/motion_provider.h" // std includes @@ -73,12 +73,14 @@ class Problem : public std::enable_shared_from_this<Problem> MapBasePtr map_ptr_; std::map<int, MotionProviderPtr> motion_provider_map_; std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_; - SizeEigen state_size_, state_cov_size_, dim_; + // SizeEigen state_size_, state_cov_size_; + SizeEigen dim_; std::map<FactorBasePtr, Notification> factor_notification_map_; std::map<StateBlockPtr, Notification> state_block_notification_map_; mutable std::mutex mut_notifications_; mutable std::mutex mut_covariances_; - StateStructure frame_structure_; + //StateStructure frame_structure_; + TypeComposite frame_structure_; SpecComposite prior_options_; bool prior_applied_; @@ -87,10 +89,14 @@ class Problem : public std::enable_shared_from_this<Problem> mutable std::mutex mut_transform_; private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! + Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !! Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !! void setup(); public: + static ProblemPtr create(const TypeComposite& _frame_structure, + SizeEigen _dim, + MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR! static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR! @@ -109,7 +115,8 @@ class Problem : public std::enable_shared_from_this<Problem> const StateStructure& getFrameStructure() const; protected: - void appendToStructure(const StateStructure& _structure); + //void appendToStructure(const StateStructure& _structure); + void appendToStructure(const TypeComposite& _structure); @@ -194,21 +201,21 @@ class Problem : public std::enable_shared_from_this<Problem> FrameBasePtr applyPriorOptions(const TimeStamp& _ts); FrameBasePtr setPrior(const SpecComposite& priors, const TimeStamp& _ts); - /** \brief Emplace frame from string frame_structure, dimension and vector - * \param _time_stamp Time stamp of the frame - * \param _frame_structure String indicating the frame structure. - * \param _dim variable indicating the dimension of the problem - * \param _frame_state State vector; must match the size and format of the chosen frame structure - * - * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: - * - Create a Frame - * - Add it to Trajectory - * - If it is key-frame, update state-block lists in Problem - */ - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim, // - const Eigen::VectorXd& _frame_state); + // /** \brief Emplace frame from string frame_structure, dimension and vector + // * \param _time_stamp Time stamp of the frame + // * \param _frame_structure String indicating the frame structure. + // * \param _dim variable indicating the dimension of the problem + // * \param _frame_state State vector; must match the size and format of the chosen frame structure + // * + // * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: + // * - Create a Frame + // * - Add it to Trajectory + // * - If it is key-frame, update state-block lists in Problem + // */ + // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + // const StateStructure& _frame_structure, // + // const SizeEigen _dim, // + // const Eigen::VectorXd& _frame_state); /** \brief Emplace frame from string frame_structure and state * \param _time_stamp Time stamp of the frame @@ -222,8 +229,11 @@ class Problem : public std::enable_shared_from_this<Problem> * - Add it to Trajectory * - If it is key-frame, update state-block lists in Problem */ + // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + // const StateStructure& _frame_structure, // + // const VectorComposite& _frame_state); FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // + const TypeComposite& _frame_structure, // const VectorComposite& _frame_state); /** \brief Emplace frame from state @@ -241,22 +251,22 @@ class Problem : public std::enable_shared_from_this<Problem> FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // const VectorComposite& _frame_state); - /** \brief Emplace frame from string frame_structure and dimension - * \param _time_stamp Time stamp of the frame - * \param _frame_structure String indicating the frame structure. - * \param _dim variable indicating the dimension of the problem - * - * - The dimension is taken from Problem - * - The state is taken from Problem - * - * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: - * - Create a Frame - * - Add it to Trajectory - * - If it is key-frame, update state-block lists in Problem - */ - FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim); + // /** \brief Emplace frame from string frame_structure and dimension + // * \param _time_stamp Time stamp of the frame + // * \param _frame_structure String indicating the frame structure. + // * \param _dim variable indicating the dimension of the problem + // * + // * - The dimension is taken from Problem + // * - The state is taken from Problem + // * + // * This acts as a Frame factory, but also takes care to update related lists in WolfProblem: + // * - Create a Frame + // * - Add it to Trajectory + // * - If it is key-frame, update state-block lists in Problem + // */ + // FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, // + // const StateStructure& _frame_structure, // + // const SizeEigen _dim); /** \brief Emplace frame from state vector * \param _time_stamp Time stamp of the frame diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h index 9cec9025a..faa85b47e 100644 --- a/include/core/processor/motion_provider.h +++ b/include/core/processor/motion_provider.h @@ -22,7 +22,7 @@ #pragma once #include "core/common/wolf.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" #include "core/common/params_base.h" // for toString #include "yaml-cpp/yaml.h" @@ -33,20 +33,20 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider); struct ParamsMotionProvider { - bool state_getter = true; - int state_priority = 1; + bool state_provider = true; + int state_order = 1; ParamsMotionProvider() = default; ParamsMotionProvider(const YAML::Node& _n) { - state_getter = _n["state_getter"].as<bool>(); - if (state_getter) - state_priority = _n["state_priority"].as<double>(); + state_provider = _n["state_provider"].as<bool>(); + if (state_provider) + state_order = _n["state_order"].as<double>(); } std::string print() const { - return "state_getter: " + toString(state_getter) + "\n" - + "state_priority: " + toString(state_priority) + "\n"; + return "state_provider: " + toString(state_provider) + "\n" + + "state_provider_order: " + toString(state_order) + "\n"; } }; @@ -58,7 +58,8 @@ class MotionProvider { public: - MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params); + //MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params); + MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params); virtual ~MotionProvider(); // Queries to the processor: @@ -70,21 +71,25 @@ class MotionProvider void setOdometry(const VectorComposite&); bool isStateGetter() const; - int getStatePriority() const; - void setStatePriority(int); + int getOrder() const; + void setOrder(int); public: - const StateStructure& getStateStructure ( ) const { return state_structure_; }; - void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; + //const StateStructure& getStateStructure ( ) const { return state_structure_; }; + const TypeComposite& getStateStructure ( ) const { return state_structure_; }; + //void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; }; + void setStateStructure(TypeComposite _state_structure) { state_structure_ = _state_structure; }; void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr); protected: - StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) + //StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) + TypeComposite state_structure_; VectorComposite odometry_; ParamsMotionProviderPtr params_motion_provider_; }; -inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) : +// inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) : +inline MotionProvider::MotionProvider(const TypeComposite& _structure, ParamsMotionProviderPtr _params) : state_structure_(_structure), params_motion_provider_(_params) { @@ -107,17 +112,17 @@ inline MotionProvider::~MotionProvider() inline bool MotionProvider::isStateGetter() const { - return params_motion_provider_->state_getter; + return params_motion_provider_->state_provider; } -inline int MotionProvider::getStatePriority() const +inline int MotionProvider::getOrder() const { - return params_motion_provider_->state_priority; + return params_motion_provider_->state_order; } -inline void MotionProvider::setStatePriority(int _priority) +inline void MotionProvider::setOrder(int _order) { - params_motion_provider_->state_priority = _priority; + params_motion_provider_->state_order = _order; } } /* namespace wolf */ \ No newline at end of file diff --git a/include/core/sensor/spec_state_sensor.h b/include/core/sensor/spec_state_sensor.h index c1391a534..5773fbc7d 100644 --- a/include/core/sensor/spec_state_sensor.h +++ b/include/core/sensor/spec_state_sensor.h @@ -53,7 +53,7 @@ class SpecStateSensor : public SpecState std::string print(const std::string& _spaces = "") const override; }; -typedef TemplateComposite<SpecStateSensor> SpecSensorComposite; +typedef Composite<SpecStateSensor> SpecSensorComposite; inline bool SpecStateSensor::isDynamic() const { return dynamic_; } diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h new file mode 100644 index 000000000..83f73f5ab --- /dev/null +++ b/include/core/state_block/composite.h @@ -0,0 +1,116 @@ +//--------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 <string> +#include <unordered_map> +#include "yaml-cpp/yaml.h" + +namespace wolf +{ + +typedef std::string StateStructure; +using std::unordered_map; + +template <typename T> +class Composite : public unordered_map<char, T> +{ + public: + using CompositeType = T; + using unordered_map<char, T>::unordered_map; + + Composite(const YAML::Node& _n); + virtual ~Composite() = default; + + template <typename CompositeOther> + CompositeOther cast() const; + + bool has(char _key) const; + bool has(const StateStructure &_structure) const; + StateStructure getStructure() const; + + friend std::ostream& operator<<(std::ostream &_os, const Composite<T> &_x) + { + for (const auto &pair : _x) + { + _os << pair.first << ": " << pair.second; + } + return _os; + } +}; + +template <typename T> +template <typename CompositeOther> +inline CompositeOther Composite<T>::cast() const +{ + CompositeOther casted_composite; + for (auto pair : *this) + { + casted_composite.emplace(pair.first, static_cast<typename CompositeOther::CompositeType>(pair.second)); + } + return casted_composite; +} + +template <typename T> +inline Composite<T>::Composite(const YAML::Node& _n) +{ + if (not _n.IsMap()) + { + throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); + } + for (auto spec_pair : _n) + { + this->emplace(spec_pair.first.as<char>(), T(spec_pair.second)); + } +} + +template <typename T> +inline bool Composite<T>::has(char _key) const +{ + return this->count(_key) != 0; +} + +template <typename T> +inline bool Composite<T>::has(const StateStructure &_structure) const +{ + for (auto key : _structure) + { + if (not has(key)) + { + return false; + } + } + return true; +} + +template <typename T> +inline std::string Composite<T>::getStructure() const +{ + std::string structure; + for (auto pair : *this) + { + structure.push_back(pair.first); + } + return structure; +} + +} // namespace wolf \ No newline at end of file diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index a61e16055..f728227f9 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -22,7 +22,7 @@ #pragma once #include "core/common/wolf.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" #include "core/state_block/spec_composite.h" #include <map> @@ -39,14 +39,15 @@ class HasStateBlocks HasStateBlocks(); HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {} HasStateBlocks(const SpecComposite& _specs); + HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors); virtual ~HasStateBlocks(); const StateStructure& getStructure() const { return structure_; } void appendToStructure(const char& _new_key){ structure_ += _new_key; } bool isInStructure(const char& _sb_key) const { return structure_.find(_sb_key) != std::string::npos; } - const std::unordered_map<char, StateBlockConstPtr>& getStateBlockMap() const; - const std::unordered_map<char, StateBlockPtr>& getStateBlockMap(); + std::unordered_map<char, StateBlockConstPtr> getStateBlockMap() const; + std::unordered_map<char, StateBlockPtr> getStateBlockMap(); std::vector<StateBlockConstPtr> getStateBlockVec() const; std::vector<StateBlockPtr> getStateBlockVec(); @@ -69,8 +70,8 @@ class HasStateBlocks bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const; StateBlockConstPtr getStateBlock(const char& _sb_key) const; StateBlockPtr getStateBlock(const char& _sb_key); - std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const; - std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb); + //std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockConstPtr& _sb) const; + //std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb); // Emplace derived state blocks (angle, quaternion, etc). template<typename SB, typename ... Args> @@ -103,7 +104,6 @@ class HasStateBlocks private: StateStructure structure_; std::unordered_map<char, StateBlockPtr> state_block_map_; - std::unordered_map<char, StateBlockConstPtr> state_block_const_map_; }; @@ -118,8 +118,7 @@ namespace wolf{ inline HasStateBlocks::HasStateBlocks() : structure_(std::string("")), - state_block_map_(), - state_block_const_map_() + state_block_map_() { // } @@ -129,12 +128,15 @@ inline HasStateBlocks::~HasStateBlocks() // } -inline const std::unordered_map<char, StateBlockConstPtr>& HasStateBlocks::getStateBlockMap() const +inline std::unordered_map<char, StateBlockConstPtr> HasStateBlocks::getStateBlockMap() const { - return state_block_const_map_; + std::unordered_map<char, StateBlockConstPtr> map_const; + for (auto&& pair : state_block_map_) + map_const[pair.first] = pair.second; + return map_const; } -inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() +inline std::unordered_map<char, StateBlockPtr> HasStateBlocks::getStateBlockMap() { return state_block_map_; } @@ -162,13 +164,12 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_key) { return state_block_map_.erase(_sb_key); - return state_block_const_map_.erase(_sb_key); } template<typename SB, typename ... Args> inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor) { - assert(state_block_map_.count(_sb_key) == 0 && state_block_const_map_.count(_sb_key) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead."); + assert(state_block_map_.count(_sb_key) == 0 and "Trying to add a state block with an existing type! Use setStateBlock instead."); std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...); addStateBlock(_sb_key, sb, _problem); @@ -178,17 +179,16 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key inline bool HasStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb) { - assert (structure_.find(_sb_key) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead."); - assert ( state_block_map_.count(_sb_key) > 0 && state_block_const_map_.count(_sb_key) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead."); + assert (structure_.find(_sb_key) > 0 and "Cannot set a state block out of the state structure! Use addStateBlock instead."); + assert ( state_block_map_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead."); state_block_map_.at(_sb_key) = _sb; - state_block_const_map_.at(_sb_key) = _sb; return true; // success } inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_key) const { - auto it = state_block_const_map_.find(_sb_key); - if (it != state_block_const_map_.end()) + auto it = state_block_map_.find(_sb_key); + if (it != state_block_map_.end()) return it->second; else return nullptr; @@ -257,7 +257,7 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _ const auto& sb = getStateBlock(key); if (!sb) WOLF_ERROR("Stateblock key ", key, " not in the structure"); - assert (sb && "Stateblock key not in the structure"); + assert (sb and "Stateblock key not in the structure"); sb->setState(vec, _notify); } @@ -272,13 +272,13 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS structure = _structure; int size = getSize(structure); - assert(_state.size() == size && "In FrameBase::setState wrong state size"); + assert(_state.size() == size and "In FrameBase::setState wrong state size"); unsigned int index = 0; for (const char key : structure) { const auto& sb = getStateBlock(key); - assert (sb && "Stateblock key not in the structure"); + assert (sb and "Stateblock key not in the structure"); sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver index += sb->getSize(); @@ -296,8 +296,8 @@ inline void HasStateBlocks::setState (const Eigen::VectorXd& _state, for (const auto& key : _structure) { const auto& sb = getStateBlock(key); - assert (sb && "Stateblock key not in the structure"); - assert(*size_it == sb->getSize() && "State block size mismatch"); + assert (sb and "Stateblock key not in the structure"); + assert(*size_it == sb->getSize() and "State block size mismatch"); sb->setState(_state.segment(index, *size_it), _notify); index += *size_it; @@ -312,8 +312,8 @@ inline void HasStateBlocks::setState(const StateStructure& _structure, const std for (const auto key : _structure) { const auto& sb = getStateBlock(key); - assert (sb && "Stateblock key not in the structure"); - assert(vec_it->size() == sb->getSize() && "State block size mismatch"); + assert (sb and "Stateblock key not in the structure"); + assert(vec_it->size() == sb->getSize() and "State block size mismatch"); sb->setState(*vec_it, _notify); vec_it ++; @@ -337,7 +337,7 @@ inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) { const auto& sb = getStateBlock(key); - assert(sb != nullptr && "Requested StateBlock key not in the structure"); + assert(sb != nullptr and "Requested StateBlock key not in the structure"); state.segment(index,sb->getSize()) = sb->getState(); index += sb->getSize(); @@ -380,20 +380,46 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co return size; } -inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const +// 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::hasStateBlock(const StateBlockConstPtr &_sb) const { - const auto& it = std::find_if(state_block_const_map_.begin(), - state_block_const_map_.end(), - [_sb](const std::pair<char, StateBlockConstPtr>& pair) + 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; + return it != state_block_map_.end(); } -inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) +inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const { const auto& it = std::find_if(state_block_map_.begin(), state_block_map_.end(), @@ -403,21 +429,7 @@ inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::f } ); - return it; -} - -inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const -{ - return this->find(_sb) != state_block_const_map_.end(); -} - -inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const -{ - const auto& it = this->find(_sb); - - bool found = (it != state_block_const_map_.end()); - - if (found) + if (it != state_block_map_.end()) { _key = it->first; return true; diff --git a/include/core/state_block/spec_composite.h b/include/core/state_block/spec_composite.h index f0bda4d63..05fa48c3a 100644 --- a/include/core/state_block/spec_composite.h +++ b/include/core/state_block/spec_composite.h @@ -21,7 +21,8 @@ //--------LICENSE_END-------- #pragma once -#include "core/state_block/state_composite.h" +#include "core/state_block/composite.h" +#include "core/state_block/vector_composite.h" #include <string> #include <unordered_map> @@ -63,88 +64,59 @@ class SpecState virtual void check() const; virtual std::string print(const std::string& _spaces = "") const; + friend std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x); }; +typedef Composite<std::string> TypeComposite; + template <typename T> -class TemplateComposite : public std::unordered_map<char, T> +class SpecBaseComposite : public Composite<T> { public: - using unordered_map<char, T>::unordered_map; - - TemplateComposite(const YAML::Node& _n); - virtual ~TemplateComposite() = default; - - VectorComposite getState() const; + using Composite<T>::Composite; + virtual ~SpecBaseComposite() = default; - template <typename OTHER_T> - TemplateComposite<OTHER_T> cast() const - { - TemplateComposite<OTHER_T> casted_composite; - for (auto pair : *this) - { - casted_composite.emplace(pair.first, static_cast<OTHER_T>(pair.second)); - } - return casted_composite; - } - - void emplace(char _key, const T& _spec); - - std::string print() const; + VectorComposite getVectorComposite() const; + TypeComposite getTypeComposite() const; }; -typedef TemplateComposite<SpecState> SpecComposite; - +typedef SpecBaseComposite<SpecState> SpecComposite; -template <typename T> -TemplateComposite<T>::TemplateComposite(const YAML::Node& _n) +template <> +inline VectorComposite SpecBaseComposite<SpecState>::getVectorComposite() const { - if (not _n.IsMap()) - { - throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); - } - for (auto spec_pair : _n) + VectorComposite states; + for (auto spec_pair : *this) { - this->emplace(spec_pair.first.as<char>(), T(spec_pair.second)); + states.emplace(spec_pair.first, spec_pair.second.getState()); } + return states; } template <typename T> -void TemplateComposite<T>::emplace(char _key, const T& _spec) +inline VectorComposite SpecBaseComposite<T>::getVectorComposite() const { - this->insert({_key,_spec}); + throw std::runtime_error("Impossible to build a VectorComposite from this Composite"); } -template <typename T> -VectorComposite TemplateComposite<T>::getState() const +template <> +inline TypeComposite SpecBaseComposite<SpecState>::getTypeComposite() const { - VectorComposite states; + TypeComposite types; for (auto spec_pair : *this) { - states.emplace(spec_pair.first, spec_pair.second.getState()); + types.emplace(spec_pair.first, spec_pair.second.getType()); } - return states; + return types; } -// template <typename T, typename OTHER_T> -// TemplateComposite<OTHER_T> TemplateComposite<T>::cast() const -// { -// return TemplateComposite<OTHER_T>(); -// } - template <typename T> -std::string TemplateComposite<T>::print() const +inline TypeComposite SpecBaseComposite<T>::getTypeComposite() const { - std::string output; - - for (auto spec_pair : *this) - { - output += std::string(1,spec_pair.first) + ":\n"; - output += spec_pair.second.print("\t"); - } - - return output; + throw std::runtime_error("Impossible to build a TypeComposite from this Composite"); } + inline const std::string& SpecState::getType() const { return type_; } inline const std::string& SpecState::getMode() const { return mode_; } @@ -159,4 +131,5 @@ inline bool SpecState::isInitialGuess() const { return mode_ == "initial_guess"; inline bool SpecState::isFactor() const { return mode_ == "factor"; } -} // namespace wolf \ No newline at end of file +} // namespace wolf + diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h index 2f967122c..ca355e307 100644 --- a/include/core/state_block/state_block.h +++ b/include/core/state_block/state_block.h @@ -29,7 +29,7 @@ class LocalParametrizationBase; //Wolf includes #include "core/common/wolf.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" //std includes #include <iostream> diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h index 83c07d4c4..7307b8afb 100644 --- a/include/core/state_block/state_composite.h +++ b/include/core/state_block/state_composite.h @@ -22,74 +22,21 @@ #pragma once #include "core/common/wolf.h" +#include "core/state_block/vector_composite.h" #include <unordered_map> - #include <iostream> - namespace wolf { using std::string; +using std::unordered_map; using namespace Eigen; -typedef std::string StateStructure; typedef std::unordered_map < char, StateBlockPtr > StateBlockMap; typedef StateBlockMap::const_iterator StateBlockMapCIter; -class VectorComposite : public std::unordered_map < char, Eigen::VectorXd > -{ - public: - VectorComposite() {}; - VectorComposite(const StateStructure& _s); - VectorComposite(const StateStructure& _s, const std::list<int>& _sizes); - /** - * \brief Construct from Eigen::VectorXd and structure - * - * Usage: - * - * VectorXd vec_eigen(1,2,3,4,5); - * - * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !! - * - * result: - * - * vec_comp["a"].transpose(); // = (1,2); - * vec_comp["b"].transpose(); // = (3,4,5); - */ - VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes); - VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors); - - Eigen::VectorXd vector(const StateStructure& _structure) const; - - /** - * \brief set from Eigen::VectorXd and structure - * - * Usage: - * - * Eigen::VectorXd vec_eigen; - * wolf::VectorComposite vec_comp; - * - * vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !! - * - * result: - * - * vec_comp["a"].transpose(); // = (1,2); - * vec_comp["b"].transpose(); // = (3,4,5); - */ - void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes); - void setZero(); - - bool includesStructure(const StateStructure &_structure) const; - - friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x); - friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y); - friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y); - friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x); -}; - - class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > > { diff --git a/include/core/state_block/vector_composite.h b/include/core/state_block/vector_composite.h new file mode 100644 index 000000000..f5a87bcea --- /dev/null +++ b/include/core/state_block/vector_composite.h @@ -0,0 +1,86 @@ +//--------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/common/wolf.h" +#include "core/state_block/composite.h" + +#include <unordered_map> +#include <iostream> + +namespace wolf +{ + +using namespace Eigen; + +class VectorComposite : public Composite<Eigen::VectorXd> +{ + public: + using Composite<Eigen::VectorXd>::Composite; + + VectorComposite() {}; + VectorComposite(const StateStructure& _s); + VectorComposite(const StateStructure& _s, const std::list<int>& _sizes); + /** + * \brief Construct from Eigen::VectorXd and structure + * + * Usage: + * + * VectorXd vec_eigen(1,2,3,4,5); + * + * VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !! + * + * result: + * + * vec_comp["a"].transpose(); // = (1,2); + * vec_comp["b"].transpose(); // = (3,4,5); + */ + VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes); + VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors); + + Eigen::VectorXd vector(const StateStructure& _structure) const; + + /** + * \brief set from Eigen::VectorXd and structure + * + * Usage: + * + * Eigen::VectorXd vec_eigen; + * wolf::VectorComposite vec_comp; + * + * vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !! + * + * result: + * + * vec_comp["a"].transpose(); // = (1,2); + * vec_comp["b"].transpose(); // = (3,4,5); + */ + void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes); + void setZero(); + + friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x); + // friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y); + // friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y); + // friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x); +}; + +} \ No newline at end of file diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 2362b88ce..194c942d7 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -31,7 +31,7 @@ class FrameBase; #include "core/common/wolf.h" #include "core/common/node_base.h" #include "core/common/time_stamp.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" namespace wolf { diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h index f989149fb..9fd1d9a3e 100644 --- a/include/core/utils/converter.h +++ b/include/core/utils/converter.h @@ -24,6 +24,7 @@ // 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 diff --git a/schema/processor/MotionProvider.schema b/schema/processor/MotionProvider.schema index a557389eb..6fe9328c1 100644 --- a/schema/processor/MotionProvider.schema +++ b/schema/processor/MotionProvider.schema @@ -1,10 +1,10 @@ -state_getter: +state_provider: mandatory: true yaml_type: scalar type: bool doc: If the processor is used by the problem as provider of state. -state_priority: +state_provider_order: mandatory: false yaml_type: scalar type: double - doc: The priority of this processor when problem gets the state (only used if state_getter = true). Two processors cannot have the same priority (if so, when installing the second is increased). \ No newline at end of file + doc: The order number of this processor when problem gets the state (only used if state_provider = true). Two processors cannot have the same priority (if so, when installing the second is increased). \ No newline at end of file diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 47da4c53d..a681f73a2 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -33,48 +33,48 @@ namespace wolf { unsigned int FrameBase::frame_id_count_ = 0; -FrameBase::FrameBase(const TimeStamp& _ts, - const StateStructure& _frame_structure, - const VectorComposite& _state) : - NodeBase("FRAME", "FrameBase"), - HasStateBlocks(_frame_structure), - trajectory_ptr_(), - frame_id_(++frame_id_count_), - time_stamp_(_ts) -{ - assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure"); - - for (auto key : getStructure()) - { - StateBlockPtr sb = FactoryStateBlock::create(string(1,key), _state.at(key), false); // false = non fixed - addStateBlock(key, sb, getProblem()); - } -} - - -FrameBase::FrameBase(const TimeStamp& _ts, - const StateStructure& _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(string(1,key), vec, false); // false = non fixed - - addStateBlock(key, sb, getProblem()); - - vec_it++; - } -} +// FrameBase::FrameBase(const TimeStamp& _ts, +// const StateStructure& _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 : getStructure()) +// { +// 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 StateStructure& _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, @@ -113,6 +113,18 @@ FrameBase::FrameBase(const TimeStamp& _ts, } +FrameBase::FrameBase(const TimeStamp& _ts, + const TypeComposite& _frame_types, + const VectorComposite& _frame_vectors) : + NodeBase("FRAME", "FrameBase"), + HasStateBlocks(_frame_types, _frame_vectors), + trajectory_ptr_(), + frame_id_(++frame_id_count_), + time_stamp_(_ts) +{ +} + + FrameBase::~FrameBase() { } diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index dc7720fe5..acab5763a 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -41,7 +41,7 @@ namespace wolf { -Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) : +Problem::Problem(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map) : tree_manager_(nullptr), hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), @@ -51,27 +51,61 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr prior_options_(), prior_applied_(false), transformed_(false) +{ +} + +Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) : + tree_manager_(nullptr), + hardware_ptr_(std::make_shared<HardwareBase>()), + trajectory_ptr_(std::make_shared<TrajectoryBase>()), + map_ptr_(_map), + motion_provider_map_(), + //frame_structure_(_frame_structure), + frame_structure_(), + prior_options_(), + prior_applied_(false), + transformed_(false) { dim_ = _dim; - if (_frame_structure == "PO" and _dim == 2) - { - state_size_ = 3; - state_cov_size_ = 3; - }else if (_frame_structure == "PO" and _dim == 3) - { - state_size_ = 7; - state_cov_size_ = 6; - }else if (_frame_structure == "POV" and _dim == 3) - { - state_size_ = 10; - state_cov_size_ = 9; - }else if (_frame_structure == "POVCDL" and _dim == 3) + for (auto key : _frame_structure) { - state_size_ = 19; - state_cov_size_ = 18; + if (key == 'P') + { + appendToStructure({{'P', (dim_ == 2 ? "StatePoint2d" : "StatePoint3d")}}); + } + else if (key == 'O') + { + appendToStructure({{'P', (dim_ == 2 ? "StateAngle" : "StateQuaternion")}}); + } + else if (key == 'V') + { + appendToStructure({{'P', (dim_ == 2 ? "StateVector2d" : "StateVector3d")}}); + } + else + { + throw std::runtime_error("Problem::Problem: Unknown type for key " + std::string(1,key) + " use the constructor via TypeComposite"); + } } - else std::runtime_error( - "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement."); + + // if (_frame_structure == "PO" and _dim == 2) + // { + // state_size_ = 3; + // state_cov_size_ = 3; + // }else if (_frame_structure == "PO" and _dim == 3) + // { + // state_size_ = 7; + // state_cov_size_ = 6; + // }else if (_frame_structure == "POV" and _dim == 3) + // { + // state_size_ = 10; + // state_cov_size_ = 9; + // }else if (_frame_structure == "POVCDL" and _dim == 3) + // { + // state_size_ = 19; + // state_cov_size_ = 18; + // } + // else std::runtime_error( + // "Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement."); } void Problem::setup() @@ -82,6 +116,13 @@ void Problem::setup() map_ptr_ -> setProblem(shared_from_this()); } +ProblemPtr Problem::create(const TypeComposite& _frame_structure, SizeEigen _dim, MapBasePtr _map) +{ + ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. + p->setup(); + return p->shared_from_this(); +} + ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) { ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. @@ -91,7 +132,6 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::vector<std::string>& _primary_schema_folders) { - // Loaders auto loaders = std::list<std::shared_ptr<Loader>>(); std::string plugins_path = _WOLF_LIB_DIR; @@ -148,9 +188,14 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve // structure and dimension WOLF_INFO("Loading problem parameters"); YAML::Node problem_node = param_node["problem"]; - std::string frame_structure = problem_node["frame_structure"].as<std::string>(); - int dim = problem_node["dimension"].as<int>(); - auto problem = Problem::create(frame_structure, dim, nullptr); + std::string frame_structure; + if (problem_node["frame_structure"]) + { + // TODO: accept types! + frame_structure = problem_node["frame_structure"].as<std::string>(); + } + int dim = problem_node["dimension"].as<int>(); + auto problem = Problem::create(frame_structure, dim, nullptr); // Tree manager WOLF_INFO("Loading tree manager parameters"); @@ -167,7 +212,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve WOLF_INFO("Loading problem first frame parameters"); WOLF_INFO(problem_node["first_frame"]) SpecComposite priors(problem_node["first_frame"]); - WOLF_INFO(priors.print()); + WOLF_INFO(priors); problem->setPriorOptions(priors); // SENSORS =============================================================================== @@ -348,56 +393,57 @@ ProcessorBasePtr Problem::findProcessor(const std::string& _processor_name) return nullptr; } -FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim, // - const Eigen::VectorXd& _frame_state) -{ - VectorComposite state; - SizeEigen index = 0; - SizeEigen size = 0; - for (auto key : _frame_structure) - { - if (key == 'O') - { - if (_dim == 2) size = 1; - else size = 4; - } - else - size = _dim; - - assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small."); - - state.emplace(key, _frame_state.segment(index, size)); - - // append new key to frame structure - if (frame_structure_.find(key) == std::string::npos) // not found - frame_structure_ += std::string(1,key); - - index += size; - } - - assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large."); - - auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, - _time_stamp, - _frame_structure, - state); - - return frm; -} - -FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const SizeEigen _dim) -{ - return emplaceFrame(_time_stamp, - _frame_structure, - getState(_time_stamp)); -} +// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // +// const StateStructure& _frame_structure, // +// const SizeEigen _dim, // +// const Eigen::VectorXd& _frame_state) +// { +// VectorComposite state; +// SizeEigen index = 0; +// SizeEigen size = 0; +// for (auto key : _frame_structure) +// { +// if (key == 'O') +// { +// if (_dim == 2) size = 1; +// else size = 4; +// } +// else +// size = _dim; + +// assert (_frame_state.size() >= index+size && "State vector incompatible with given structure and dimension! Vector too small."); + +// state.emplace(key, _frame_state.segment(index, size)); + +// // append new key to frame structure +// if (frame_structure_.find(key) == std::string::npos) // not found +// frame_structure_ += std::string(1,key); + +// index += size; +// } + +// assert (_frame_state.size() == index && "State vector incompatible with given structure and dimension! Vector too large."); + +// auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, +// _time_stamp, +// _frame_structure, +// state); + +// return frm; +// } + +// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // +// const StateStructure& _frame_structure, // +// const SizeEigen _dim) +// { +// return emplaceFrame(_time_stamp, +// _frame_structure, +// getState(_time_stamp)); +// } FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // + //const StateStructure& _frame_structure, // + const TypeComposite& _frame_structure, // const VectorComposite& _frame_state) { return FrameBase::emplace<FrameBase>(getTrajectory(), @@ -409,12 +455,18 @@ FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // const VectorComposite& _frame_state) { - // append new keys to frame structure - for (auto pair_key_vec : _frame_state) + // // append new keys to frame structure + // for (auto pair_key_vec : _frame_state) + // { + // if (not frame_structure_.has(pair_key_vec.first)) + // auto key = pair_key_vec.first; + // if (frame_structure_.find(key) == std::string::npos) // not found + // frame_structure_ += std::string(1,key); + // } + + if (not frame_structure_.has(_frame_state.getStructure())) { - auto key = pair_key_vec.first; - if (frame_structure_.find(key) == std::string::npos) // not found - frame_structure_ += std::string(1,key); + throw std::runtime_error("Problem::emplaceFrame: the given VectorComposite has a state with an unknown key"); } return FrameBase::emplace<FrameBase>(getTrajectory(), @@ -423,21 +475,25 @@ FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // _frame_state); } -FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // - const Eigen::VectorXd& _frame_state) -{ - return emplaceFrame(_time_stamp, - this->getFrameStructure(), - this->getDim(), - _frame_state); -} - -FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp) -{ - return emplaceFrame(_time_stamp, - this->getFrameStructure(), - this->getDim()); -} +// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // +// const Eigen::VectorXd& _frame_state) +// { +// // return emplaceFrame(_time_stamp, +// // this->getFrameStructure(), +// // this->getDim(), +// // _frame_state); +// return emplaceFrame(_time_stamp, +// this->getFrameStructure(), +// this->getDim(), +// _frame_state); +// } + +// FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp) +// { +// return emplaceFrame(_time_stamp, +// this->getFrameStructure(), +// this->getDim()); +// } FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, const SpecComposite& _frame_spec_composite) @@ -445,7 +501,7 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // Checks if (_frame_spec_composite.count('P') == 0 or _frame_spec_composite.count('O') == 0) { - WOLF_INFO(_frame_spec_composite.print()); + WOLF_INFO(_frame_spec_composite); throw std::runtime_error("Problem::emplaceFrame: emplacing a frame without P or O"); } if (_frame_spec_composite.at('P').getType() != "P" and @@ -533,7 +589,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const state_last = last_kf->getState(structure); else if (not prior_options_.empty()) - state_last = prior_options_.getState(); + state_last = prior_options_.getVectorComposite(); for (auto key : structure) { @@ -545,7 +601,7 @@ VectorComposite Problem::getState(const StateStructure& _structure) const state.emplace(key, state_last_it->second); else - state.emplace(key, stateZero(string(1,key)).at(key)); + state.emplace(key, stateZero(std::string(1,key)).at(key)); } } @@ -587,7 +643,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ state_last = last_kf->getState(structure); else if (not prior_options_.empty()) - state_last = prior_options_.getState(); + state_last = prior_options_.getVectorComposite(); for (auto key : structure) { @@ -599,7 +655,7 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _ state.emplace(key, state_last_it->second); else - state.emplace(key, stateZero(string(1,key)).at(key)); + state.emplace(key, stateZero(std::string(1,key)).at(key)); } } @@ -630,7 +686,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const VectorComposite state_last; if (not prior_options_.empty()) - state_last = prior_options_.getState(); + state_last = prior_options_.getVectorComposite(); for (auto key : structure) { @@ -642,7 +698,7 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const odom_state.emplace(key, state_last_it->second); else - odom_state.emplace(key, stateZero(string(1,key)).at(key)); + odom_state.emplace(key, stateZero(std::string(1,key)).at(key)); } } @@ -656,14 +712,22 @@ SizeEigen Problem::getDim() const const StateStructure& Problem::getFrameStructure() const { - return frame_structure_; + // return frame_structure_; + return frame_structure_.getStructure(); } -void Problem::appendToStructure(const StateStructure& _structure) +// void Problem::appendToStructure(const StateStructure& _structure) +// { +// for (auto key : _structure) +// if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append! +// frame_structure_ += key; +// } + +void Problem::appendToStructure(const TypeComposite& _types) { - for (auto key : _structure) - if (frame_structure_.find(key) == std::string::npos) // now key not found in problem structure -> append! - frame_structure_ += key; + for (auto pair : _types) + if (not frame_structure_.has(pair.first)) // now key not found in problem structure -> append! + frame_structure_.emplace(pair.first, pair.second); } void Problem::setTreeManager(TreeManagerBasePtr _gm) @@ -677,33 +741,36 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm) void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr) { + // Grow TypeComposite storing information about the types of the StateBlocks corresponding to each key + // TODO + // Check if is state getter if (not _motion_provider_ptr->isStateGetter()) { - WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_getter=false. Not adding this processor"); + WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_provider=false. Not adding this processor"); return; } // check duplicated priority - while (motion_provider_map_.count(_motion_provider_ptr->getStatePriority()) == 1) + while (motion_provider_map_.count(_motion_provider_ptr->getOrder()) == 1) { WOLF_ERROR("Problem::addMotionProvider: adding a MotionProvider processor with priority = ", - _motion_provider_ptr->getStatePriority(), + _motion_provider_ptr->getOrder(), " which is already taken. Trying to add it with priority = ", - _motion_provider_ptr->getStatePriority()+1); - _motion_provider_ptr->setStatePriority(_motion_provider_ptr->getStatePriority()+1); + _motion_provider_ptr->getOrder()+1); + _motion_provider_ptr->setOrder(_motion_provider_ptr->getOrder()+1); } // add to map ordered by priority - motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr); + motion_provider_map_.emplace(_motion_provider_ptr->getOrder(), _motion_provider_ptr); appendToStructure(_motion_provider_ptr->getStateStructure()); } void Problem::removeMotionProvider(MotionProviderPtr proc) { - WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor"); + WOLF_WARN_COND(motion_provider_map_.count(proc->getOrder()) == 0, "Problem::clearMotionProvider: missing processor"); - motion_provider_map_.erase(proc->getStatePriority()); + motion_provider_map_.erase(proc->getOrder()); // rebuild frame structure with remaining motion processors frame_structure_.clear(); @@ -1117,7 +1184,6 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) // Factor else if (prior_pair.second.isFactor()) { - // Emplace a capture (if not done already) if (not prior_cap) prior_cap = CaptureBase::emplace<CaptureVoid>(first_frame, _ts, nullptr); @@ -1128,7 +1194,10 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) { throw std::runtime_error("Problem::applyPriorOptions: covariance matrix has wrong size"); } - auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "Prior " + string(1,key), prior_pair.second.getState(), cov); + auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, + "Prior " + std::string(1,key), + prior_pair.second.getState(), + cov); // Emplace a factor if (sb->hasLocalParametrization()) diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp index e2723db61..4cd155830 100644 --- a/src/processor/motion_provider.cpp +++ b/src/processor/motion_provider.cpp @@ -29,7 +29,7 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion setOdometry(_prb_ptr->stateZero(state_structure_)); if (not isStateGetter()) { - WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_getter=false. Not adding this processor"); + WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_provider=false. Not adding this processor"); return; } _prb_ptr->addMotionProvider(_motion_ptr); @@ -37,7 +37,7 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion void MotionProvider::setOdometry(const VectorComposite& _odom) { - assert(_odom.includesStructure(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom."); + assert(_odom.has(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom."); for (auto key : state_structure_) odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_ diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index a934fe6b7..2936dfc16 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -284,7 +284,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) for (auto pair_key_vec : proc_state) if (!keyframe_from_callback->isInStructure(pair_key_vec.first)) keyframe_from_callback->addStateBlock(pair_key_vec.first, - FactoryStateBlock::create(string(1, pair_key_vec.first), + FactoryStateBlock::create(std::string(1, pair_key_vec.first), pair_key_vec.second, false), getProblem()); @@ -375,7 +375,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) for (auto pair_key_vector : proc_state) if (!keyframe_from_callback->isInStructure(pair_key_vector.first)) keyframe_from_callback->addStateBlock(pair_key_vector.first, - FactoryStateBlock::create(string(1, pair_key_vector.first), + FactoryStateBlock::create(std::string(1, pair_key_vector.first), pair_key_vector.second, false), getProblem()); @@ -428,7 +428,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) for (auto & pair_key_vec : state_propa) if (!last_ptr_->getFrame()->isInStructure(pair_key_vec.first)) last_ptr_->getFrame()->addStateBlock(pair_key_vec.first, - FactoryStateBlock::create(string(1, pair_key_vec.first), + FactoryStateBlock::create(std::string(1, pair_key_vec.first), pair_key_vec.second, false), getProblem()); diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index ec6a42639..be37af7ec 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -22,7 +22,7 @@ #include "core/processor/processor_odom_2d.h" #include "core/sensor/sensor_odom.h" #include "core/math/covariance.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" #include "core/factor/factor_relative_pose_2d.h" namespace wolf diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index b2a5f8ee6..725b9f903 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -74,9 +74,9 @@ SensorBase::SensorBase(const std::string& _type, setDriftStd(key, prior.getDriftStd()); } - WOLF_INFO(_priors.print()); - auto base_prior = _priors.cast<SpecState>(); - WOLF_INFO(base_prior.print()); + WOLF_INFO(_priors); + auto base_prior = _priors.cast<SpecComposite>(); + WOLF_INFO(base_prior); } SensorBase::~SensorBase() diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp index 2f59aa27e..717fc720e 100644 --- a/src/state_block/has_state_blocks.cpp +++ b/src/state_block/has_state_blocks.cpp @@ -55,14 +55,50 @@ HasStateBlocks::HasStateBlocks(const SpecComposite& _specs) } } +HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors) +{ + if (not _types.has(_vectors.getStructure()) or not _vectors.has(_types.getStructure())) + { + throw std::runtime_error("HasStateBlocks::HasStateBlocks: provided type and vector composites don't have the same structure"); + } + + for (auto type_pair : _types) + { + auto key = type_pair.first; + auto type = type_pair.second; + auto vector = _vectors.at(key); + + if (key == 'P' and + type != "P" and + type != "StatePoint2d" and + type != "StatePoint3d" ) + { + throw std::runtime_error("HasStateBlocks: in key 'P', the state block should be of type 'P', 'StatePoint2d' or 'StatePoint3d'"); + } + if (key == 'O' and + type != "O" and + type != "StateAngle" and + type != "StateQuaternion" ) + { + throw std::runtime_error("HasStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'"); + } + if (key == 'V' and + type != "V" and + type != "StateVector2d" and + type != "StateVector3d" ) + { + throw std::runtime_error("HasStateBlocks: in key 'O', the state block should be of type 'O', 'StateAngle' or 'StateQuaternion'"); + } + addStateBlock(key, FactoryStateBlock::create(type, vector, false), nullptr); + } +} + StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem) { assert(state_block_map_.count(_sb_key) == 0 and - state_block_const_map_.count(_sb_key) == 0 and "Trying to add a state block with an existing type! Use setStateBlock instead."); state_block_map_.emplace(_sb_key, _sb); - state_block_const_map_.emplace(_sb_key, _sb); if (not isInStructure(_sb_key)) appendToStructure(_sb_key); diff --git a/src/state_block/spec_composite.cpp b/src/state_block/spec_composite.cpp index 7a648badb..f6a43c2c8 100644 --- a/src/state_block/spec_composite.cpp +++ b/src/state_block/spec_composite.cpp @@ -27,6 +27,7 @@ namespace wolf { + SpecState::SpecState(const std::string& _type, const Eigen::VectorXd& _state, const std::string& _mode, @@ -74,10 +75,16 @@ void SpecState::check() const std::string SpecState::print(const std::string& _spaces) const { - return _spaces + "SpecState " + type_ + "\n" + + return _spaces + "type: " + type_ + "\n" + _spaces + "mode: " + mode_ + "\n" + _spaces + "state: " + toString(state_) + "\n" + (mode_ == "factor" ? _spaces + "noise_std: " + toString(noise_std_) + "\n" : ""); } +std::ostream& operator <<(std::ostream &_os, const wolf::SpecState &_x) +{ + _os << _x.print(); + return _os; +} + } // namespace wolf \ No newline at end of file diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp index ff1888db0..a84a74f09 100644 --- a/src/state_block/state_composite.cpp +++ b/src/state_block/state_composite.cpp @@ -20,163 +20,11 @@ // //--------LICENSE_END-------- - - #include "core/state_block/state_composite.h" #include "core/state_block/state_block.h" namespace wolf{ -////// VECTOR COMPOSITE ////////// - -VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes) -{ - auto size_it = _sizes.begin(); - for ( const auto& key : _structure) - { - const auto& size = *size_it; - - this->emplace(key, VectorXd(size).setZero()); - - size_it ++; - } -} - -VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes) -{ - int index = 0; - auto size_it = _sizes.begin(); - for ( const auto& key : _structure) - { - const auto& size = *size_it; - - (*this)[key] = _v.segment(index,size); - - index += size; - size_it ++; - } -} - -VectorComposite::VectorComposite (const StateStructure& _s) -{ - for (const auto& key : _s) - { - this->emplace(key,VectorXd(0)); - } -} - -VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors) -{ - assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch"); - - auto vector_it = _vectors.begin(); - - for (const auto& key : _structure) - { - this->emplace(key, *vector_it); - vector_it++; - } -} - - -Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const -{ - // traverse once with unordered_map access - std::vector<const VectorXd*> vp; - unsigned int size = 0; - for (const auto& key : _structure) - { - vp.push_back(&(this->at(key))); - size += vp.back()->size(); - } - - Eigen::VectorXd x(size); - - // traverse once linearly - unsigned int index = 0; - for (const auto& blkp : vp) - { - x.segment(index, blkp->size()) = *blkp; - index += blkp->size(); - } - return x; -} - -bool VectorComposite::includesStructure(const StateStructure &_structure) const -{ - for (auto key : _structure) - if (count(key) == 0) - return false; - return true; -} - -std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x) -{ - for (const auto &pair_key_vec : _x) - { - const auto &key = pair_key_vec.first; - const auto &vec = pair_key_vec.second; - _os << key << ": (" << vec.transpose() << "); "; - } - return _os; -} - -wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y) -{ - wolf::VectorComposite xpy; - for (const auto& pair_i_xi : _x) - { - const auto& i = pair_i_xi.first; - - xpy.emplace(i, _x.at(i) + _y.at(i)); - } - return xpy; -} - -VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y) -{ - VectorComposite xpy; - for (const auto& pair_i_xi : _x) - { - const auto& i = pair_i_xi.first; - - xpy.emplace(i, _x.at(i) - _y.at(i)); - } - return xpy; -} - -VectorComposite operator -(const wolf::VectorComposite &_x) -{ - wolf::VectorComposite m; - for (const auto& pair_i_xi : _x) - { - const auto& i = pair_i_xi.first; - m.emplace(i, - _x.at(i)); - } - return m; -} - -void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes) -{ - int index = 0; - auto size_it = _sizes.begin(); - for ( const auto& key : _structure) - { - const auto& size = *size_it; - - (*this)[key] = _v.segment(index,size); - - index += size; - size_it ++; - } -} - -void VectorComposite::setZero() -{ - for (auto& pair_key_vec : *this) - pair_key_vec.second.setZero(); -} - ////// MATRIX COMPOSITE ////////// bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk) diff --git a/src/state_block/vector_composite.cpp b/src/state_block/vector_composite.cpp new file mode 100644 index 000000000..f01efc485 --- /dev/null +++ b/src/state_block/vector_composite.cpp @@ -0,0 +1,167 @@ +//--------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/state_block/vector_composite.h" + +namespace wolf{ + +VectorComposite::VectorComposite(const StateStructure& _structure, const std::list<int>& _sizes) +{ + auto size_it = _sizes.begin(); + for ( const auto& key : _structure) + { + const auto& size = *size_it; + + this->emplace(key, VectorXd(size).setZero()); + + size_it ++; + } +} + +VectorComposite::VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes) +{ + int index = 0; + auto size_it = _sizes.begin(); + for ( const auto& key : _structure) + { + const auto& size = *size_it; + + (*this)[key] = _v.segment(index,size); + + index += size; + size_it ++; + } +} + +VectorComposite::VectorComposite (const StateStructure& _s) +{ + for (const auto& key : _s) + { + this->emplace(key,VectorXd(0)); + } +} + +VectorComposite::VectorComposite (const StateStructure& _structure, const std::list<VectorXd>& _vectors) +{ + assert(_structure.size() == _vectors.size() && "Structure and vector list size mismatch"); + + auto vector_it = _vectors.begin(); + + for (const auto& key : _structure) + { + this->emplace(key, *vector_it); + vector_it++; + } +} + + +Eigen::VectorXd VectorComposite::vector(const StateStructure &_structure) const +{ + // traverse once with unordered_map access + std::vector<const VectorXd*> vp; + unsigned int size = 0; + for (const auto& key : _structure) + { + vp.push_back(&(this->at(key))); + size += vp.back()->size(); + } + + Eigen::VectorXd x(size); + + // traverse once linearly + unsigned int index = 0; + for (const auto& blkp : vp) + { + x.segment(index, blkp->size()) = *blkp; + index += blkp->size(); + } + return x; +} + +std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x) +{ + for (const auto &pair_key_vec : _x) + { + const auto &key = pair_key_vec.first; + const auto &vec = pair_key_vec.second; + _os << key << ": (" << vec.transpose() << "); "; + } + return _os; +} + +// wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y) +// { +// wolf::VectorComposite xpy; +// for (const auto& pair_i_xi : _x) +// { +// const auto& i = pair_i_xi.first; + +// xpy.emplace(i, _x.at(i) + _y.at(i)); +// } +// return xpy; +// } + +// VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y) +// { +// VectorComposite xpy; +// for (const auto& pair_i_xi : _x) +// { +// const auto& i = pair_i_xi.first; + +// xpy.emplace(i, _x.at(i) - _y.at(i)); +// } +// return xpy; +// } + +// VectorComposite operator -(const wolf::VectorComposite &_x) +// { +// wolf::VectorComposite m; +// for (const auto& pair_i_xi : _x) +// { +// const auto& i = pair_i_xi.first; +// m.emplace(i, - _x.at(i)); +// } +// return m; +// } + +void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes) +{ + int index = 0; + auto size_it = _sizes.begin(); + for ( const auto& key : _structure) + { + const auto& size = *size_it; + + (*this)[key] = _v.segment(index,size); + + index += size; + size_it ++; + } +} + +void VectorComposite::setZero() +{ + for (auto& pair_key_vec : *this) + pair_key_vec.second.setZero(); +} + +} // namespace wolf \ No newline at end of file diff --git a/test/gtest_motion_provider.cpp b/test/gtest_motion_provider.cpp index bcec7f0ba..b63823360 100644 --- a/test/gtest_motion_provider.cpp +++ b/test/gtest_motion_provider.cpp @@ -62,7 +62,7 @@ class MotionProviderTest : public testing::Test ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>(); prc1_params->time_tolerance = dt/2; prc1_params->state_structure = "PO"; - prc1_params->state_getter = false; + prc1_params->state_provider = false; prc1 = problem->installProcessor("MotionProviderDummy", "not getter processor", sen, @@ -72,8 +72,8 @@ class MotionProviderTest : public testing::Test ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>(); prc2_params->time_tolerance = dt/2; prc2_params->state_structure = "PO"; - prc2_params->state_getter = true; - prc2_params->state_priority = 1; + prc2_params->state_provider = true; + prc2_params->state_order = 1; prc2 = problem->installProcessor("MotionProviderDummy", "getter processor", sen, @@ -83,8 +83,8 @@ class MotionProviderTest : public testing::Test ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>(); prc3_params->time_tolerance = dt/2; prc3_params->state_structure = "POV"; - prc3_params->state_getter = true; - prc3_params->state_priority = 1; + prc3_params->state_provider = true; + prc3_params->state_order = 1; prc3 = problem->installProcessor("MotionProviderDummy", "getter processor low priority", sen, @@ -95,12 +95,12 @@ class MotionProviderTest : public testing::Test /* * Things to be tested: - * - state_getter - * - state_priority + * - state_provider + * - state_order * - Duplicated priority (Problem handles this) * - setOdometry (stateStructures) * - getOdomtry - * - Problem::getState/getOdometry (state_getter, state_priority) + * - Problem::getState/getOdometry (state_provider, state_order) */ TEST_F(MotionProviderTest, install) @@ -117,8 +117,8 @@ TEST_F(MotionProviderTest, install) ASSERT_FALSE(im1->isStateGetter()); ASSERT_TRUE(im2->isStateGetter()); ASSERT_TRUE(im3->isStateGetter()); - ASSERT_EQ(im2->getStatePriority(), 1); - ASSERT_EQ(im3->getStatePriority(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised. + ASSERT_EQ(im2->getOrder(), 1); + ASSERT_EQ(im3->getOrder(), 2); // If duplicated priority, 2nd is changed to +1 priority. A WARN should be raised. ASSERT_EQ(im1->getStateStructure(), "PO"); ASSERT_EQ(im2->getStateStructure(), "PO"); ASSERT_EQ(im3->getStateStructure(), "POV"); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 6c8dbf4ac..1b65d7c04 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -151,7 +151,7 @@ TEST(Problem, SetOrigin_PO_2d) ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0); // check that the state is correct - ASSERT_POSE2d_APPROX(prior.getState().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL ); + ASSERT_POSE2d_APPROX(prior.getVectorComposite().vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL ); // check that we have one frame, one capture, one feature, one factor TrajectoryBasePtr T = P->getTrajectory(); @@ -180,7 +180,7 @@ TEST(Problem, SetOrigin_PO_2d) ASSERT_FALSE(fac->getCaptureOther()); ASSERT_FALSE(fac->getFeatureOther()); } - auto x0_vector = prior.getState().vector("PO"); + auto x0_vector = prior.getVectorComposite().vector("PO"); auto P0_p = prior.at('P').getNoiseStd().cwiseAbs2().asDiagonal(); auto P0_o = prior.at('O').getNoiseStd().cwiseAbs2().asDiagonal(); @@ -214,7 +214,7 @@ TEST(Problem, SetOrigin_PO_3d) ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0); // check that the state is correct - ASSERT_TRUE((prior.getState().vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL)); + ASSERT_TRUE((prior.getVectorComposite().vector("PO") - P->getState().vector("PO")).isMuchSmallerThan(1, Constants::EPS_SMALL)); // check that we have one frame, one capture, one feature, one factor TrajectoryBasePtr T = P->getTrajectory(); @@ -244,7 +244,7 @@ TEST(Problem, SetOrigin_PO_3d) ASSERT_FALSE(fac->getFeatureOther()); } - auto x0_vector = prior.getState().vector("PO"); + auto x0_vector = prior.getVectorComposite().vector("PO"); auto P0_p = prior.at('P').getNoiseStd().cwiseAbs2().asDiagonal(); auto P0_o = prior.at('O').getNoiseStd().cwiseAbs2().asDiagonal(); diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp index 17ad54a8c..5a6272fe4 100644 --- a/test/gtest_state_block.cpp +++ b/test/gtest_state_block.cpp @@ -32,7 +32,7 @@ #include "core/state_block/state_quaternion.h" #include "core/state_block/state_angle.h" #include "core/state_block/state_block_derived.h" -#include "core/state_block/state_composite.h" +#include "core/state_block/vector_composite.h" #include <iostream> diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp index bcb2c0832..f077c1608 100644 --- a/test/gtest_state_composite.cpp +++ b/test/gtest_state_composite.cpp @@ -308,50 +308,50 @@ TEST(VectorComposite, operatorStream) WOLF_DEBUG("X = ", x); } -TEST(VectorComposite, operatorPlus) -{ - VectorComposite x, y; +// TEST(VectorComposite, operatorPlus) +// { +// VectorComposite x, y; - x.emplace('P', Vector2d(1,1)); - x.emplace('O', Vector3d(2,2,2)); - y.emplace('P', Vector2d(1,1)); - y.emplace('O', Vector3d(2,2,2)); +// x.emplace('P', Vector2d(1,1)); +// x.emplace('O', Vector3d(2,2,2)); +// y.emplace('P', Vector2d(1,1)); +// y.emplace('O', Vector3d(2,2,2)); - WOLF_DEBUG("x + y = ", x + y); +// WOLF_DEBUG("x + y = ", x + y); - ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20); - ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20); -} +// ASSERT_MATRIX_APPROX((x+y).at('P'), Vector2d(2,2), 1e-20); +// ASSERT_MATRIX_APPROX((x+y).at('O'), Vector3d(4,4,4), 1e-20); +// } -TEST(VectorComposite, operatorMinus) -{ - VectorComposite x, y; +// TEST(VectorComposite, operatorMinus) +// { +// VectorComposite x, y; - x.emplace('P', Vector2d(3,3)); - x.emplace('O', Vector3d(6,6,6)); - y.emplace('P', Vector2d(1,1)); - y.emplace('O', Vector3d(2,2,2)); +// x.emplace('P', Vector2d(3,3)); +// x.emplace('O', Vector3d(6,6,6)); +// y.emplace('P', Vector2d(1,1)); +// y.emplace('O', Vector3d(2,2,2)); - WOLF_DEBUG("x = ", x); - WOLF_DEBUG("y = ", y); - WOLF_DEBUG("x - y = ", x - y); +// WOLF_DEBUG("x = ", x); +// WOLF_DEBUG("y = ", y); +// WOLF_DEBUG("x - y = ", x - y); - ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20); - ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20); -} +// ASSERT_MATRIX_APPROX((x-y).at('P'), Vector2d(2,2), 1e-20); +// ASSERT_MATRIX_APPROX((x-y).at('O'), Vector3d(4,4,4), 1e-20); +// } -TEST(VectorComposite, unary_Minus) -{ - VectorComposite x, y; +// TEST(VectorComposite, unary_Minus) +// { +// VectorComposite x, y; - x.emplace('P', Vector2d(1,1)); - x.emplace('O', Vector3d(2,2,2)); +// x.emplace('P', Vector2d(1,1)); +// x.emplace('O', Vector3d(2,2,2)); - WOLF_DEBUG("-x = ", -x); +// WOLF_DEBUG("-x = ", -x); - ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20); - ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20); -} +// ASSERT_MATRIX_APPROX((-x).at('P'), Vector2d(-1,-1), 1e-20); +// ASSERT_MATRIX_APPROX((-x).at('O'), Vector3d(-2,-2,-2), 1e-20); +// } TEST(VectorComposite, stateVector) { -- GitLab