Skip to content
Snippets Groups Projects
Commit 42fc32ec authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

gtest_map_yaml working

parent 26ba2e2d
No related branches found
No related tags found
1 merge request!278Resolve "Revisit demos (formerly called examples) and update them"
...@@ -189,12 +189,11 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) ...@@ -189,12 +189,11 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
unsigned int id = _node["id"] .as< unsigned int >(); unsigned int id = _node["id"] .as< unsigned int >();
Eigen::VectorXs pos = _node["position"] .as< Eigen::VectorXs >(); Eigen::VectorXs pos = _node["position"] .as< Eigen::VectorXs >();
bool pos_fixed = _node["position fixed"] .as< bool >(); bool pos_fixed = _node["position fixed"] .as< bool >();
std::string type = _node["type"] .as< std::string >();
StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed); StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
StateBlockPtr ori_sb = nullptr; StateBlockPtr ori_sb = nullptr;
if (!_node["orientation"].as<std::string>().empty()) if (_node["orientation"])
{ {
Eigen::VectorXs ori = _node["orientation"].as< Eigen::VectorXs >(); Eigen::VectorXs ori = _node["orientation"].as< Eigen::VectorXs >();
bool ori_fixed = _node["orientation fixed"].as< bool >(); bool ori_fixed = _node["orientation fixed"].as< bool >();
...@@ -205,7 +204,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) ...@@ -205,7 +204,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
ori_sb = std::make_shared<StateBlock>(ori, ori_fixed); ori_sb = std::make_shared<StateBlock>(ori, ori_fixed);
} }
LandmarkBasePtr lmk = std::make_shared<LandmarkBase>(type, pos_sb, ori_sb); LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("BASE", pos_sb, ori_sb);
lmk->setId(id); lmk->setId(id);
return lmk; return lmk;
......
...@@ -61,7 +61,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml) ...@@ -61,7 +61,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
{ {
YAML::Node lmk_node = map["landmarks"][i]; YAML::Node lmk_node = map["landmarks"][i];
LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node); LandmarkBasePtr lmk_ptr = LandmarkFactory::get().create(lmk_node["type"].as<std::string>(), lmk_node);
addLandmark(lmk_ptr); lmk_ptr->link(shared_from_this());
} }
} }
......
...@@ -11,28 +11,12 @@ ...@@ -11,28 +11,12 @@
#include "core/map/map_base.h" #include "core/map/map_base.h"
#include "core/landmark/landmark_base.h" #include "core/landmark/landmark_base.h"
#include "core/state_block/state_block.h" #include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/yaml/yaml_conversion.h" #include "core/yaml/yaml_conversion.h"
#include <iostream> #include <iostream>
using namespace wolf; using namespace wolf;
void print(MapBase& _map)
{
for (auto lmk_ptr : _map.getLandmarkList())
{
std::cout << "Lmk ID: " << lmk_ptr->id() << std::endl;
std::cout << "\tLmk type: " << lmk_ptr->getType() << std::endl;
if (lmk_ptr->getType() == "BASE")
{
std::cout << "\tpos: " << lmk_ptr->getP()->getState().transpose() << (lmk_ptr->getP()->isFixed() ? " -- fixed" : " -- estimated\n");
if (lmk_ptr->getO())
std::cout << "\tori: " << lmk_ptr->getO()->getState().transpose() << (lmk_ptr->getO()->isFixed() ? " -- fixed" : " -- estimated\n");
else
std::cout << "\tori: none\n";
}
}
}
TEST(MapYaml, save_2D) TEST(MapYaml, save_2D)
{ {
ProblemPtr problem = Problem::create("PO", 2); ProblemPtr problem = Problem::create("PO", 2);
...@@ -46,22 +30,24 @@ TEST(MapYaml, save_2D) ...@@ -46,22 +30,24 @@ TEST(MapYaml, save_2D)
o3 << 3; o3 << 3;
StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1); StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
StateBlockPtr o1_sb = nullptr;
StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2); StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2); StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true); StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, true);
StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true); StateBlockPtr o3_sb = std::make_shared<StateBlock>(o3, true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb, o1_sb); LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, o2_sb); LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, o2_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, o3_sb); LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, o3_sb);
std::string wolf_root = _WOLF_ROOT_DIR; std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml"; std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
std::string filename = map_path + "/map_2D_problem.yaml"; std::string filename = map_path + "/map_2D_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl; std::cout << "Saving map to file: " << filename << std::endl;
problem->saveMap(filename, "2D map saved from Problem::saveMap()"); problem->saveMap(filename, "2D map saved from Problem::saveMap()");
// Check Map functions
filename = map_path + "/map_2D_map.yaml"; filename = map_path + "/map_2D_map.yaml";
std::cout << "Saving map to file: " << filename << std::endl; std::cout << "Saving map to file: " << filename << std::endl;
problem->getMap()->save(filename, "2D map saved from Map::save()"); problem->getMap()->save(filename, "2D map saved from Map::save()");
...@@ -73,6 +59,8 @@ TEST(MapYaml, load_2D) ...@@ -73,6 +59,8 @@ TEST(MapYaml, load_2D)
std::string wolf_root = _WOLF_ROOT_DIR; std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml"; std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
std::string filename = map_path + "/map_2D_problem.yaml"; std::string filename = map_path + "/map_2D_problem.yaml";
std::cout << "Loading map to file: " << filename << std::endl; std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename); problem->loadMap(filename);
...@@ -98,16 +86,187 @@ TEST(MapYaml, load_2D) ...@@ -98,16 +86,187 @@ TEST(MapYaml, load_2D)
ASSERT_FALSE(lmk->getO()->isFixed()); ASSERT_FALSE(lmk->getO()->isFixed());
} }
else if (lmk->id() == 3) else if (lmk->id() == 3)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<5,6).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<3).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
}
}
// empty the map
{
auto lmk_list = problem->getMap()->getLandmarkList();
for (auto lmk : lmk_list)
lmk->remove();
}
ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
// Check Map functions
filename = map_path + "/map_2D_map.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->getMap()->load(filename);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
for (auto lmk : problem->getMap()->getLandmarkList())
{
if (lmk->id() == 1)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<1,2).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{ {
ASSERT_TRUE(lmk->getP() != nullptr); ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr); ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<3,4).finished(), 1e-12); ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<3,4).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12); ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
}
else if (lmk->id() == 3)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<5,6).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<3).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed()); ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed()); ASSERT_TRUE(lmk->getO()->isFixed());
} }
} }
} }
TEST(MapYaml, save_3D)
{
ProblemPtr problem = Problem::create("PO", 3);
Eigen::Vector3s p1, p2, p3;
Eigen::Vector4s q2, q3;
p1 << 1, 2, 3;
p2 << 4, 5, 6;
p3 << 7, 8, 9;
q2 << 0, 1, 0, 0;
q3 << 0, 0, 1, 0;
auto p1_sb = std::make_shared<StateBlock>(p1);
auto p2_sb = std::make_shared<StateBlock>(p2);
auto q2_sb = std::make_shared<StateQuaternion>(q2);
auto p3_sb = std::make_shared<StateBlock>(p3, true);
auto q3_sb = std::make_shared<StateQuaternion>(q3, true);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p1_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p2_sb, q2_sb);
LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "BASE", p3_sb, q3_sb);
std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
std::string filename = map_path + "/map_3D_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
problem->saveMap(filename, "3D map saved from Problem::saveMap()");
// Check Map functions
filename = map_path + "/map_3D_map.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
problem->getMap()->save(filename, "3D map saved from Map::save()");
}
TEST(MapYaml, load_3D)
{
ProblemPtr problem = Problem::create("PO", 3);
std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
std::string filename = map_path + "/map_3D_problem.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
for (auto lmk : problem->getMap()->getLandmarkList())
{
if (lmk->id() == 1)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<1,2,3).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<4,5,6).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,1,0,0).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
// TODO check if localparameterization of type quaternion
}
else if (lmk->id() == 3)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<7,8,9).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,0,1,0).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
// TODO check if localparameterization of type quaternion
}
}
// empty the map
{
auto lmk_list = problem->getMap()->getLandmarkList();
for (auto lmk : lmk_list)
lmk->remove();
}
ASSERT_TRUE(problem->getMap()->getLandmarkList().empty());
// Check Map functions
filename = map_path + "/map_3D_map.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->getMap()->load(filename);
ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 3);
for (auto lmk : problem->getMap()->getLandmarkList())
{
if (lmk->id() == 1)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<1,2,3).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<4,5,6).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,1,0,0).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
// TODO check if localparameterization of type quaternion
}
else if (lmk->id() == 3)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<7,8,9).finished(), 1e-12);
ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,0,1,0).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
// TODO check if localparameterization of type quaternion
}
}
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
......
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