diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 0527407da7354ecc01fda1230a69a93458654fc0..0e9fd3add22c3f9ab379c669b2091c060382df64 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -8,7 +8,7 @@
#ifndef STATE_BLOCK_HAS_STATE_BLOCKS_H_
#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
-#include "core/common/wolf.h"
+//#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
#include <unordered_map>
@@ -16,53 +16,9 @@
namespace wolf
{
-typedef std::unordered_map<std::string, StateBlockPtr> StateBlockMap;
-
-class StateBlockComposite
-{
- public:
- StateBlockComposite() = default;
- virtual ~StateBlockComposite() = default;
-
- const StateBlockMap& getStateBlockMap() const;
-
- // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
- virtual void fix();
- virtual void unfix();
- virtual bool isFixed() const;
-
- virtual StateBlockPtr emplace(const std::string& _sb_type, const StateBlockPtr& _sb);
- virtual unsigned int erase(const std::string& _sb_type);
- virtual unsigned int erase(const char _sb_type);
- unsigned int count(const std::string& _sb_type) const { return state_block_map_.count(_sb_type); }
- unsigned int count(const StateBlockPtr& _sb) const;
- StateBlockPtr at(const std::string& _sb_type) const;
- bool set(const std::string _sb_type, const StateBlockPtr& _sb);
- bool key(const StateBlockPtr& _sb, std::string& _key) const;
- StateBlockMap::const_iterator find(const StateBlockPtr& _sb) const;
-
- // Emplace derived state blocks (angle, quaternion, etc).
- template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplace(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
-
- // Emplace base state blocks.
- template<typename ... Args>
- inline StateBlockPtr emplace(const std::string& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
-
- // Perturb state with noise
- void perturb(double amplitude = 0.01);
-
- // Plus operator
- void plus(const VectorComposite& _dx);
-
- // Get composite of state vectors (not blocks)
- VectorComposite getStateComposite() const;
- bool getStateComposite(VectorComposite& _state) const;
- void setStateComposite(const VectorComposite& _state);
-
- private:
- StateBlockMap state_block_map_;
-};
+typedef std::string StateStructure;
+typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap;
+typedef std::unordered_map < std::string, Eigen::VectorXd > VectorComposite;
class HasStateBlocks
@@ -92,14 +48,14 @@ class HasStateBlocks
virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem) { return addStateBlock(std::string(1,_sb_type), _sb, _problem); }
virtual unsigned int removeStateBlock(const std::string& _sb_type);
virtual unsigned int removeStateBlock(const char _sb_type);
- bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
- bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
- bool hasStateBlock(const StateBlockPtr& _sb) const;
- StateBlockPtr getStateBlock(const std::string& _sb_type) const;
- StateBlockPtr getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); }
- bool setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb);
- bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); }
- bool stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const;
+ bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
+ bool hasStateBlock(const StateBlockPtr& _sb) const;
+ StateBlockPtr getStateBlock(const std::string& _sb_type) const;
+ StateBlockPtr getStateBlock(const char _sb_type) const { return getStateBlock(std::string(1,_sb_type)); }
+ bool setStateBlock(const std::string _sb_type, const StateBlockPtr& _sb);
+ bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return setStateBlock(std::string(1, _sb_type), _sb); }
+ bool stateBlockKey(const StateBlockPtr& _sb, std::string& _key) const;
StateBlockMap::const_iterator find(const StateBlockPtr& _sb) const;
// Emplace derived state blocks (angle, quaternion, etc).
@@ -121,7 +77,7 @@ class HasStateBlocks
void getState(Eigen::VectorXd& _state, const StateStructure& _sub_structure = StateStructure()) const;
Eigen::VectorXd getState(const StateStructure& _sub_structure = StateStructure()) const;
- VectorComposite getStateComposite() const;
+ VectorComposite getStateComposite() const;
bool getStateComposite(VectorComposite& _state) const;
void setStateComposite(const VectorComposite& _state);
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index aa1bfaa4e87fc450c33407e67edc19103beb5f2d..3238fb8ece01ff1f48e4cfcd5b403760a5bb4c2f 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -291,7 +291,7 @@ inline double* StateBlock::getStateData()
inline void StateBlock::setIdentity(bool _notify)
{
- setState( VectorXd::Zero(state_size_), _notify );
+ setState( Eigen::VectorXd::Zero(state_size_), _notify );
}
inline void StateBlock::setZero(bool _notify)
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 4298c7ad2fc9de7245ce0561922c80c2b41e5a1c..267a6e1b23caddfa5f8f210510e6cde2a0d3b99b 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -21,9 +21,56 @@ namespace wolf
using std::string;
using namespace Eigen;
-/// State of nodes containing several state blocks, and Jacobians
typedef std::string StateStructure;
+typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap;
typedef std::unordered_map < std::string, Eigen::VectorXd > VectorComposite;
+typedef StateBlockMap::const_iterator StateBlockMapCIter;
+
+class StateBlockComposite
+{
+ public:
+ StateBlockComposite() = default;
+ virtual ~StateBlockComposite() = default;
+
+ const StateBlockMap& getStateBlockMap() const;
+
+ // These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
+ virtual void fix();
+ virtual void unfix();
+ virtual bool isFixed() const;
+
+ virtual unsigned int remove(const std::string& _sb_type);
+ bool has(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
+ bool has(const StateBlockPtr& _sb) const;
+ StateBlockPtr at(const std::string& _sb_type) const;
+ void set(const std::string _sb_type, const StateBlockPtr& _sb);
+ bool key(const StateBlockPtr& _sb, std::string& _key) const;
+ StateBlockMapCIter find(const StateBlockPtr& _sb) const;
+
+ virtual StateBlockPtr add(const std::string& _sb_type, const StateBlockPtr& _sb);
+
+ // Emplace derived state blocks (angle, quaternion, etc).
+ template<typename SB, typename ... Args>
+ std::shared_ptr<SB> emplace(const std::string& _sb_type, Args&&... _args_of_derived_state_block_constructor);
+
+ // Emplace base state blocks.
+ template<typename ... Args>
+ inline StateBlockPtr emplace(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
+
+ // Perturb state with noise
+ void perturb(double amplitude = 0.01);
+
+ // Plus operator
+ void plus(const VectorComposite& _dx);
+
+ // Get composite of state vectors (not blocks)
+ VectorComposite getVectorComposite() const;
+ bool getVectorComposite(VectorComposite& _state) const;
+ void setVectorComposite(const VectorComposite& _state);
+
+ private:
+ StateBlockMap state_block_map_;
+};
class MatrixComposite
@@ -87,8 +134,32 @@ inline unsigned int MatrixComposite::count(const std::string &_row, const std::s
std::ostream& operator <<(std::ostream &_os, const VectorComposite &_x);
+
+template<typename SB, typename ... Args>
+inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+ Args &&... _args_of_derived_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "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)...);
+
+ add(_sb_type, sb);
+
+ return sb;
}
+template<typename ... Args>
+inline StateBlockPtr wolf::StateBlockComposite::emplace(const std::string &_sb_type,
+ Args &&... _args_of_base_state_block_constructor)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
+
+ add(_sb_type, sb);
+
+ return sb;
+}
+
+}
#endif /* STATE_BLOCK_STATE_COMPOSITE_H_ */
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 2333dddf00249bbfc3f772a18900e9a7387673ec..6fd4cdded28dbfe9fcc7b0e42d976678b25a975a 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -2,6 +2,7 @@
#include "core/state_block/state_composite.h"
+#include "core/state_block/state_block.h"
namespace wolf
{
@@ -146,4 +147,147 @@ std::ostream& operator <<(std::ostream &_os, const VectorComposite &_x)
return _os;
}
+
+const StateBlockMap& StateBlockComposite::getStateBlockMap() const
+{
+ return state_block_map_;
+}
+
+void StateBlockComposite::fix()
+{
+}
+
+void StateBlockComposite::unfix()
+{
+}
+
+bool StateBlockComposite::isFixed() const
+{
+ bool fixed = true;
+ for (auto pair_key_sbp : state_block_map_)
+ {
+ if (pair_key_sbp.second)
+ fixed &= pair_key_sbp.second->isFixed();
+ }
+ return fixed;
+}
+
+unsigned int StateBlockComposite::remove(const std::string &_sb_type)
+{
+ return state_block_map_.erase(_sb_type);
+}
+
+bool StateBlockComposite::has(const StateBlockPtr &_sb) const
+{
+ bool found = false;
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ found = found || (pair_key_sb.second == _sb);
+ }
+ return found;
+}
+
+StateBlockPtr StateBlockComposite::at(const std::string &_sb_type) const
+{
+ auto it = state_block_map_.find(_sb_type);
+ if (it != state_block_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+void StateBlockComposite::set(const std::string _sb_type, const StateBlockPtr &_sb)
+{
+ auto it = state_block_map_.find(_sb_type);
+ assert ( it != state_block_map_.end() && "Cannot set an inexistent state block! Use addStateBlock instead.");
+
+ it->second = _sb;
}
+
+bool StateBlockComposite::key(const StateBlockPtr &_sb, std::string &_key) const
+{
+ const auto& it = this->find(_sb);
+
+ bool found = (it != state_block_map_.end());
+
+ if (found)
+ {
+ _key = it->first;
+ return true;
+ }
+ else
+ {
+ _key = "";
+ return false;
+ }
+}
+
+StateBlockMapCIter StateBlockComposite::find(const StateBlockPtr &_sb) const
+{
+ const auto& it = std::find_if(state_block_map_.begin(),
+ state_block_map_.end(),
+ [_sb](const std::pair<std::string, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+StateBlockPtr StateBlockComposite::add(const std::string &_sb_type, const StateBlockPtr &_sb)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ state_block_map_.emplace(_sb_type, _sb);
+
+ return _sb;
+}
+
+void StateBlockComposite::perturb(double amplitude)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ auto& sb = pair_key_sb.second;
+ if (!sb->isFixed())
+ sb->perturb(amplitude);
+ }
+}
+
+void StateBlockComposite::plus(const VectorComposite &_dx)
+{
+ for (const auto& pair_key_sb : state_block_map_)
+ {
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
+ const auto& dv = _dx.at(key);
+
+ sb->plus(dv);
+ }
+}
+
+VectorComposite StateBlockComposite::getVectorComposite() const
+{
+ VectorComposite v;
+ getVectorComposite(v);
+ return v;
+}
+
+bool StateBlockComposite::getVectorComposite(VectorComposite &_state) const
+{
+ for (auto &pair_key_sb : state_block_map_)
+ {
+ _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ }
+ return true;
+}
+
+void StateBlockComposite::setVectorComposite(const VectorComposite &_state)
+{
+ for (const auto &pair_key_sb : _state)
+ {
+ state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+ }
+}
+
+} // namespace wolf
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 5eebc7e5f2fd2cc4ef7c29074b165784e3a14ff2..d814d0918fab6f1f15b63ae5051ddbc571f7a1a4 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -11,8 +11,10 @@
#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
+#include "core/state_block/state_quaternion.h"
using namespace wolf;
+using namespace std;
/*
// You may use this to make some methods of Foo public
@@ -21,26 +23,110 @@ class FooPublic : public Foo
{
// You may use this to make some methods of Foo public
}
+*/
-class TestInit : public testing::Test
+class StateBlockCompositeInit : public testing::Test
{
- // You may use this to initialize stuff
- // Combine it with TEST_F(FooTest, testName) { }
- SetUp()
- {
- // Init all you want here
- // e.g. FooPublic foo;
- }
- TearDown(){}
+ public:
+
+ StateBlockPtr sbp, sbv, sbx;
+ StateQuaternionPtr sbq;
+
+ StateBlockComposite states;
+
+ void SetUp() override
+ {
+ sbp = states.emplace("P", Vector3d(1,2,3));
+ sbv = states.emplace("V", Vector3d(4,5,6));
+ sbx = std::make_shared<StateBlock>(Vector3d(7,8,9));
+ sbq = states.emplace<StateQuaternion>("Q", Vector4d(.5,.5,.5,.5));
+ }
};
-TEST_F(TestInit, testName)
+TEST_F(StateBlockCompositeInit, emplace)
+{
+ // Emplaces called in SetUp()
+
+ // check 3 elements
+ ASSERT_EQ(states.getStateBlockMap().size(), 3);
+}
+
+TEST_F(StateBlockCompositeInit, has_key)
{
- // Use class TestInit
+ ASSERT_TRUE(states.has("P"));
+ ASSERT_FALSE(states.has("X"));
}
-*/
-using namespace std;
+TEST_F(StateBlockCompositeInit, has_sb)
+{
+ ASSERT_TRUE(states.has(sbp));
+ ASSERT_FALSE(states.has(sbx));
+}
+
+TEST_F(StateBlockCompositeInit, at)
+{
+ ASSERT_EQ(states.at("P"), sbp);
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, set)
+{
+ states.set("P", sbx);
+
+ ASSERT_EQ(states.at("P"), sbx);
+
+ states.set("P", sbp);
+
+ ASSERT_DEATH(states.set("X", sbx), "");
+
+ ASSERT_EQ(states.at("X"), nullptr);
+}
+
+TEST_F(StateBlockCompositeInit, key)
+{
+ std::string key;
+ ASSERT_TRUE(states.key(sbp, key));
+ ASSERT_EQ(key, "P");
+
+ ASSERT_FALSE(states.key(sbx, key));
+ ASSERT_EQ(key, "");
+}
+
+TEST_F(StateBlockCompositeInit, find)
+{
+ auto it = states.find(sbp);
+ ASSERT_NE(it, states.getStateBlockMap().end());
+
+ it = states.find(sbx);
+ ASSERT_EQ(it, states.getStateBlockMap().end());
+}
+
+TEST_F(StateBlockCompositeInit, add)
+{
+ states.add("X", sbx);
+
+ ASSERT_EQ(states.at("X"), sbx);
+}
+
+TEST_F(StateBlockCompositeInit, perturb)
+{
+ ASSERT_TRUE(states.at("P")->getState().isApprox(sbp->getState(), 1e-3));
+ ASSERT_TRUE(states.at("V")->getState().isApprox(sbv->getState(), 1e-3));
+ ASSERT_TRUE(states.at("Q")->getState().isApprox(sbq->getState(), 1e-3));
+
+ states.perturb(0.1);
+
+ // values have moved wrt original
+ ASSERT_FALSE(states.at("P")->getState().isApprox(Vector3d(1,2,3), 1e-3));
+ ASSERT_FALSE(states.at("V")->getState().isApprox(Vector3d(4,5,6), 1e-3));
+ ASSERT_FALSE(states.at("Q")->getState().isApprox(Vector4d(.5,.5,.5,.5), 1e-3));
+}
+
+
+
+
+
TEST(VectorComposite, operatorStream)
{