Resolve "New data structure for storing stateblocks"

Closes #256 (closed)

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();