diff --git a/CMakeLists.txt b/CMakeLists.txt
index 98de6dbd9bfb7521238c7c784b077456b35b5629..2cb1b2028d2b29a4d02aea6ca0e94943719b28a1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -216,6 +216,7 @@ SET(HDRS_STATE_BLOCK
include/core/state_block/local_parametrization_quaternion.h
include/core/state_block/state_angle.h
include/core/state_block/state_block.h
+ include/core/state_block/state_composite.h
include/core/state_block/state_homogeneous_3d.h
include/core/state_block/state_quaternion.h
)
@@ -316,6 +317,7 @@ SET(SRCS_STATE_BLOCK
src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp
src/state_block/state_block.cpp
+ src/state_block/state_composite.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 3c0593f670d2a8cbf5ce4418734b8fd571766407..8f801a6a8620a0f5e05af911ce4daf9f26783eee 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -106,10 +106,6 @@ typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmaj
namespace wolf {
-/// State of nodes containing several state blocks
-typedef std::unordered_map<std::string, Eigen::VectorXd> StateComposite;
-typedef std::string StateStructure;
-
//////////////////////////////////////////////////////////
/** Check Eigen Matrix sizes, both statically and dynamically
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index e289ec29854b510f2b99a763bca2d3f3758e11f2..84cf09e093e4dde53a704383cf39135e7b89c8e0 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -9,7 +9,7 @@
#define PROCESSOR_IS_MOTION_H_
#include "core/common/wolf.h"
-//#include "core/state_block/has_state_blocks.h"
+#include "core/state_block/state_composite.h"
namespace wolf
{
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 958b4f199e4a69eb41e8edd78d78fe2bf7834c5f..58c79858b9134fe38fe34dc91e3a93708a0b6db1 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -9,6 +9,7 @@
#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
#include <unordered_map>
@@ -262,30 +263,6 @@ inline Eigen::VectorXd HasStateBlocks::getState(const StateStructure& _sub_struc
return state;
}
-inline bool HasStateBlocks::getStateComposite(StateComposite& _state) const
-{
- for (auto& pair_key_sb : state_block_map_)
- {
- _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
- }
- return true;
-}
-
-inline StateComposite HasStateBlocks::getStateComposite() const
-{
- StateComposite state;
- getStateComposite(state);
- return state;
-}
-
-inline void HasStateBlocks::setStateComposite(const StateComposite &_state)
-{
- for (const auto& pair_key_sb : _state)
- {
- state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
- }
-}
-
inline std::unordered_map<std::string, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
{
const auto& it = std::find_if(state_block_map_.begin(),
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
new file mode 100644
index 0000000000000000000000000000000000000000..76b476aa33daebd713c447de5be17102c4f6e608
--- /dev/null
+++ b/include/core/state_block/state_composite.h
@@ -0,0 +1,93 @@
+/*
+ * state_composite.h
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+#ifndef STATE_BLOCK_STATE_COMPOSITE_H_
+#define STATE_BLOCK_STATE_COMPOSITE_H_
+
+#include "core/common/wolf.h"
+
+#include <unordered_map>
+
+#include <iostream>
+
+
+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, Eigen::VectorXd > StateComposite;
+
+
+class MatrixComposite
+{
+
+ private:
+ std::unordered_map < std::string, std::unordered_map < std::string, Eigen::MatrixXd > > matrix_composite_;
+ std::unordered_map < string, unsigned int> size_rows_, size_cols_;
+
+ public:
+ MatrixComposite() = default;
+ virtual ~MatrixComposite() = default;
+
+ unsigned int count(const std::string &_row,
+ const std::string &_col) const;
+
+ bool emplace(const std::string &_row,
+ const std::string &_col,
+ const Eigen::MatrixXd &_mat_blk);
+
+ bool at(const std::string &_row,
+ const std::string &_col,
+ Eigen::MatrixXd &_mat_blk) const;
+ const MatrixXd& at(const std::string &_row,
+ const std::string &_col) const;
+ MatrixXd& at(const std::string &_row,
+ const std::string &_col);
+
+ MatrixXd& operator ()(const std::string &_row,
+ const std::string &_col);
+ const MatrixXd& operator ()(const std::string &_row,
+ const std::string &_col) const;
+
+ StateComposite operator *(const StateComposite &_second) const;
+ MatrixComposite operator *(const MatrixComposite &_second) const;
+
+ friend std::ostream& operator << (std::ostream& _os, const MatrixComposite& _M);
+};
+
+
+
+
+inline const Eigen::MatrixXd& MatrixComposite::operator ()(const std::string &_row, const std::string &_col) const
+{
+ return matrix_composite_.at(_row).at(_col);
+}
+
+inline Eigen::MatrixXd& MatrixComposite::operator ()(const std::string &_row, const std::string &_col)
+{
+ return matrix_composite_[_row][_col];
+}
+
+inline unsigned int MatrixComposite::count(const std::string &_row, const std::string &_col) const
+{
+ if (matrix_composite_.count(_row) == 0)
+ return 0;
+
+ return (matrix_composite_.at(_row).count(_col));
+}
+
+std::ostream& operator <<(std::ostream &_os, const StateComposite &_x);
+
+}
+
+
+
+#endif /* STATE_BLOCK_STATE_COMPOSITE_H_ */
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 83cf44b26bdfcad7e415633ef6d8fe783564a75c..55a343f735d5b55a796e1dec3537927331fead4c 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -44,6 +44,30 @@ void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
}
}
+bool HasStateBlocks::getStateComposite(StateComposite &_state) const
+{
+ for (auto &pair_key_sb : state_block_map_)
+ {
+ _state.emplace(pair_key_sb.first, pair_key_sb.second->getState());
+ }
+ return true;
+}
+
+StateComposite HasStateBlocks::getStateComposite() const
+{
+ StateComposite state;
+ getStateComposite(state);
+ return state;
+}
+
+void HasStateBlocks::setStateComposite(const StateComposite &_state)
+{
+ for (const auto &pair_key_sb : _state)
+ {
+ state_block_map_[pair_key_sb.first]->setState(pair_key_sb.second);
+ }
+}
+
void HasStateBlocks::perturb(double amplitude)
{
for (const auto& pair_key_sb : state_block_map_)
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..84a7a526a3f05b8d1728f07cc4fc08fc672589ad
--- /dev/null
+++ b/src/state_block/state_composite.cpp
@@ -0,0 +1,136 @@
+
+
+
+#include "core/state_block/state_composite.h"
+
+namespace wolf
+{
+
+bool MatrixComposite::emplace(const std::string &_row, const std::string &_col, const Eigen::MatrixXd &_mat_blk)
+{
+ // check rows
+ if (size_rows_.count(_row) == 0)
+ size_rows_[_row] = _mat_blk.rows();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // check cols
+ if (size_cols_.count(_col) == 0)
+ size_cols_[_col] = _mat_blk.cols();
+ else
+ assert(size_rows_.at(_row) == _mat_blk.rows() && "Provided matrix block has wrong number of rows!");
+
+ // now it's safe to use [] operator
+ matrix_composite_[_row][_col] = _mat_blk;
+ return true;
+}
+
+bool MatrixComposite::at(const std::string &_row, const std::string &_col, Eigen::MatrixXd &_mat_blk) const
+{
+ const auto &row_it = matrix_composite_.find(_row);
+ if(row_it != matrix_composite_.end())
+ return false;
+
+ const auto &col_it = row_it->second.find(_col);
+ if(col_it != row_it->second.end())
+ return false;
+
+ _mat_blk = col_it->second;
+
+ return true;
+}
+
+Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col)
+{
+ const auto &row_it = matrix_composite_.find(_row);
+ assert(row_it != matrix_composite_.end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+const Eigen::MatrixXd& MatrixComposite::at(const std::string &_row, const std::string &_col) const
+{
+ const auto &row_it = matrix_composite_.find(_row);
+ assert(row_it != matrix_composite_.end() && "Requested matrix block does not exist in matrix composite.");
+
+ const auto &col_it = row_it->second.find(_col);
+ assert(col_it != row_it->second.end() && "Requested matrix block does not exist in matrix composite.");
+
+ return col_it->second;
+}
+
+wolf::MatrixComposite MatrixComposite::operator *(const MatrixComposite &_N) const
+{
+ MatrixComposite MN;
+ for (const auto &pair_i_Mi : this->matrix_composite_)
+ {
+ const auto &i = pair_i_Mi.first;
+ const auto &Mi = pair_i_Mi.second;
+ for (const auto &pair_k_Nk : _N.matrix_composite_)
+ {
+ const auto &k = pair_k_Nk.first;
+ const auto &Nk = pair_k_Nk.second;
+ for (const auto &pair_j_Nkj : Nk)
+ {
+ const auto &j = pair_j_Nkj.first;
+ const auto &Nkj = pair_j_Nkj.second;
+ const auto &Mik = Mi.at(k);
+ if (MN.count(i, j) == 0)
+ MN.emplace(i, j, Mik * Nkj);
+ else
+ MN.at(i, j) += Mik * Nkj;
+ }
+ }
+ }
+ return MN;
+}
+
+wolf::StateComposite MatrixComposite::operator *(const StateComposite &_x) const
+{
+ StateComposite y;
+ for (const auto &pair_rkey_row : matrix_composite_)
+ {
+ const auto &rkey = pair_rkey_row.first;
+ const auto &row = pair_rkey_row.second;
+ for (const auto &pair_ckey_mat : row)
+ {
+ const auto &ckey = pair_ckey_mat.first;
+ const auto &J_r_c = pair_ckey_mat.second;
+ if (y.count(rkey))
+ y.at(rkey) += J_r_c * _x.at(ckey);
+ else
+ y.emplace(rkey, J_r_c * _x.at(ckey));
+ }
+ }
+ return y;
+}
+
+std::ostream& operator <<(std::ostream &_os, const MatrixComposite &_M)
+{
+ for (const auto &pair_row_cols : _M.matrix_composite_)
+ {
+ const auto row = pair_row_cols.first;
+ for (const auto &pair_col_blk : pair_row_cols.second)
+ {
+ const auto &col = pair_col_blk.first;
+ _os << "\n block(" << row << "," << col << ") = \n" << pair_col_blk.second;
+ }
+ }
+ return _os;
+}
+
+std::ostream& operator <<(std::ostream &_os, const StateComposite &_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;
+}
+
+}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 501965cdff67102f807e482f87ca7eafb7109e54..2558c2452ea038d9b622c06a863c754424ed988a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -31,11 +31,13 @@ wolf_add_gtest(gtest_example gtest_example.cpp) #
target_link_libraries(gtest_example ${PROJECT_NAME}) #
# #
###########################################################
+
set(SRC_DUMMY
dummy/processor_tracker_feature_dummy.cpp
dummy/processor_tracker_landmark_dummy.cpp
)
add_library(dummy ${SRC_DUMMY})
+
################# ADD YOUR TESTS BELOW ####################
# #
# ==== IN ALPHABETICAL ORDER! ==== #
@@ -137,6 +139,10 @@ target_link_libraries(gtest_solver_manager ${PROJECT_NAME})
wolf_add_gtest(gtest_state_block gtest_state_block.cpp)
target_link_libraries(gtest_state_block ${PROJECT_NAME})
+# StateComposite class test
+wolf_add_gtest(gtest_state_composite gtest_state_composite.cpp)
+target_link_libraries(gtest_state_composite ${PROJECT_NAME})
+
# TimeStamp class test
wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp)
target_link_libraries(gtest_time_stamp ${PROJECT_NAME})
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abda2f373f2e0d951b8206518711a5efe6e3505e
--- /dev/null
+++ b/test/gtest_state_composite.cpp
@@ -0,0 +1,182 @@
+/*
+ * gtest_state_composite.cpp
+ *
+ * Created on: Apr 6, 2020
+ * Author: jsola
+ */
+
+
+#include "core/utils/utils_gtest.h"
+#include "core/utils/logging.h"
+
+#include "core/state_block/state_composite.h"
+
+using namespace wolf;
+
+/*
+// You may use this to make some methods of Foo public
+WOLF_PTR_TYPEDEFS(FooPublic);
+class FooPublic : public Foo
+{
+ // You may use this to make some methods of Foo public
+}
+
+class TestInit : 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(){}
+};
+
+TEST_F(TestInit, testName)
+{
+ // Use class TestInit
+}
+*/
+
+using namespace std;
+
+TEST(StateComposite, operatorStream)
+{
+ StateComposite x;
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("x = " , x);
+
+}
+TEST(MatrixComposite, emplace_operatorStream)
+{
+ MatrixComposite M;
+
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+
+ ASSERT_TRUE(M.emplace("P", "P", Mpp));
+ WOLF_DEBUG("M = " , M);
+
+ ASSERT_TRUE(M.emplace("P", "O", Mpo));
+ WOLF_DEBUG("M = " , M);
+
+ ASSERT_TRUE(M.emplace("O", "P", Mop));
+ WOLF_DEBUG("M = " , M);
+
+ ASSERT_TRUE(M.emplace("O", "O", Moo));
+ WOLF_DEBUG("M = " , M);
+}
+
+TEST(MatrixComposite, productVector)
+{
+ unsigned int psize, osize;
+ psize = 2;
+ osize = 3;
+
+ StateComposite x;
+ x.emplace("P", Vector2d(1,1));
+ x.emplace("O", Vector3d(2,2,2));
+
+ WOLF_DEBUG("x = " , x);
+
+ MatrixComposite M;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ StateComposite y;
+
+ y = M * x;
+
+ WOLF_DEBUG("y = M * x = " , y);
+
+}
+
+TEST(MatrixComposite, productMatrix)
+{
+ unsigned int psize, osize, vsize;
+ psize = 2;
+ osize = 1;
+ vsize = 2;
+
+ MatrixComposite M, N;
+
+ MatrixXd Mpp(psize,psize), Mpo(psize, osize), Mop(osize,psize), Moo(osize,osize);
+ Mpp.setOnes();
+ Mpo.setOnes(); Mpo *= 2;
+ Mop.setOnes(); Mop *= 3;
+ Moo.setOnes(); Moo *= 4;
+
+ M.emplace("P", "P", Mpp);
+ M.emplace("P", "O", Mpo);
+ M.emplace("O", "P", Mop);
+ M.emplace("O", "O", Moo);
+ WOLF_DEBUG("M = " , M);
+
+ MatrixXd Noo(osize,osize), Nov(osize, vsize), Npo(psize,osize), Npv(psize,vsize);
+ Noo.setOnes();
+ Nov.setOnes(); Nov *= 2;
+ Npo.setOnes(); Npo *= 3;
+ Npv.setOnes(); Npv *= 4;
+
+ N.emplace("O", "O", Noo);
+ N.emplace("O", "V", Nov);
+ N.emplace("P", "O", Npo);
+ N.emplace("P", "V", Npv);
+ WOLF_DEBUG("N = " , N);
+
+ MatrixComposite MN;
+
+ MN = M * N;
+
+ WOLF_DEBUG("MN = M * N = " , MN);
+
+ MatrixXd MNpo(MatrixXd::Ones(psize,osize) * 8);
+ MatrixXd MNpv(MatrixXd::Ones(psize,vsize) * 12);
+ MatrixXd MNoo(MatrixXd::Ones(osize,osize) * 22);
+ MatrixXd MNov(MatrixXd::Ones(osize,vsize) * 32);
+
+ ASSERT_MATRIX_APPROX(MN.at("P","O"), MNpo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("P","V"), MNpv, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("O","O"), MNoo, 1e-20);
+ ASSERT_MATRIX_APPROX(MN.at("O","V"), MNov, 1e-20);
+
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+
+ // restrict to a group of tests only
+ //::testing::GTEST_FLAG(filter) = "TestInit.*";
+
+ // restrict to this test only
+ //::testing::GTEST_FLAG(filter) = "TestInit.testName";
+
+ return RUN_ALL_TESTS();
+}