Add frame structure string

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

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),