diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 6602a4e9cf24245df8febd5abf56dd6785e285fb..482dff8644d9ade38612bfd29cab879e63955275 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -339,7 +339,7 @@ inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBloc
inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic)
{
- assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
+ assert(has(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
state_block_dynamic_[_key] = _dynamic;
}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index a5c97899f48605e40c866ddca9256f99f8668a2e..06341d5a8d24fb790be63ddc5aeb2c59567b4559 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -38,19 +38,17 @@ class HasStateBlocks
public:
HasStateBlocks();
- HasStateBlocks(const StateKeys& _keys) : keys_order_(_keys), state_block_map_() {}
HasStateBlocks(const SpecComposite& _specs);
HasStateBlocks(const TypeComposite& _types, const VectorComposite& _vectors);
virtual ~HasStateBlocks();
- StateKeys getKeys() const { return keys_order_; }
+ StateKeys getKeys() const { return state_block_composite_.getKeys(); }
SpecComposite getSpecs() const;
- void appendToStructure(const char& _new_key){ keys_order_ += _new_key; }
- bool hasKey(const char& _sb_key) const { return keys_order_.find(_sb_key) != std::string::npos; }
- bool hasKeys(const std::string& _keys) const;
+ bool has(const char& _sb_key) const { return state_block_composite_.has(_sb_key); }
+ bool has(const std::string& _keys) const { return state_block_composite_.has(_keys); }
- std::unordered_map<char, StateBlockConstPtr> getStateBlockMap() const;
- std::unordered_map<char, StateBlockPtr> getStateBlockMap();
+ std::map<char, StateBlockConstPtr> getStateBlockMap() const;
+ std::map<char, StateBlockPtr> getStateBlockMap();
std::vector<StateBlockConstPtr> getStateBlockVec() const;
std::vector<StateBlockPtr> getStateBlockVec();
@@ -67,7 +65,6 @@ class HasStateBlocks
virtual StateBlockPtr addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem);
virtual unsigned int removeStateBlock(const char& _sb_key);
- bool hasStateBlock(const char& _sb_key) const { return state_block_map_.count(_sb_key) > 0; }
bool hasStateBlock(const StateBlockConstPtr& _sb) const;
bool setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
@@ -103,8 +100,7 @@ class HasStateBlocks
void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
private:
- StateKeys keys_order_;
- std::unordered_map<char, StateBlockPtr> state_block_map_;
+ Composite<StateBlockPtr> state_block_composite_;
};
@@ -118,8 +114,7 @@ class HasStateBlocks
namespace wolf{
inline HasStateBlocks::HasStateBlocks() :
- keys_order_(std::string("")),
- state_block_map_()
+ state_block_composite_()
{
//
}
@@ -129,23 +124,23 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline std::unordered_map<char, StateBlockConstPtr> HasStateBlocks::getStateBlockMap() const
+inline std::map<char, StateBlockConstPtr> HasStateBlocks::getStateBlockMap() const
{
- std::unordered_map<char, StateBlockConstPtr> map_const;
- for (auto&& pair : state_block_map_)
+ std::map<char, StateBlockConstPtr> map_const;
+ for (auto&& pair : state_block_composite_)
map_const[pair.first] = pair.second;
return map_const;
}
-inline std::unordered_map<char, StateBlockPtr> HasStateBlocks::getStateBlockMap()
+inline std::map<char, StateBlockPtr> HasStateBlocks::getStateBlockMap()
{
- return state_block_map_;
+ return state_block_composite_;
}
inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
{
std::vector<StateBlockConstPtr> sbv;
- for (auto& key : keys_order_)
+ for (auto key : getKeys())
{
sbv.push_back(getStateBlock(key));
}
@@ -155,7 +150,7 @@ inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
{
std::vector<StateBlockPtr> sbv;
- for (auto& key : keys_order_)
+ for (auto key : getKeys())
{
sbv.push_back(getStateBlock(key));
}
@@ -164,13 +159,13 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_key)
{
- return state_block_map_.erase(_sb_key);
+ return state_block_composite_.erase(_sb_key);
}
template<typename SB, typename ... Args>
inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
- assert(state_block_map_.count(_sb_key) == 0 and "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_composite_.count(_sb_key) == 0 and "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
addStateBlock(_sb_key, sb, _problem);
@@ -180,16 +175,16 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_key
inline bool HasStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
{
- assert (keys_order_.find(_sb_key) > 0 and "Cannot set a state block out of the state keys! Use addStateBlock instead.");
- assert ( state_block_map_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead.");
- state_block_map_.at(_sb_key) = _sb;
+ assert (has(_sb_key) and "Cannot set a state block out of the state keys! Use addStateBlock instead.");
+ assert (state_block_composite_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead.");
+ state_block_composite_.at(_sb_key) = _sb;
return true; // success
}
inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_key) const
{
- auto it = state_block_map_.find(_sb_key);
- if (it != state_block_map_.end())
+ auto it = state_block_composite_.find(_sb_key);
+ if (it != state_block_composite_.end())
return it->second;
else
return nullptr;
@@ -197,8 +192,8 @@ inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_ke
inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_key)
{
- auto it = state_block_map_.find(_sb_key);
- if (it != state_block_map_.end())
+ auto it = state_block_composite_.find(_sb_key);
+ if (it != state_block_composite_.end())
return it->second;
else
return nullptr;
@@ -226,14 +221,14 @@ inline wolf::StateBlockPtr HasStateBlocks::getO()
inline void HasStateBlocks::fix()
{
- for (auto pair_key_sbp : state_block_map_)
+ for (auto pair_key_sbp : state_block_composite_)
if (pair_key_sbp.second != nullptr)
pair_key_sbp.second->fix();
}
inline void HasStateBlocks::unfix()
{
- for (auto pair_key_sbp : state_block_map_)
+ for (auto pair_key_sbp : state_block_composite_)
if (pair_key_sbp.second != nullptr)
pair_key_sbp.second->unfix();
}
@@ -241,7 +236,7 @@ inline void HasStateBlocks::unfix()
inline bool HasStateBlocks::isFixed() const
{
bool fixed = true;
- for (auto pair_key_sbp : state_block_map_)
+ for (auto pair_key_sbp : state_block_composite_)
{
if (pair_key_sbp.second)
fixed &= pair_key_sbp.second->isFixed();
@@ -266,17 +261,11 @@ inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _
inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify)
{
- StateKeys keys;
- if (_keys == "")
- keys = keys_order_;
- else
- keys = _keys;
-
- int size = getSize(keys);
+ int size = getSize(_keys);
assert(_state.size() == size and "In FrameBase::setState wrong state size");
unsigned int index = 0;
- for (const char key : keys)
+ for (const char key : _keys)
{
const auto& sb = getStateBlock(key);
assert (sb and "Stateblock key not in the keys");
@@ -325,16 +314,10 @@ inline void HasStateBlocks::setState(const StateKeys& _keys, const std::list<Vec
//// _keys can be either stateblock keys of the node or a subset of this keys
inline VectorXd HasStateBlocks::getStateVector(const StateKeys& _keys) const
{
- StateKeys keys;
- if (_keys == "")
- keys = keys_order_;
- else
- keys = _keys;
-
- VectorXd state(getSize(keys));
+ VectorXd state(getSize(_keys));
unsigned int index = 0;
- for (const char key : keys)
+ for (const char key : _keys)
{
const auto& sb = getStateBlock(key);
@@ -348,15 +331,15 @@ inline VectorXd HasStateBlocks::getStateVector(const StateKeys& _keys) const
inline VectorComposite HasStateBlocks::getState(const StateKeys& _keys) const
{
- const StateKeys& keys = (_keys == "" ? keys_order_ : _keys);
+ const StateKeys& keys = (_keys == "" ? getKeys() : _keys);
VectorComposite state;
for (const auto key : keys)
{
- auto state_it = state_block_map_.find(key);
+ auto state_it = state_block_composite_.find(key);
- if (state_it != state_block_map_.end())
+ if (state_it != state_block_composite_.end())
state.emplace(key, state_it->second->getState());
}
@@ -366,7 +349,7 @@ inline VectorComposite HasStateBlocks::getState(const StateKeys& _keys) const
inline unsigned int HasStateBlocks::getSize(const StateKeys& _keys) const
{
- const StateKeys& keys = (_keys == "" ? keys_order_ : _keys);
+ const StateKeys& keys = (_keys == "" ? getKeys() : _keys);
unsigned int size = 0;
for (const char key : keys)
@@ -381,42 +364,30 @@ inline unsigned int HasStateBlocks::getSize(const StateKeys& _keys) const
return size;
}
-inline bool HasStateBlocks::hasKeys(const std::string& _keys) const
-{
- for (auto key : _keys)
- {
- if (not hasKey(key))
- {
- return false;
- }
- }
- return true;
-}
-
inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
{
- const auto& it = std::find_if(state_block_map_.begin(),
- state_block_map_.end(),
+ const auto& it = std::find_if(state_block_composite_.begin(),
+ state_block_composite_.end(),
[_sb](const std::pair<char, StateBlockPtr>& pair)
{
return pair.second == _sb;
}
);
- return it != state_block_map_.end();
+ return it != state_block_composite_.end();
}
inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
{
- const auto& it = std::find_if(state_block_map_.begin(),
- state_block_map_.end(),
+ const auto& it = std::find_if(state_block_composite_.begin(),
+ state_block_composite_.end(),
[_sb](const std::pair<char, StateBlockPtr>& pair)
{
return pair.second == _sb;
}
);
- if (it != state_block_map_.end())
+ if (it != state_block_composite_.end())
{
_key = it->first;
return true;
@@ -432,7 +403,7 @@ inline unsigned int HasStateBlocks::getLocalSize(const StateKeys& _keys) const
{
StateKeys keys;
if (_keys == "")
- keys = keys_order_;
+ keys = getKeys();
else
keys = _keys;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 0eb2cc216394aaff927df49cdfaeb8ca77f38a40..4257c9f834eb0c0bf4ac9d339500a96ff224f4cc 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -35,7 +35,7 @@ CaptureBase::CaptureBase(const std::string& _type,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr) :
NodeBase("CAPTURE", _type),
- HasStateBlocks(""),
+ HasStateBlocks(),
frame_ptr_(), // nullptr
sensor_ptr_(_sensor_ptr),
capture_id_(++capture_id_count_),
@@ -194,7 +194,7 @@ bool CaptureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
{
- if (getSensor() and getSensor()->hasStateBlock(_key))
+ if (getSensor() and getSensor()->has(_key))
{
if (getSensor()->isStateBlockDynamic(_key))
return HasStateBlocks::getStateBlock(_key);
@@ -207,7 +207,7 @@ StateBlockConstPtr CaptureBase::getStateBlock(const char& _key) const
StateBlockPtr CaptureBase::getStateBlock(const char& _key)
{
- if (getSensor() and getSensor()->hasStateBlock(_key))
+ if (getSensor() and getSensor()->has(_key))
{
if (getSensor()->isStateBlockDynamic(_key))
return HasStateBlocks::getStateBlock(_key);
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 83368de1ca47b17f139872eed0cce274d7f69f8a..7836a9f894ccfe0210bba2336f058f8555c71ede 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -251,9 +251,9 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
while (frame)
{
- if (frame->hasStateBlock('P') and
+ if (frame->has('P') and
isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
- frame->hasStateBlock('V') and
+ frame->has('V') and
isStateBlockRegisteredDerived(frame->getStateBlock('V')))
{
break;
@@ -298,17 +298,17 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
{
// get capture gnss
for (CaptureBasePtr cap : frame->getCaptureList())
- if (cap->hasStateBlock('T'))
+ if (cap->has('T'))
{
sb_gnss_T = cap->getStateBlock('T');
break;
}
- if (frame->hasStateBlock('P') and
+ if (frame->has('P') and
isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
- frame->hasStateBlock('O') and
+ frame->has('O') and
isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
- frame->hasStateBlock('V') and
+ frame->has('V') and
isStateBlockRegisteredDerived(frame->getStateBlock('V')) and
sb_gnss_T and
isStateBlockRegisteredDerived(sb_gnss_T))
@@ -364,13 +364,13 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
{
// get capture gnss
for (CaptureBasePtr cap : frame->getCaptureList())
- if (cap->hasStateBlock('T'))
+ if (cap->has('T'))
{
sb_gnss_T = cap->getStateBlock('T');
break;
}
- if (frame->hasStateBlock('P') and
+ if (frame->has('P') and
isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
sb_gnss_T and
isStateBlockRegisteredDerived(sb_gnss_T))
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index b4c0ffb24c2bfd3c44ab43f1c2df1c5199d63ca3..9819bce8c6d484368d78581b58f3aefa3ac72ba9 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -38,7 +38,7 @@ FrameBase::FrameBase(const TimeStamp& _ts,
StateBlockPtr _o_ptr,
StateBlockPtr _v_ptr) :
NodeBase("FRAME", "FrameBase"),
- HasStateBlocks(""),
+ HasStateBlocks(),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index f697d7482aeab3efa3ec51717c26a6dde47d9e5a..3db9d6dbdc7b2aa7a1d20e7b8c6734303855c75f 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -36,7 +36,7 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
NodeBase("LANDMARK", _type),
- HasStateBlocks(""),
+ HasStateBlocks(),
map_ptr_(),
landmark_id_(++landmark_id_count_)
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d5139bd359312bdff6cf98a742cb45eae4eed1eb..9097c1ef97cd82b1dfe15c89fd850c650510b0f6 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1000,7 +1000,7 @@ bool Problem::getCovariance(HasStateBlocksConstPtr _has_blocks, const StateKeys&
WOLF_WARN("Problem::getCovariance: called with a nullptr");
return false;
}
- if (not _has_blocks->hasKeys(_keys))
+ if (not _has_blocks->has(_keys))
{
WOLF_WARN(" does not have the keys ", _keys, ". Available keys: ", _has_blocks->getKeys());
return false;
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 65dd33ac78fa171d52b4370dd82c67c382f0daeb..d47ea9b91149949c0f94dfa757746deec525982c 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -274,7 +274,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = getState(timestamp_from_callback);
for (auto pair_key_vec : proc_state)
- if (!keyframe_from_callback->hasKey(pair_key_vec.first))
+ if (!keyframe_from_callback->has(pair_key_vec.first))
keyframe_from_callback->addStateBlock(pair_key_vec.first,
FactoryStateBlock::create(std::string(1, pair_key_vec.first),
pair_key_vec.second,
@@ -365,7 +365,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// update KF state (adding missing StateBlocks)
auto proc_state = this->getState(timestamp_from_callback);
for (auto pair_key_vector : proc_state)
- if (!keyframe_from_callback->hasKey(pair_key_vector.first))
+ if (!keyframe_from_callback->has(pair_key_vector.first))
keyframe_from_callback->addStateBlock(pair_key_vector.first,
FactoryStateBlock::create(std::string(1, pair_key_vector.first),
pair_key_vector.second,
@@ -418,7 +418,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->getFrame()->setTimeStamp( ts );
VectorComposite state_propa = getState( ts );
for (auto & pair_key_vec : state_propa)
- if (!last_ptr_->getFrame()->hasKey(pair_key_vec.first))
+ if (!last_ptr_->getFrame()->has(pair_key_vec.first))
last_ptr_->getFrame()->addStateBlock(pair_key_vec.first,
FactoryStateBlock::create(std::string(1, pair_key_vec.first),
pair_key_vec.second,
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 7a32e9908556445c5a437676861c21aed7a96d90..082ef79a609e59d06a2133c93149ce2f34576065 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -96,7 +96,7 @@ HasStateBlocks::HasStateBlocks(const TypeComposite& _types, const VectorComposit
SpecComposite HasStateBlocks::getSpecs() const
{
SpecComposite specs;
- for (auto && state_pair : state_block_map_)
+ for (auto && state_pair : state_block_composite_)
{
specs.emplace(state_pair.first, SpecState(state_pair.second->getType(),
state_pair.second->getState(),
@@ -107,12 +107,10 @@ SpecComposite HasStateBlocks::getSpecs() const
StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem)
{
- assert(state_block_map_.count(_sb_key) == 0 and
+ assert(state_block_composite_.count(_sb_key) == 0 and
"Trying to add a state block with an existing type! Use setStateBlock instead.");
- state_block_map_.emplace(_sb_key, _sb);
- if (not hasKey(_sb_key))
- appendToStructure(_sb_key);
+ state_block_composite_.emplace(_sb_key, _sb);
// conditionally register to problem
if(_problem)
@@ -149,7 +147,7 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
void HasStateBlocks::perturb(double amplitude)
{
- for (const auto& pair_key_sb : state_block_map_)
+ for (const auto& pair_key_sb : state_block_composite_)
{
auto& sb = pair_key_sb.second;
if (!sb->isFixed())
@@ -159,7 +157,7 @@ void HasStateBlocks::perturb(double amplitude)
void HasStateBlocks::transform(const VectorComposite& _transformation)
{
- for (auto& pair_key_sb : state_block_map_)
+ for (auto& pair_key_sb : state_block_composite_)
pair_key_sb.second->transform(_transformation);
}