Skip to content
Snippets Groups Projects
Commit d9c78507 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Move state accessors to HasStateBlocks

parent 9b7b5adb
No related branches found
No related tags found
1 merge request!323Resolve "New data structure for storing stateblocks"
Pipeline #4353 failed
Stage: build
    This commit is part of merge request !323. Comments created here will be created in the context of that merge request.
    Hide whitespace changes
    Inline Side-by-side
    include/core/frame/frame_base.h
    + 0
    − 5
    View file @ d9c78507
    ......@@ -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 ---------------------------------------------------
    ......
    include/core/state_block/has_state_blocks.h
    + 91
    − 9
    View file @ d9c78507
    ......@@ -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_ */
    src/frame/frame_base.cpp
    + 11
    − 63
    View file @ d9c78507
    ......@@ -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
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment