diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 6587cf5ea242396a40f29feb07bc0828271d8f4a..3c0593f670d2a8cbf5ce4418734b8fd571766407 100644
--- a/include/core/common/wolf.h
+++ b/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;
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index edda0b3b59fa839cdb6c5156abf61e1f2c338174..219668403aaaf2dc5db0f39398faded44bdf1cb7 100644
--- a/include/core/problem/problem.h
+++ b/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);
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index f9f77b10cfb4382cb849c9c9d51cb8698c389f78..8655f4d78ecd84313f6bd1a8795e63fad525ea48 100644
--- a/include/core/processor/is_motion.h
+++ b/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;};
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 815149b3e914bc48ec9df794a0b0716019eead1f..c7809b8e0ea301c50790c567412d8ceb9700c3a5 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/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){
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 747737dc1ff0ea4b742a97017065d229889ea7d6..7788f8419164daec510776518452839ff9046d3a 100644
--- a/src/problem/problem.cpp
+++ b/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