remove all commented code related to state_block_vec_

include/core/frame/frame_base.h

@@ -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

@@ -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;