WIP getStateComposite in Prob, IsMot and ProcMot

include/core/problem/problem.h

@@ -44,8 +44,10 @@ class Problem : public std::enable_shared_from_this<Problem>
 
     // TODO move somewhere below
     public:
-        StateComposite  getStateComposite() const;
-        StateComposite  getStateComposite(const TimeStamp& _ts) const;
+    bool  getStateComposite(StateComposite& _state) const;
+    bool  getStateComposite(const TimeStamp& _ts, StateComposite& _state) const;
+    StateComposite  getStateComposite() const;
+    StateComposite  getStateComposite(const TimeStamp& _ts) const;
 
 
 

include/core/processor/is_motion.h

@@ -23,9 +23,8 @@ class IsMotion
     public:
 
         // TODO move somewhere below
-        virtual bool  getStateComposite(StateComposite& _state) const = 0;
-        virtual bool  getStateComposite(const TimeStamp& _ts, StateComposite& _state) const = 0;
-
+        virtual bool    getStateComposite(StateComposite& _state) const = 0;
+        virtual bool    getStateComposite(const TimeStamp& _ts, StateComposite& _state) const = 0;
         StateComposite  getStateComposite() const;
         StateComposite  getStateComposite(const TimeStamp& _ts) const;
 
@@ -85,6 +84,13 @@ inline TimeStamp IsMotion::getCurrentTimeStamp() const
     return ts;
 }
 
+inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
+{
+    Eigen::VectorXd x;
+    getState(_ts, x);
+    return x;
+}
+
 inline StateComposite IsMotion::getStateComposite() const
 {
     StateComposite state;
@@ -99,13 +105,6 @@ inline StateComposite IsMotion::getStateComposite(const TimeStamp &_ts) const
     return state;
 }
 
-inline Eigen::VectorXd IsMotion::getState(const TimeStamp& _ts) const
-{
-    Eigen::VectorXd x;
-    getState(_ts, x);
-    return x;
-}
-
 
 
 } /* namespace wolf */

include/core/state_block/has_state_blocks.h

@@ -72,7 +72,8 @@ class HasStateBlocks
         void            getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
         Eigen::VectorXd getState(const StateStructure& _sub_structure = StateStructure()) const;
 
-        StateComposite  getStateComposite();
+        StateComposite  getStateComposite() const;
+        bool            getStateComposite(StateComposite& _state) const;
         void            setStateComposite(const StateComposite& _state);
 
         // Perturb state
@@ -261,13 +262,19 @@ inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_struc
     return state;
 }
 
-inline StateComposite HasStateBlocks::getStateComposite()
+inline bool HasStateBlocks::getStateComposite(StateComposite& _state) const
 {
-    StateComposite state;
     for (auto& pair_key_sb : state_block_map_)
     {
-        state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+        _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
     }
+    return true;
+}
+
+inline StateComposite HasStateBlocks::getStateComposite() const
+{
+    StateComposite state;
+    getStateComposite(state);
     return state;
 }
 

src/problem/problem.cpp

@@ -376,17 +376,15 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& _ts) c
         _state = zeroState();
 }
 
-inline StateComposite Problem::getStateComposite() const
+inline bool Problem::getStateComposite(StateComposite& _state) 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();
+            _state = last_kf_or_aux->getStateComposite();
         else
-            state = StateComposite();
+            _state = StateComposite();
     }
     else // Compose from different processor motion
     {
@@ -403,13 +401,13 @@ inline StateComposite Problem::getStateComposite() const
         {
             for (const auto& pair_key_vec : prc->getStateComposite(min_ts))
             {
-                if (state.count(pair_key_vec.first) == 0)
-                    state.insert(pair_key_vec);
+                if (_state.count(pair_key_vec.first) == 0)
+                    _state.insert(pair_key_vec);
             }
         }
     }
 
-    return state;
+    return true;
 }
 
 

src/processor/processor_motion.cpp

@@ -398,6 +398,25 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
     return true;
 }
 
+bool ProcessorMotion::getStateComposite(StateComposite& _state) const
+{
+    // TODO implement this
+    if (last_ptr_ && last_ptr_->getFrame())
+        return getLast()->getFrame()->getStateComposite(_state);
+    else
+        return false;
+}
+
+bool ProcessorMotion::getStateComposite(const TimeStamp& _ts, StateComposite& _state) const
+{
+    // TODO implement this
+    if (last_ptr_ && last_ptr_->getFrame())
+        return getLast()->getFrame()->getStateComposite(_state);
+    else
+        return false;
+}
+
+
 FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin)
 {
     FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);