Complete class StateBlockComposite, with tests

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);
 

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)

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_ */

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

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)
 {