diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index e5fd2a71de378e2722ffdd656dee9bfe05c636f3..d959b2c5cf93fd6b27c572c58ead62023174df92 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -39,7 +39,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha TrajectoryBaseWPtr trajectory_ptr_; CaptureBasePtrList capture_list_; FactorBasePtrList constrained_by_list_; - std::string structure_; static unsigned int frame_id_count_; @@ -107,10 +106,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha // States public: void setState(const Eigen::VectorXs& _state); - Eigen::VectorXs getState() const; - void getState(Eigen::VectorXs& _state) const; - unsigned int getSize() const; - unsigned int getLocalSize() const; bool getCovariance(Eigen::MatrixXs& _cov) const; // Wolf tree access --------------------------------------------------- diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 4d8ab0c5e1e26a61951df9d88709ff61fd384886..06ee5ce0d0910fc7ae3e381d7f7c03cc6ea70269 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -9,7 +9,6 @@ #define STATE_BLOCK_HAS_STATE_BLOCKS_H_ #include "core/common/wolf.h" -#include "core/state_block/state_block.h" #include <map> @@ -20,8 +19,12 @@ class HasStateBlocks { public: HasStateBlocks(); + HasStateBlocks(const std::string& _structure) : structure_(_structure){} virtual ~HasStateBlocks(); + const std::string& getFrameStructure() const {return structure_;} + std::string& getFrameStructure(){return structure_;} + void appendToStructure(const std::string& _frame_type){structure_ += _frame_type;} const std::map<std::string, StateBlockPtr>& getStateBlockMap() const; std::map<std::string, StateBlockPtr>& getStateBlockMap(); @@ -52,14 +55,8 @@ class HasStateBlocks } StateBlockPtr getStateBlock(const std::string& _sb_type) const; StateBlockPtr getStateBlock(const char& _sb_type) const {return getStateBlock(std::string(1,_sb_type));} - unsigned int removeStateBlock(const std::string& _sb_type) - { - return state_block_map_.erase(_sb_type); - } - unsigned int removeStateBlock(const char& _sb_type) - { - return removeStateBlock(std::string(1,_sb_type)); - } + unsigned int removeStateBlock(const std::string& _sb_type); + unsigned int removeStateBlock(const char& _sb_type); // Emplace derived state blocks (angle, quaternion, etc). template<typename SB, typename ... Args> @@ -69,12 +66,29 @@ class HasStateBlocks template<typename ... Args> inline std::__1::shared_ptr<StateBlock> emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor); + // States + public: + void setState(const Eigen::VectorXs& _state, const bool _notify = true); + Eigen::VectorXs getState() const; + void getState(Eigen::VectorXs& _state) const; + unsigned int getSize() const; + unsigned int getLocalSize() const; private: std::map<std::string, StateBlockPtr> state_block_map_; + std::string structure_; }; +} // namespace wolf + + +//////////// IMPLEMENTATION ///////////// + +#include "core/state_block/state_block.h" + +namespace wolf{ + inline HasStateBlocks::HasStateBlocks() : state_block_map_() { @@ -103,6 +117,16 @@ inline wolf::StateBlockPtr HasStateBlocks::setStateBlock(const std::string& _sb_ return _sb; } +inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type) +{ + return removeStateBlock(std::string(1, _sb_type)); +} + +inline unsigned int HasStateBlocks::removeStateBlock(const std::string& _sb_type) +{ + return state_block_map_.erase(_sb_type); +} + template<typename SB, typename ... Args> inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor) { @@ -205,5 +229,63 @@ inline bool HasStateBlocks::isFixed() const return fixed; } +inline void HasStateBlocks::setState(const Eigen::VectorXs& _state, const bool _notify) +{ + int size = getSize(); + + assert(_state.size() == size && "In FrameBase::setState wrong state size"); + + unsigned int index = 0; + for (const char& key : getFrameStructure()) // note: key is a char + { + const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char). + + sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver + index += sb->getSize(); + } + +} + +inline void HasStateBlocks::getState(Eigen::VectorXs& _state) const +{ + _state.resize(getSize()); + + unsigned int index = 0; + for (const char& key : getFrameStructure()) // note: key is a char + { + const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char). + + _state.segment(index,sb->getSize()) = sb->getState(); + index += sb->getSize(); + } + +} + +inline Eigen::VectorXs HasStateBlocks::getState() const +{ + Eigen::VectorXs state; + + getState(state); + + return state; +} + +inline unsigned int HasStateBlocks::getSize() const +{ + unsigned int size = 0; + for (const auto& pair_key_sb : getStateBlockMap()) + size += pair_key_sb.second->getSize(); + return size; +} + +inline unsigned int HasStateBlocks::getLocalSize() const +{ + unsigned int size = 0; + for (const auto& pair_key_sb : getStateBlockMap()) + size += pair_key_sb.second->getLocalSize(); + return size; +} + + } // namespace wolf #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */ diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index b7d8b539afa88ad546dcb06ce96d9668f3f3c1d8..a3a33834fba343a77e8b10346b914962fec5d6df 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -13,8 +13,8 @@ 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(""), trajectory_ptr_(), - structure_(""), frame_id_(++frame_id_count_), type_(NON_ESTIMATED), time_stamp_(_ts) @@ -22,24 +22,24 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ if (_p_ptr) { setStateBlock("P", _p_ptr); - structure_ += "P"; + appendToStructure("P"); } if (_o_ptr) { setStateBlock("O", _o_ptr); - structure_ += "O"; + appendToStructure("O"); } if (_v_ptr) { setStateBlock("V", _v_ptr); - structure_ += "V"; + appendToStructure("V"); } } FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : NodeBase("FRAME", "Base"), + HasStateBlocks(""), trajectory_ptr_(), - structure_(""), frame_id_(++frame_id_count_), type_(_tp), time_stamp_(_ts) @@ -47,24 +47,24 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr if (_p_ptr) { setStateBlock("P", _p_ptr); - structure_ += "P"; + appendToStructure("P"); } if (_o_ptr) { setStateBlock("O", _o_ptr); - structure_ += "O"; + appendToStructure("O"); } if (_v_ptr) { setStateBlock("V", _v_ptr); - structure_ += "V"; + appendToStructure("V"); } } FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) : NodeBase("FRAME", "Base"), + HasStateBlocks(_frame_structure), trajectory_ptr_(), - structure_(_frame_structure), frame_id_(++frame_id_count_), type_(_tp), time_stamp_(_ts) @@ -154,7 +154,7 @@ void FrameBase::registerNewStateBlocks() void FrameBase::removeStateBlocks() { - for (const char& key : structure_) // note: key is a char + for (const char& key : getFrameStructure()) // note: key is a char { auto sbp = getStateBlock(key); if (sbp != nullptr) @@ -213,59 +213,7 @@ void FrameBase::setAux() void FrameBase::setState(const Eigen::VectorXs& _state) { - int size = getSize(); - - assert(_state.size() == size && "In FrameBase::setState wrong state size"); - - unsigned int index = 0; - for (const char& key : structure_) // note: key is a char - { - const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char). - - sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver - index += sb->getSize(); - } - -} - -void FrameBase::getState(Eigen::VectorXs& _state) const -{ - _state.resize(getSize()); - - unsigned int index = 0; - for (const char& key : structure_) // note: key is a char - { - const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char). - - _state.segment(index,sb->getSize()) = sb->getState(); - index += sb->getSize(); - } - -} - -Eigen::VectorXs FrameBase::getState() const -{ - Eigen::VectorXs state; - - getState(state); - - return state; -} - -unsigned int FrameBase::getSize() const -{ - unsigned int size = 0; - for (const auto& pair_key_sb : getStateBlockMap()) - size += pair_key_sb.second->getSize(); - return size; -} - -unsigned int FrameBase::getLocalSize() const -{ - unsigned int size = 0; - for (const auto& pair_key_sb : getStateBlockMap()) - size += pair_key_sb.second->getLocalSize(); - return size; + HasStateBlocks::setState(_state, isKeyOrAux()); } bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const