diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 0e4695e5a8c82ebe74ae1c67c5fc37cfc396ecea..023990a0b5df42cc28c9989b3627a97c14a36d36 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -230,9 +230,12 @@ int main()
if (!pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto sb : lmk->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ for (auto& pair_key_sb : lmk->getStateBlockMap())
+ if (!pair_key_sb.second->isFixed())
+ pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+// for (auto sb : lmk->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index 5bc4c42b94f13f552f498a86f1ad1143d0782c48..1a098cfd9eb2ecf70f69715a7e3629514238d4eb 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -223,9 +223,12 @@ int main()
if (pair_key_sb.second && !pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
for (auto lmk : problem->getMap()->getLandmarkList())
- for (auto sb : lmk->getStateBlockVec())
- if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ for (auto& pair_key_sb : lmk->getStateBlockMap())
+ if (!pair_key_sb.second->isFixed())
+ pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
+// for (auto sb : lmk->getStateBlockVec())
+// if (sb && !sb->isFixed())
+// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 21e5f1d0cd092a15554ef5cec46563a40317417b..d2b75c8a2c0803bceeb97ff4d6f6d0ce120ae5e7 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -11,6 +11,7 @@ class StateBlock;
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
+#include "core/state_block/has_state_blocks.h"
//std includes
@@ -20,7 +21,7 @@ class StateBlock;
namespace wolf {
//class LandmarkBase
-class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
+class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<LandmarkBase>
{
friend FactorBase;
@@ -57,23 +58,23 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
unsigned int id() const;
void setId(unsigned int _id);
- // Fix / unfix
- void fix();
- void unfix();
- bool isFixed() const;
+// // Fix / unfix
+// void fix();
+// void unfix();
+// bool isFixed() const;
// State blocks
- const std::vector<StateBlockPtr>& getStateBlockVec() const;
- std::vector<StateBlockPtr>& getStateBlockVec();
+// const std::vector<StateBlockPtr>& getStateBlockVec() const;
+// std::vector<StateBlockPtr>& getStateBlockVec();
std::vector<StateBlockPtr> getUsedStateBlockVec() const;
- StateBlockPtr getStateBlock(unsigned int _i) const;
- void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
- void setP(const StateBlockPtr _p_ptr);
- void setO(const StateBlockPtr _o_ptr);
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
+// StateBlockPtr getStateBlock(unsigned int _i) const;
+// void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
+// StateBlockPtr getP() const;
+// StateBlockPtr getO() const;
+// void setP(const StateBlockPtr _p_ptr);
+// void setO(const StateBlockPtr _o_ptr);
+// Eigen::VectorXs getState() const;
+// void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
protected:
@@ -156,49 +157,49 @@ inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
return constrained_by_list_;
}
-inline const std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec() const
-{
- return state_block_vec_;
-}
-
-inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
-{
- return state_block_vec_;
-}
-
-inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
-{
- // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- if (_i < state_block_vec_.size())
- return state_block_vec_[_i];
- else
- return nullptr;
-}
-
-inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
-{
- state_block_vec_[_i] = _sb_ptr;
-}
-
-inline StateBlockPtr LandmarkBase::getP() const
-{
- return getStateBlock(0);
-}
-
-inline StateBlockPtr LandmarkBase::getO() const
-{
- return getStateBlock(1);
-}
-
-inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
-{
- setStateBlock(0, _st_ptr);
-}
-
-inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
-{
- setStateBlock(1, _st_ptr);
-}
+//inline const std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec() const
+//{
+// return state_block_vec_;
+//}
+//
+//inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
+//{
+// return state_block_vec_;
+//}
+//
+//inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
+//{
+// // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
+// if (_i < state_block_vec_.size())
+// return state_block_vec_[_i];
+// else
+// return nullptr;
+//}
+//
+//inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
+//{
+// state_block_vec_[_i] = _sb_ptr;
+//}
+//
+//inline StateBlockPtr LandmarkBase::getP() const
+//{
+// return getStateBlock(0);
+//}
+//
+//inline StateBlockPtr LandmarkBase::getO() const
+//{
+// return getStateBlock(1);
+//}
+//
+//inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
+//{
+// setStateBlock(0, _st_ptr);
+//}
+//
+//inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
+//{
+// setStateBlock(1, _st_ptr);
+//}
inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
{
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 20e32da0a4e7d5d03a4475cb7ed37e54a7865311..386779f1e0165cf6f744ecdac9f34bbf90d073ea 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -14,15 +14,21 @@ unsigned int LandmarkBase::landmark_id_count_ = 0;
LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
NodeBase("LANDMARK", _type),
+ HasStateBlocks(""),
map_ptr_(),
state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
- state_block_vec_[0] = _p_ptr;
+ if (_p_ptr)
+ {
+ setStateBlock("P", _p_ptr);
+ appendToStructure("P");
+ }
if (_o_ptr)
- state_block_vec_[1] = _o_ptr;
- else
- state_block_vec_.resize(1);
+ {
+ setStateBlock("O", _o_ptr);
+ appendToStructure("O");
+ }
}
@@ -54,30 +60,30 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-void LandmarkBase::fix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->fix();
-}
-
-void LandmarkBase::unfix()
-{
- for (auto sbp : state_block_vec_)
- if (sbp != nullptr)
- sbp->unfix();
-}
-
-bool LandmarkBase::isFixed() const
-{
- bool fixed = true;
- for (auto sb : getStateBlockVec())
- {
- if (sb)
- fixed &= sb->isFixed();
- }
- return fixed;
-}
+//void LandmarkBase::fix()
+//{
+// for (auto sbp : state_block_vec_)
+// if (sbp != nullptr)
+// sbp->fix();
+//}
+//
+//void LandmarkBase::unfix()
+//{
+// for (auto sbp : state_block_vec_)
+// if (sbp != nullptr)
+// sbp->unfix();
+//}
+//
+//bool LandmarkBase::isFixed() const
+//{
+// bool fixed = true;
+// for (auto sb : getStateBlockVec())
+// {
+// if (sb)
+// fixed &= sb->isFixed();
+// }
+// return fixed;
+//}
std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
{
@@ -92,9 +98,9 @@ void LandmarkBase::registerNewStateBlocks()
{
if (getProblem() != nullptr)
{
- for (auto sbp : getStateBlockVec())
- if (sbp != nullptr)
- getProblem()->notifyStateBlock(sbp,ADD);
+ for (auto pair_key_sbp : getStateBlockMap())
+ if (pair_key_sbp.second != nullptr)
+ getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
}
}
@@ -105,46 +111,46 @@ bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
void LandmarkBase::removeStateBlocks()
{
- for (unsigned int i = 0; i < state_block_vec_.size(); i++)
+ for (const char& key : getStructure()) // note: key is a char
{
- auto sbp = getStateBlock(i);
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
- setStateBlock(i, nullptr);
}
+ removeStateBlock(key);
}
}
-Eigen::VectorXs LandmarkBase::getState() const
-{
- Eigen::VectorXs state;
-
- getState(state);
-
- return state;
-}
-
-void LandmarkBase::getState(Eigen::VectorXs& _state) const
-{
- SizeEigen size = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- size += sb->getSize();
-
- _state = Eigen::VectorXs(size);
-
- SizeEigen index = 0;
- for (StateBlockPtr sb : state_block_vec_)
- if (sb)
- {
- _state.segment(index,sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
-}
+//Eigen::VectorXs LandmarkBase::getState() const
+//{
+// Eigen::VectorXs state;
+//
+// getState(state);
+//
+// return state;
+//}
+//
+//void LandmarkBase::getState(Eigen::VectorXs& _state) const
+//{
+// SizeEigen size = 0;
+// for (StateBlockPtr sb : state_block_vec_)
+// if (sb)
+// size += sb->getSize();
+//
+// _state = Eigen::VectorXs(size);
+//
+// SizeEigen index = 0;
+// for (StateBlockPtr sb : state_block_vec_)
+// if (sb)
+// {
+// _state.segment(index,sb->getSize()) = sb->getState();
+// index += sb->getSize();
+// }
+//}
YAML::Node LandmarkBase::saveToYaml() const
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 987c6152e2958f621da2e19589b1efa68cab3afa..d7f05b972bd494807e70d975afc512a0670ee16a 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -666,38 +666,63 @@ bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov) const
bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) const
{
bool success(true);
- int i = 0, j = 0;
-
- const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
-
- // computing size
- SizeEigen sz = 0;
- for (const auto& sb : state_bloc_vec)
- if (sb)
- sz += sb->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ SizeEigen sz = _landmark_ptr->getLocalSize();
+ _covariance.resize(sz, sz);
// filling covariance
-
- for (const auto& sb_i : state_bloc_vec)
+ int i = 0, j = 0;
+ for (const auto& key_i : _landmark_ptr->getStructure())
{
- if (sb_i)
+ const auto& sb_i = _landmark_ptr->getStateBlock(key_i);
+ j = 0;
+ for (const auto& key_j : _landmark_ptr->getStructure())
{
- j = 0;
- for (const auto& sb_j : state_bloc_vec)
- {
- if (sb_j)
- {
- success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
- j += sb_j->getLocalSize();
- }
- }
- i += sb_i->getLocalSize();
+ const auto& sb_j = _landmark_ptr->getStateBlock(key_j);
+ success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+ j += sb_j->getLocalSize();
}
+ i += sb_i->getLocalSize();
}
+
return success;
+
+
+
+// bool success(true);
+// int i = 0, j = 0;
+//
+// const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
+//
+// // computing size
+// SizeEigen sz = 0;
+// for (const auto& sb : state_bloc_vec)
+// if (sb)
+// sz += sb->getLocalSize();
+//
+// // resizing
+// _covariance = Eigen::MatrixXs(sz, sz);
+//
+// // filling covariance
+//
+// for (const auto& sb_i : state_bloc_vec)
+// {
+// if (sb_i)
+// {
+// j = 0;
+// for (const auto& sb_j : state_bloc_vec)
+// {
+// if (sb_j)
+// {
+// success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
+// j += sb_j->getLocalSize();
+// }
+// }
+// i += sb_i->getLocalSize();
+// }
+// }
+// return success;
}
MapBasePtr Problem::getMap() const
@@ -1040,9 +1065,15 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
if (state_blocks)
{
cout << " sb:";
- for (auto sb : L->getStateBlockVec())
+ for (const auto& key : L->getStructure())
+ {
+ const auto& sb = L->getStateBlock(key);
if (sb != nullptr)
cout << (sb->isFixed() ? " Fix" : " Est");
+ }
+// for (auto sb : L->getStateBlockVec())
+// if (sb != nullptr)
+// cout << (sb->isFixed() ? " Fix" : " Est");
cout << endl;
}
} // for L
@@ -1364,7 +1395,8 @@ bool Problem::check(int verbose_level) const
}
// find in constrained landmark
if (Lo)
- found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
+ found = found || (std::find_if(Lo->getStateBlockMap().begin(), Lo->getStateBlockMap().end(), [sb](const std::pair<std::string, StateBlockPtr> & t)->bool {return t.second == sb;}) != Lo->getStateBlockMap().end());
+// found = found || (std::find(Lo->getStateBlockVec().begin(), Lo->getStateBlockVec().end(), sb) != Lo->getStateBlockVec().end());
if (verbose_level > 0)
{
if (found)
@@ -1397,8 +1429,12 @@ bool Problem::check(int verbose_level) const
cout << " L" << L->id() << " @ " << L.get() << endl;
cout << " -> P @ " << L->getProblem().get() << endl;
cout << " -> M @ " << L->getMap().get() << endl;
- for (auto sb : L->getStateBlockVec())
+ for (const auto& key : L->getStructure())
{
+ const auto& sb = L->getStateBlock(key);
+// }
+// for (auto sb : L->getStateBlockVec())
+// {
cout << " sb @ " << sb.get();
if (sb)
{