diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h index b533eb686740ee2db8c8643fb4356c71d96d39c3..1f996181554d453ce17042dc6337f4a10312f737 100644 --- a/include/core/state_block/state_composite.h +++ b/include/core/state_block/state_composite.h @@ -26,6 +26,17 @@ typedef std::unordered_map < std::string, StateBlockPtr > StateBlockMap; typedef std::unordered_map < std::string, Eigen::VectorXd > VectorComposite; typedef StateBlockMap::const_iterator StateBlockMapCIter; +typedef std::unordered_map < std::string, Eigen::VectorXd > VectorComposite; + +} + +namespace Eigen +{ +std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x); +} + +namespace wolf{ + class StateBlockComposite { public: @@ -139,8 +150,6 @@ inline unsigned int MatrixComposite::count(const std::string &_row, const std::s return (matrix_composite_.at(_row).count(_col)); } -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, diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp index b11d911fd3e32f273fd3d2dfb30fd72b68df78ee..941e9376f4d555b12f1b204481188b9c7a1d1f12 100644 --- a/src/state_block/state_composite.cpp +++ b/src/state_block/state_composite.cpp @@ -4,6 +4,19 @@ #include "core/state_block/state_composite.h" #include "core/state_block/state_block.h" +namespace Eigen{ +std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x) +{ + for (const auto &pair_key_vec : _x) + { + const auto &key = pair_key_vec.first; + const auto &vec = pair_key_vec.second; + _os << "\n block(" << key << ") = \n" << vec; + } + return _os; +} +} + namespace wolf { @@ -135,18 +148,6 @@ std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M) return _os; } -std::ostream& operator <<(std::ostream &_os, const VectorComposite &_x) -{ - for (const auto &pair_row_blk : _x) - { - const auto &row = pair_row_blk.first; - const auto &vec = pair_row_blk.second; - - _os << "\n block(" << row << ") = \n" << vec; - } - return _os; -} - const StateBlockMap& StateBlockComposite::getStateBlockMap() const { diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp index ce6897eaa0c0ec175605883a7b49eb696aaaaf73..2a74e9911fed25fd5ca45b30313d964a8b0a9cea 100644 --- a/test/gtest_state_composite.cpp +++ b/test/gtest_state_composite.cpp @@ -5,14 +5,11 @@ * Author: jsola */ - -#include "core/utils/utils_gtest.h" -#include "core/utils/logging.h" - -#include "core/common/wolf.h" #include "core/state_block/state_composite.h" #include "core/state_block/state_quaternion.h" +#include "core/utils/utils_gtest.h" + using namespace wolf; using namespace std; @@ -215,6 +212,8 @@ TEST(VectorComposite, operatorStream) cout << "x = " << x << endl; + WOLF_DEBUG("X = ", x); + } TEST(MatrixComposite, emplace_operatorStream) @@ -330,7 +329,9 @@ TEST(MatrixComposite, productVector) x.emplace("P", Vector2d(1,1)); x.emplace("O", Vector3d(2,2,2)); - WOLF_DEBUG("x = " , x); + cout << "x= " << x << endl; + +// WOLF_DEBUG("x = " , x); MatrixComposite M; @@ -350,7 +351,7 @@ TEST(MatrixComposite, productVector) y = M * x; - WOLF_DEBUG("y = M * x = " , y); +// WOLF_DEBUG("y = M * x = " , y); /* M * x = y * p o