add some getStateComposite()

include/core/common/wolf.h

@@ -107,7 +107,7 @@ typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmaj
 namespace wolf {
 
 /// State of nodes containing several state blocks
-typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+typedef std::unordered_map<std::string, Eigen::VectorXd> StateComposite;
 typedef std::string StateStructure;
 
 

include/core/problem/problem.h

@@ -279,6 +279,9 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Perturb state
         void            perturb(double amplitude = 0.01);
 
+        const StateComposite&  getStateComposite() const;
+        const StateComposite&  getStateComposite(const TimeStamp& _ts) const;
+
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;
         void loadMap(const std::string& _filename_dot_yaml);

include/core/processor/is_motion.h

@@ -44,6 +44,9 @@ class IsMotion
         TimeStamp       getCurrentTimeStamp() const;
         Eigen::VectorXd getState(const TimeStamp& _ts) const;
 
+        StateComposite  getStateComposite() const;
+        StateComposite  getStateComposite(const TimeStamp& _ts) const;
+
         const StateStructure& getStateStructure(){return state_structure_;};
         void setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;};
     

include/core/state_block/has_state_blocks.h

@@ -15,10 +15,6 @@
 namespace wolf
 {
 
-///// State of nodes containing several state blocks
-//typedef std::unordered_map<std::string, Eigen::VectorXd> State;
-//typedef std::list<std::string> StateStructure;
-
 
 class HasStateBlocks
 {
@@ -29,7 +25,7 @@ class HasStateBlocks
 
         const StateStructure& getStructure() const { return structure_; }
         void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
-        bool isInStructure(const std::string& _sb_type) { return std::find(structure_.begin(), structure_.end(), _sb_type) != structure_.end(); }
+        bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type); }
 
         const std::unordered_map<std::string, StateBlockPtr>& getStateBlockMap() const;
         std::vector<StateBlockPtr> getStateBlockVec() const;
@@ -70,15 +66,14 @@ class HasStateBlocks
         void removeStateBlocks(ProblemPtr _problem);
 
         // States
-        inline void     setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true);
-        void            getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
-        Eigen::VectorXd getState(const StateStructure& structure = StateStructure()) const;
         unsigned int    getSize(const StateStructure& _sub_structure = StateStructure()) const;
         unsigned int    getLocalSize(const StateStructure& _sub_structure = StateStructure()) const;
+        inline void     setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true);
+        void            getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
+        Eigen::VectorXd getState(const StateStructure& _sub_structure = StateStructure()) const;
 
-        inline unsigned int getLocalSize(StateStructure _sub_structure = "") const;
-
-        State           getStateComposite();
+        StateComposite  getStateComposite();
+        void            setStateComposite(const StateComposite& _state);
 
         // Perturb state
         void            perturb(double amplitude = 0.01);
@@ -160,7 +155,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
 
 inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
 {
-    assert (std::find(structure_.begin(), structure_.end(),_sb_type) != structure_.end() && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
+    assert (structure_.find(_sb_type) && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
     assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
     state_block_map_.at(_sb_type) = _sb;
     return true; // success
@@ -212,17 +207,17 @@ inline bool HasStateBlocks::isFixed() const
 
 inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
 {
-    StateStructure sub_structure;
+    StateStructure structure;
     if (_sub_structure.empty())
-        sub_structure = structure_;
+        structure = structure_;
     else
-        sub_structure = _sub_structure;
+        structure = _sub_structure;
 
-    int size = getSize(sub_structure);
+    int size = getSize(structure);
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const auto& key : sub_structure)
+    for (const auto& key : structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
@@ -237,15 +232,15 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateS
 // _sub_structure can be either stateblock structure of the node or a subset of this structure
 inline void HasStateBlocks::getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure) const
 {
-    StateStructure sub_structure;
+    StateStructure structure;
     if (_sub_structure.empty())
-        sub_structure = structure_;
+        structure = structure_;
     else
-        sub_structure = _sub_structure;
-    _state.resize(getSize(sub_structure));
+        structure = _sub_structure;
+    _state.resize(getSize(structure));
 
     unsigned int index = 0;
-    for (const auto& key : sub_structure)
+    for (const auto& key : structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){
@@ -266,35 +261,22 @@ inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_struc
     return state;
 }
 
-inline State HasStateBlocks::getStateComposite()
+inline StateComposite HasStateBlocks::getStateComposite()
 {
-    State state;
-    for (auto& pair_key_kf : state_block_map_)
+    StateComposite state;
+    for (auto& pair_key_sb : state_block_map_)
     {
-        state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+        state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
     }
     return state;
 }
 
-inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
+inline void HasStateBlocks::setStateComposite(const StateComposite &_state)
 {
-    StateStructure sub_structure;
-    if (_sub_structure.empty())
-        sub_structure = structure_;
-    else
-        sub_structure = _sub_structure;
-
-    unsigned int size = 0;
-    for (const auto& key : sub_structure)
+    for (const auto& pair_key_sb : _state)
     {
-        const auto& sb = getStateBlock(key);
-        if (!sb){
-            WOLF_ERROR("Stateblock key ", key, " not in the structure");
-        }
-        size += sb->getSize();
+        state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
     }
-
-    return size;
 }
 
 inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
@@ -335,15 +317,35 @@ inline bool HasStateBlocks::stateBlockKey(const StateBlockPtr &_sb, std::string&
     }
 }
 
-inline unsigned int HasStateBlocks::getLocalSize(StateStructure _sub_structure) const
+inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
+{
+    StateStructure structure;
+    if (_sub_structure.empty())
+        structure = structure_;
+    else
+        structure = _sub_structure;
+    unsigned int size = 0;
+    for (const auto& key : structure)
+    {
+        const auto& sb = getStateBlock(key);
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
+        size += sb->getSize();
+    }
+
+    return size;
+}
+
+inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
 {
-    StateStructure sub_structure;
+    StateStructure structure;
     if (_sub_structure.empty())
-        sub_structure = structure_;
+        structure = structure_;
     else
-        sub_structure = _sub_structure;
+        structure = _sub_structure;
     unsigned int size = 0;
-    for (const auto& key : sub_structure)
+    for (const auto& key : structure)
     {
         const auto& sb = getStateBlock(key);
         if (!sb){

src/problem/problem.cpp

@@ -376,6 +376,43 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
         _state = zeroState();
 }
 
+inline const StateComposite& Problem::getStateComposite() const
+{
+    StateComposite state;
+
+    if ( processor_is_motion_list_.empty() ) // Use last estimated frame's state
+    {
+        auto last_kf_or_aux = trajectory_ptr_->getLastKeyOrAuxFrame();
+        if (last_kf_or_aux)
+            state = last_kf_or_aux->getStateComposite();
+        else
+            state = StateComposite();
+    }
+    else // Compose from different processor motion
+    {
+        // get a timestamp where all processor motions have state
+        std::list<TimeStamp> proc_motion_ts;
+        for (const auto& prc : processor_is_motion_list_)
+        {
+            proc_motion_ts.push_back(prc->getCurrentTimeStamp());
+        }
+        auto min_ts = *(std::min_element(proc_motion_ts.begin(), proc_motion_ts.end()));
+
+        // compose the states of all processor motions into one only state
+        for (const auto& prc : processor_is_motion_list_)
+        {
+            for (const auto& pair_key_vec : prc->getStateComposite(min_ts))
+            {
+                if (state.count(pair_key_vec.first) == 0)
+                    state.insert(pair_key_vec);
+            }
+        }
+    }
+
+    return state;
+}
+
+
 
 // Problem of this implmentation: if more state
 void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const