Skip to content
Snippets Groups Projects
Commit 607dae49 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

WIP Use StateStructure instead of std::string for state structures

parent a13278ce
No related branches found
No related tags found
2 merge requests!358WIP: Resolve "Complete state vector new data structure?",!343WIP: Resolve "Complete state vector new data structure?"
Pipeline #4977 failed
This commit is part of merge request !343. Comments created here will be created in the context of that merge request.
...@@ -81,7 +81,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s ...@@ -81,7 +81,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
const FactorBasePtrList& getConstrainedByList() const; const FactorBasePtrList& getConstrainedByList() const;
// State blocks // State blocks
const std::string& getStructure() const; const StateStructure& getStructure() const;
StateBlockPtr getStateBlock(const std::string& _key) const; StateBlockPtr getStateBlock(const std::string& _key) const;
StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); } StateBlockPtr getStateBlock(const char _key) const { return getStateBlock(std::string(1, _key)); }
StateBlockPtr getSensorP() const; StateBlockPtr getSensorP() const;
......
...@@ -106,6 +106,11 @@ typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmaj ...@@ -106,6 +106,11 @@ typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmaj
namespace wolf { namespace wolf {
/// State of nodes containing several state blocks
typedef std::unordered_map<std::string, Eigen::VectorXd> State;
typedef std::list<std::string> StateStructure;
////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////
/** Check Eigen Matrix sizes, both statically and dynamically /** Check Eigen Matrix sizes, both statically and dynamically
* *
......
...@@ -69,7 +69,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha ...@@ -69,7 +69,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
**/ **/
FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr); FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x); FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
virtual ~FrameBase(); virtual ~FrameBase();
virtual void remove(bool viral_remove_empty_parent=false); virtual void remove(bool viral_remove_empty_parent=false);
......
...@@ -9,6 +9,7 @@ ...@@ -9,6 +9,7 @@
#define PROCESSOR_IS_MOTION_H_ #define PROCESSOR_IS_MOTION_H_
#include "core/common/wolf.h" #include "core/common/wolf.h"
//#include "core/state_block/has_state_blocks.h"
namespace wolf namespace wolf
{ {
...@@ -43,11 +44,11 @@ class IsMotion ...@@ -43,11 +44,11 @@ class IsMotion
TimeStamp getCurrentTimeStamp() const; TimeStamp getCurrentTimeStamp() const;
Eigen::VectorXd getState(const TimeStamp& _ts) const; Eigen::VectorXd getState(const TimeStamp& _ts) const;
std::string getStateStructure(){return state_structure_;}; const StateStructure& getStateStructure(){return state_structure_;};
void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;}; void setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;};
protected: protected:
std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames) StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
}; };
......
...@@ -10,25 +10,26 @@ ...@@ -10,25 +10,26 @@
#include "core/common/wolf.h" #include "core/common/wolf.h"
#include <map> #include <unordered_map>
namespace wolf namespace wolf
{ {
/// State of nodes containing several state blocks ///// State of nodes containing several state blocks
typedef std::unordered_map<std::string, Eigen::VectorXd> State; //typedef std::unordered_map<std::string, Eigen::VectorXd> State;
//typedef std::list<std::string> StateStructure;
class HasStateBlocks class HasStateBlocks
{ {
public: public:
HasStateBlocks(); HasStateBlocks();
HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {} HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
virtual ~HasStateBlocks(); virtual ~HasStateBlocks();
const std::string& getStructure() const { return structure_; } const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; } void appendToStructure(const std::string& _frame_type){ structure_.push_back(_frame_type); }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; } bool isInStructure(const std::string& _sb_type) { return std::find(structure_.begin(), structure_.end(), _sb_type) != structure_.end(); }
const std::map<std::string, StateBlockPtr>& getStateBlockMap() const; const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
std::vector<StateBlockPtr> getStateBlockVec() const; std::vector<StateBlockPtr> getStateBlockVec() const;
...@@ -68,16 +69,16 @@ class HasStateBlocks ...@@ -68,16 +69,16 @@ class HasStateBlocks
void removeStateBlocks(ProblemPtr _problem); void removeStateBlocks(ProblemPtr _problem);
// States // States
inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true); inline void setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure(), const bool _notify = true);
void getState(Eigen::VectorXd& _state, std::string structure="") const; void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
Eigen::VectorXd getState(std::string structure="") const; Eigen::VectorXd getState(const StateStructure& structure = StateStructure()) const;
unsigned int getSize(std::string _sub_structure="") const; unsigned int getSize(const StateStructure& _sub_structure = StateStructure()) const;
unsigned int getLocalSize(std::string _sub_structure="") const; unsigned int getLocalSize(const StateStructure& _sub_structure = StateStructure()) const;
State getStateComposite(); State getStateComposite();
private: private:
std::string structure_; StateStructure structure_;
std::map<std::string, StateBlockPtr> state_block_map_; std::map<std::string, StateBlockPtr> state_block_map_;
}; };
...@@ -92,7 +93,7 @@ class HasStateBlocks ...@@ -92,7 +93,7 @@ class HasStateBlocks
namespace wolf{ namespace wolf{
inline HasStateBlocks::HasStateBlocks() : inline HasStateBlocks::HasStateBlocks() :
structure_(std::string("")), structure_(),
state_block_map_() state_block_map_()
{ {
// //
...@@ -161,7 +162,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty ...@@ -161,7 +162,7 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const std::string& _sb_ty
inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb) inline bool HasStateBlocks::setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb)
{ {
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead."); 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 ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! 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; state_block_map_.at(_sb_type) = _sb;
return true; // success return true; // success
...@@ -221,16 +222,19 @@ inline bool HasStateBlocks::isFixed() const ...@@ -221,16 +222,19 @@ inline bool HasStateBlocks::isFixed() const
return fixed; return fixed;
} }
inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify) inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _sub_structure, const bool _notify)
{ {
if (_sub_structure == ""){ StateStructure sub_structure;
_sub_structure = structure_; if (_sub_structure.empty())
} sub_structure = structure_;
int size = getSize(_sub_structure); else
sub_structure = _sub_structure;
int size = getSize(sub_structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size"); assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0; unsigned int index = 0;
for (const char key : _sub_structure) for (const auto& key : sub_structure)
{ {
const auto& sb = getStateBlock(key); const auto& sb = getStateBlock(key);
if (!sb){ if (!sb){
...@@ -243,15 +247,17 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string ...@@ -243,15 +247,17 @@ inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string
} }
// _sub_structure can be either stateblock structure of the node or a subset of this structure // _sub_structure can be either stateblock structure of the node or a subset of this structure
inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const inline void HasStateBlocks::getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure) const
{ {
if (_sub_structure == ""){ StateStructure sub_structure;
_sub_structure = structure_; if (_sub_structure.empty())
} sub_structure = structure_;
_state.resize(getSize(_sub_structure)); else
sub_structure = _sub_structure;
_state.resize(getSize(sub_structure));
unsigned int index = 0; unsigned int index = 0;
for (const char key : _sub_structure) for (const auto& key : sub_structure)
{ {
const auto& sb = getStateBlock(key); const auto& sb = getStateBlock(key);
if (!sb){ if (!sb){
...@@ -263,7 +269,7 @@ inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_s ...@@ -263,7 +269,7 @@ inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_s
} }
inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_structure) const
{ {
Eigen::VectorXd state; Eigen::VectorXd state;
...@@ -282,13 +288,16 @@ inline State HasStateBlocks::getStateComposite() ...@@ -282,13 +288,16 @@ inline State HasStateBlocks::getStateComposite()
return state; return state;
} }
inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const inline unsigned int HasStateBlocks::getSize(const StateStructure& _sub_structure) const
{ {
if (_sub_structure == ""){ StateStructure sub_structure;
_sub_structure = structure_; if (_sub_structure.empty())
} sub_structure = structure_;
else
sub_structure = _sub_structure;
unsigned int size = 0; unsigned int size = 0;
for (const char key : _sub_structure) for (const auto& key : sub_structure)
{ {
const auto& sb = getStateBlock(key); const auto& sb = getStateBlock(key);
if (!sb){ if (!sb){
...@@ -300,13 +309,15 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const ...@@ -300,13 +309,15 @@ inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
return size; return size;
} }
inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _sub_structure) const
{ {
if (_sub_structure == ""){ StateStructure sub_structure;
_sub_structure = structure_; if (_sub_structure.empty())
} sub_structure = structure_;
else
sub_structure = _sub_structure;
unsigned int size = 0; unsigned int size = 0;
for (const char key : _sub_structure) for (const auto& key : sub_structure)
{ {
const auto& sb = getStateBlock(key); const auto& sb = getStateBlock(key);
if (!sb){ if (!sb){
......
...@@ -14,7 +14,7 @@ CaptureBase::CaptureBase(const std::string& _type, ...@@ -14,7 +14,7 @@ CaptureBase::CaptureBase(const std::string& _type,
StateBlockPtr _o_ptr, StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) : StateBlockPtr _intr_ptr) :
NodeBase("CAPTURE", _type), NodeBase("CAPTURE", _type),
HasStateBlocks(""), HasStateBlocks(StateStructure()),
frame_ptr_(), // nullptr frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr), sensor_ptr_(_sensor_ptr),
calib_size_(0), calib_size_(0),
...@@ -130,7 +130,7 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr) ...@@ -130,7 +130,7 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
constrained_by_list_.remove(_fac_ptr); constrained_by_list_.remove(_fac_ptr);
} }
const std::string& CaptureBase::getStructure() const const StateStructure& CaptureBase::getStructure() const
{ {
if (getSensor()) if (getSensor())
return getSensor()->getStructure(); return getSensor()->getStructure();
......
...@@ -13,7 +13,7 @@ unsigned int FrameBase::frame_id_count_ = 0; ...@@ -13,7 +13,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) : FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
HasStateBlocks(""), HasStateBlocks(StateStructure()),
trajectory_ptr_(), trajectory_ptr_(),
frame_id_(++frame_id_count_), frame_id_(++frame_id_count_),
type_(NON_ESTIMATED), type_(NON_ESTIMATED),
...@@ -35,7 +35,7 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _ ...@@ -35,7 +35,7 @@ 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) : FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
HasStateBlocks(""), HasStateBlocks(StateStructure()),
trajectory_ptr_(), trajectory_ptr_(),
frame_id_(++frame_id_count_), frame_id_(++frame_id_count_),
type_(_tp), type_(_tp),
...@@ -55,7 +55,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr ...@@ -55,7 +55,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::VectorXd& _x) : FrameBase::FrameBase(const StateStructure& _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
NodeBase("FRAME", "Base"), NodeBase("FRAME", "Base"),
HasStateBlocks(_frame_structure), HasStateBlocks(_frame_structure),
trajectory_ptr_(), trajectory_ptr_(),
......
...@@ -14,7 +14,7 @@ unsigned int LandmarkBase::landmark_id_count_ = 0; ...@@ -14,7 +14,7 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) : LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
NodeBase("LANDMARK", _type), NodeBase("LANDMARK", _type),
HasStateBlocks(""), HasStateBlocks(StateStructure()),
map_ptr_(), map_ptr_(),
state_block_vec_(0), // Resize in derived constructors if needed. state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_) landmark_id_(++landmark_id_count_)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment