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"
This commit is part of merge request !278. Comments created here will be created in the context of that merge request.
......@@ -189,12 +189,11 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
unsigned int id = _node["id"] .as< unsigned int >();
Eigen::VectorXs pos = _node["position"] .as< Eigen::VectorXs >();
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 ori_sb = nullptr;
if (!_node["orientation"].as<std::string>().empty())
if (_node["orientation"])
{
Eigen::VectorXs ori = _node["orientation"].as< Eigen::VectorXs >();
bool ori_fixed = _node["orientation fixed"].as< bool >();
......@@ -205,7 +204,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
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);
return lmk;
......
......@@ -61,7 +61,7 @@ void MapBase::load(const std::string& _map_file_dot_yaml)
{
YAML::Node lmk_node = map["landmarks"][i];
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 @@
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/yaml/yaml_conversion.h"
#include <iostream>
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)
{
ProblemPtr problem = Problem::create("PO", 2);
......@@ -46,22 +30,24 @@ TEST(MapYaml, save_2D)
o3 << 3;
StateBlockPtr p1_sb = std::make_shared<StateBlock>(p1);
StateBlockPtr o1_sb = nullptr;
StateBlockPtr p2_sb = std::make_shared<StateBlock>(p2);
StateBlockPtr o2_sb = std::make_shared<StateBlock>(o2);
StateBlockPtr p3_sb = std::make_shared<StateBlock>(p3, 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", p3_sb, o3_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_2D_problem.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
problem->saveMap(filename, "2D map saved from Problem::saveMap()");
// Check Map functions
filename = map_path + "/map_2D_map.yaml";
std::cout << "Saving map to file: " << filename << std::endl;
problem->getMap()->save(filename, "2D map saved from Map::save()");
......@@ -73,6 +59,8 @@ TEST(MapYaml, load_2D)
std::string wolf_root = _WOLF_ROOT_DIR;
std::string map_path = wolf_root + "/test/yaml";
// Check Problem functions
std::string filename = map_path + "/map_2D_problem.yaml";
std::cout << "Loading map to file: " << filename << std::endl;
problem->loadMap(filename);
......@@ -98,16 +86,187 @@ TEST(MapYaml, load_2D)
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->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->getO() != nullptr);
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_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->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)
{
......
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