Skip to content
Snippets Groups Projects
Commit 515c5bc1 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

New file state_composite.* with class MatrixComposite

parent 6bd43e35
No related branches found
No related tags found
2 merge requests!358WIP: Resolve "Complete state vector new data structure?",!343WIP: Resolve "Complete state vector new data structure?"
Pipeline #5116 failed
This commit is part of merge request !343. Comments created here will be created in the context of that merge request.
......@@ -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
......
......@@ -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
......
......@@ -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
{
......
......@@ -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(),
......
/*
* 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_ */
......@@ -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_)
......
#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;
}
}
......@@ -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})
......
/*
* 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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment