diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index a27970fa474f50bbb27de5d384457e09be24dce0..aafa35a21b634b89bcecbf057de5674967bcafcc 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -39,6 +39,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha TrajectoryBaseWPtr trajectory_ptr_; 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_; diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 3269ff36bba7009ab85a954a4e78f7e92f17764b..6e05103636414770603493105a1e632d280206cd 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -14,17 +14,27 @@ unsigned int FrameBase::frame_id_count_ = 0; FrameBase::FrameBase(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_(NON_ESTIMATED), time_stamp_(_ts) { if (_p_ptr) + { setStateBlock("P", _p_ptr); + structure_ += "P"; + } if (_o_ptr) + { setStateBlock("O", _o_ptr); + structure_ += "O"; + } if (_v_ptr) + { setStateBlock("V", _v_ptr); + structure_ += "V"; + } // state_block_vec_[0] = _p_ptr; // state_block_vec_[1] = _o_ptr; // state_block_vec_[2] = _v_ptr; @@ -33,17 +43,27 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ 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) { if (_p_ptr) + { setStateBlock("P", _p_ptr); + structure_ += "P"; + } if (_o_ptr) + { setStateBlock("O", _o_ptr); + structure_ += "O"; + } if (_v_ptr) + { setStateBlock("V", _v_ptr); + structure_ += "V"; + } // state_block_vec_[0] = _p_ptr; // state_block_vec_[1] = _o_ptr; // state_block_vec_[2] = _v_ptr; @@ -52,6 +72,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr 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),