From 607dae491c2322ce2aaa34ee389abc642d3ddf0a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu> Date: Wed, 25 Mar 2020 23:50:10 +0100 Subject: [PATCH] WIP Use StateStructure instead of std::string for state structures --- include/core/capture/capture_base.h | 2 +- include/core/common/wolf.h | 5 ++ include/core/frame/frame_base.h | 2 +- include/core/processor/is_motion.h | 7 +- include/core/state_block/has_state_blocks.h | 87 ++++++++++++--------- src/capture/capture_base.cpp | 4 +- src/frame/frame_base.cpp | 6 +- src/landmark/landmark_base.cpp | 2 +- 8 files changed, 66 insertions(+), 49 deletions(-) diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 70fee2c15..0a52b6193 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -81,7 +81,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s const FactorBasePtrList& getConstrainedByList() const; // State blocks - const std::string& getStructure() const; + const StateStructure& getStructure() const; StateBlockPtr getStateBlock(const std::string& _key) const; StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); } StateBlockPtr getSensorP() const; diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h index 9f548fd32..b06c4e7da 100644 --- a/include/core/common/wolf.h +++ b/include/core/common/wolf.h @@ -106,6 +106,11 @@ typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmaj namespace wolf { +/// State of nodes containing several state blocks +typedef std::unordered_map<std::string, Eigen::VectorXd> State; +typedef std::list<std::string> StateStructure; + + ////////////////////////////////////////////////////////// /** Check Eigen Matrix sizes, both statically and dynamically * diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 14bcc2139..74c2b059a 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -69,7 +69,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha **/ FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); - FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x); + FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x); virtual ~FrameBase(); virtual void remove(bool viral_remove_empty_parent=false); diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h index 8c99fe72e..f9f77b10c 100644 --- a/include/core/processor/is_motion.h +++ b/include/core/processor/is_motion.h @@ -9,6 +9,7 @@ #define PROCESSOR_IS_MOTION_H_ #include "core/common/wolf.h" +//#include "core/state_block/has_state_blocks.h" namespace wolf { @@ -43,11 +44,11 @@ class IsMotion TimeStamp getCurrentTimeStamp() const; Eigen::VectorXd getState(const TimeStamp& _ts) const; - std::string getStateStructure(){return state_structure_;}; - void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;}; + const StateStructure& getStateStructure(){return state_structure_;}; + void setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;}; protected: - std::string 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) }; diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 76df765d2..b1f59dc71 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -10,25 +10,26 @@ #include "core/common/wolf.h" -#include <map> +#include <unordered_map> namespace wolf { -/// State of nodes containing several state blocks -typedef std::unordered_map<std::string, Eigen::VectorXd> State; +///// State of nodes containing several state blocks +//typedef std::unordered_map<std::string, Eigen::VectorXd> State; +//typedef std::list<std::string> StateStructure; class HasStateBlocks { public: HasStateBlocks(); - HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {} + HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {} virtual ~HasStateBlocks(); - const std::string& getStructure() const { return structure_; } - void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; } - bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; } + const StateStructure& getStructure() const { return structure_; } + void appendToStructure(const std::string& _frame_type){ structure_.push_back(_frame_type); } + bool isInStructure(const std::string& _sb_type) { return std::find(structure_.begin(), structure_.end(), _sb_type) != structure_.end(); } const std::map<std::string, StateBlockPtr>& getStateBlockMap() const; std::vector<StateBlockPtr> getStateBlockVec() const; @@ -68,16 +69,16 @@ class HasStateBlocks void removeStateBlocks(ProblemPtr _problem); // States - inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true); - void getState(Eigen::VectorXd& _state, std::string structure="") const; - Eigen::VectorXd getState(std::string structure="") const; - unsigned int getSize(std::string _sub_structure="") const; - unsigned int getLocalSize(std::string _sub_structure="") const; + inline void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true); + void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const; + Eigen::VectorXd getState(const StateStructure& structure = StateStructure()) const; + unsigned int getSize(const StateStructure& _sub_structure = StateStructure()) const; + unsigned int getLocalSize(const StateStructure& _sub_structure = StateStructure()) const; State getStateComposite(); private: - std::string structure_; + StateStructure structure_; std::map<std::string, StateBlockPtr> state_block_map_; }; @@ -92,7 +93,7 @@ class HasStateBlocks namespace wolf{ inline HasStateBlocks::HasStateBlocks() : - structure_(std::string("")), + structure_(), state_block_map_() { // @@ -161,7 +162,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb) { - assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead."); + assert (std::find(structure_.begin(), structure_.end(),_sb_type) != structure_.end() && "Cannot set a state block out of the state structure! Use addStateBlock instead."); assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead."); state_block_map_.at(_sb_type) = _sb; return true; // success @@ -221,16 +222,19 @@ inline bool HasStateBlocks::isFixed() const return fixed; } -inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify) +inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify) { - if (_sub_structure == ""){ - _sub_structure = structure_; - } - int size = getSize(_sub_structure); + StateStructure sub_structure; + if (_sub_structure.empty()) + sub_structure = structure_; + else + sub_structure = _sub_structure; + + int size = getSize(sub_structure); assert(_state.size() == size && "In FrameBase::setState wrong state size"); unsigned int index = 0; - for (const char key : _sub_structure) + for (const auto& key : sub_structure) { const auto& sb = getStateBlock(key); if (!sb){ @@ -243,15 +247,17 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string } // _sub_structure can be either stateblock structure of the node or a subset of this structure -inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const +inline void HasStateBlocks::getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure) const { - if (_sub_structure == ""){ - _sub_structure = structure_; - } - _state.resize(getSize(_sub_structure)); + StateStructure sub_structure; + if (_sub_structure.empty()) + sub_structure = structure_; + else + sub_structure = _sub_structure; + _state.resize(getSize(sub_structure)); unsigned int index = 0; - for (const char key : _sub_structure) + for (const auto& key : sub_structure) { const auto& sb = getStateBlock(key); if (!sb){ @@ -263,7 +269,7 @@ inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_s } -inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const +inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_structure) const { Eigen::VectorXd state; @@ -282,13 +288,16 @@ inline State HasStateBlocks::getStateComposite() return state; } -inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const +inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const { - if (_sub_structure == ""){ - _sub_structure = structure_; - } + StateStructure sub_structure; + if (_sub_structure.empty()) + sub_structure = structure_; + else + sub_structure = _sub_structure; + unsigned int size = 0; - for (const char key : _sub_structure) + for (const auto& key : sub_structure) { const auto& sb = getStateBlock(key); if (!sb){ @@ -300,13 +309,15 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const return size; } -inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const +inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const { - if (_sub_structure == ""){ - _sub_structure = structure_; - } + StateStructure sub_structure; + if (_sub_structure.empty()) + sub_structure = structure_; + else + sub_structure = _sub_structure; unsigned int size = 0; - for (const char key : _sub_structure) + for (const auto& key : sub_structure) { const auto& sb = getStateBlock(key); if (!sb){ diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 6e78e69a8..aacaec893 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -14,7 +14,7 @@ CaptureBase::CaptureBase(const std::string& _type, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr) : NodeBase("CAPTURE", _type), - HasStateBlocks(""), + HasStateBlocks(StateStructure()), frame_ptr_(), // nullptr sensor_ptr_(_sensor_ptr), calib_size_(0), @@ -130,7 +130,7 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr) constrained_by_list_.remove(_fac_ptr); } -const std::string& CaptureBase::getStructure() const +const StateStructure& CaptureBase::getStructure() const { if (getSensor()) return getSensor()->getStructure(); diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index d437515e4..74131253f 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -13,7 +13,7 @@ unsigned int FrameBase::frame_id_count_ = 0; FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "Base"), - HasStateBlocks(""), + HasStateBlocks(StateStructure()), trajectory_ptr_(), frame_id_(++frame_id_count_), type_(NON_ESTIMATED), @@ -35,7 +35,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "Base"), - HasStateBlocks(""), + HasStateBlocks(StateStructure()), trajectory_ptr_(), frame_id_(++frame_id_count_), type_(_tp), @@ -55,7 +55,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr } } -FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) : +FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) : NodeBase("FRAME", "Base"), HasStateBlocks(_frame_structure), trajectory_ptr_(), diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 1e24627be..1a1d785a3 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -14,7 +14,7 @@ unsigned int LandmarkBase::landmark_id_count_ = 0; LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : NodeBase("LANDMARK", _type), - HasStateBlocks(""), + HasStateBlocks(StateStructure()), map_ptr_(), state_block_vec_(0), // Resize in derived constructors if needed. landmark_id_(++landmark_id_count_) -- GitLab