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

remove all commented code related to state_block_vec_

parent 370f9060
No related branches found
No related tags found
1 merge request!323Resolve "New data structure for storing stateblocks"
Pipeline #4352 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
    + 1
    − 66
    View file @ 9b7b5adb
    ......@@ -40,7 +40,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
    CaptureBasePtrList capture_list_;
    FactorBasePtrList constrained_by_list_;
    std::string structure_;
    // std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
    static unsigned int frame_id_count_;
    ......@@ -97,32 +96,14 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
    void getTimeStamp(TimeStamp& _ts) const;
    // // State blocks
    // public:
    // const std::vector<StateBlockPtr>& getStateBlockVec() const;
    // std::vector<StateBlockPtr>& getStateBlockVec();
    // protected:
    // StateBlockPtr getStateBlock(unsigned int _i) const;
    // void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
    // void resizeStateBlockVec(unsigned int _size);
    //
    // public:
    // StateBlockPtr getP() const;
    // StateBlockPtr getO() const;
    public:
    StateBlockPtr getV() const;
    // void setP(const StateBlockPtr _p_ptr);
    // void setO(const StateBlockPtr _o_ptr);
    void setV(const StateBlockPtr _v_ptr);
    protected:
    void registerNewStateBlocks();
    void removeStateBlocks();
    virtual void setProblem(ProblemPtr _problem) final;
    // Fixed / Estimated
    // public:
    // void fix();
    // void unfix();
    // bool isFixed() const;
    // States
    public:
    void setState(const Eigen::VectorXs& _state);
    ......@@ -214,34 +195,6 @@ inline TimeStamp FrameBase::getTimeStamp() const
    return time_stamp_;
    }
    //inline const std::vector<StateBlockPtr>& FrameBase::getStateBlockVec() const
    //{
    // return state_block_vec_;
    //}
    //inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
    //{
    // return state_block_vec_;
    //}
    //inline StateBlockPtr FrameBase::getP() const
    //{
    // return state_block_vec_[0];
    //}
    //inline void FrameBase::setP(const StateBlockPtr _p_ptr)
    //{
    // state_block_vec_[0] = _p_ptr;
    //}
    //inline StateBlockPtr FrameBase::getO() const
    //{
    // return state_block_vec_[1];
    //}
    //inline void FrameBase::setO(const StateBlockPtr _o_ptr)
    //{
    // state_block_vec_[1] = _o_ptr;
    //}
    inline StateBlockPtr FrameBase::getV() const
    {
    return getStateBlock("V");
    ......@@ -252,18 +205,6 @@ inline void FrameBase::setV(const StateBlockPtr _v_ptr)
    setStateBlock("V", _v_ptr);
    }
    //inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
    //{
    // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
    // return state_block_vec_[_i];
    //}
    //
    //inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
    //{
    // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
    // state_block_vec_[_i] = _sb_ptr;
    //}
    inline TrajectoryBasePtr FrameBase::getTrajectory() const
    {
    return trajectory_ptr_.lock();
    ......@@ -274,12 +215,6 @@ inline const CaptureBasePtrList& FrameBase::getCaptureList() const
    return capture_list_;
    }
    //inline void FrameBase::resizeStateBlockVec(unsigned int _size)
    //{
    // if (_size > state_block_vec_.size())
    // state_block_vec_.resize(_size);
    //}
    inline unsigned int FrameBase::getHits() const
    {
    return constrained_by_list_.size();
    ......
    src/frame/frame_base.cpp
    + 0
    − 47
    View file @ 9b7b5adb
    ......@@ -15,7 +15,6 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
    NodeBase("FRAME", "Base"),
    trajectory_ptr_(),
    structure_(""),
    // state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
    frame_id_(++frame_id_count_),
    type_(NON_ESTIMATED),
    time_stamp_(_ts)
    ......@@ -35,16 +34,12 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
    setStateBlock("V", _v_ptr);
    structure_ += "V";
    }
    // state_block_vec_[0] = _p_ptr;
    // state_block_vec_[1] = _o_ptr;
    // state_block_vec_[2] = _v_ptr;
    }
    FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
    NodeBase("FRAME", "Base"),
    trajectory_ptr_(),
    structure_(""),
    // state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
    frame_id_(++frame_id_count_),
    type_(_tp),
    time_stamp_(_ts)
    ......@@ -64,41 +59,29 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
    setStateBlock("V", _v_ptr);
    structure_ += "V";
    }
    // state_block_vec_[0] = _p_ptr;
    // state_block_vec_[1] = _o_ptr;
    // state_block_vec_[2] = _v_ptr;
    }
    FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
    NodeBase("FRAME", "Base"),
    trajectory_ptr_(),
    structure_(_frame_structure),
    // state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
    frame_id_(++frame_id_count_),
    type_(_tp),
    time_stamp_(_ts)
    {
    if(_frame_structure.compare("PO") == 0 and _dim == 2){
    // auto _x = Eigen::VectorXs::Zero(3);
    assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
    StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
    StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
    setStateBlock("P", p_ptr);
    setStateBlock("O", o_ptr);
    // state_block_vec_[0] = p_ptr;
    // state_block_vec_[1] = o_ptr;
    // state_block_vec_[2] = v_ptr;
    this->setType("PO 2D");
    }else if(_frame_structure.compare("PO") == 0 and _dim == 3){
    // auto _x = Eigen::VectorXs::Zero(7);
    assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
    StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
    StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
    setStateBlock("P", p_ptr);
    setStateBlock("O", o_ptr);
    // state_block_vec_[0] = p_ptr;
    // state_block_vec_[1] = o_ptr;
    // state_block_vec_[2] = v_ptr;
    this->setType("PO 3D");
    }else if(_frame_structure.compare("POV") == 0 and _dim == 3){
    // auto _x = Eigen::VectorXs::Zero(10);
    ......@@ -109,9 +92,6 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
    setStateBlock("P", p_ptr);
    setStateBlock("O", o_ptr);
    setStateBlock("V", v_ptr);
    // state_block_vec_[0] = p_ptr;
    // state_block_vec_[1] = o_ptr;
    // state_block_vec_[2] = v_ptr;
    this->setType("POV 3D");
    }else{
    std::cout << _frame_structure << " ^^ " << _dim << std::endl;
    ......@@ -231,37 +211,10 @@ void FrameBase::setAux()
    }
    }
    //void FrameBase::fix()
    //{
    // for (auto sbp : state_block_vec_)
    // if (sbp != nullptr)
    // sbp->fix();
    //}
    //
    //void FrameBase::unfix()
    //{
    // for (auto sbp : state_block_vec_)
    // if (sbp != nullptr)
    // sbp->unfix();
    //}
    //
    //bool FrameBase::isFixed() const
    //{
    // bool fixed = true;
    // for (auto sb : getStateBlockVec())
    // {
    // if (sb)
    // fixed &= sb->isFixed();
    // }
    // return fixed;
    //}
    void FrameBase::setState(const Eigen::VectorXs& _state)
    {
    int size = getSize();
    //State Vector size must be lower or equal to frame state size :
    // example : POV -> if we initialize only position and orientation due to use of processorOdom3D
    assert(_state.size() == size && "In FrameBase::setState wrong state size");
    unsigned int index = 0;
    ......
    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