diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index b4aa39530bc0e59dd71b62f552e124236ce77867..25dbf77343c40d83bc9a85960561fee198a4378b 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -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;
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 204fc7b8ffd3d1b1a8c49030c74c0e803e11b7c4..aa4b1ac6cbe7ad182731a1f2acfd5ad46334a574 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -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());
}
}
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 1b13a761875f932faf75663274324f7b91d259ae..1df25d5ef3420ad3796c62444460d1e76e97dbd0 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -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)
{