working on gtest_map_yaml

test/CMakeLists.txt

@@ -166,11 +166,14 @@ target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
 wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
 target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
 
-
 # MakePosDef function test
 wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
 target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
 
+# Map yaml save/load test
+wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
+target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
+
 # Parameter prior test
 wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
 target_link_libraries(gtest_param_prior ${PROJECT_NAME})

test/gtest_map_yaml.cpp

+/**
+ * \file test_map_yaml.cpp
+ *
+ *  Created on: Jul 27, 2016
+ *      \author: jsola
+ */
+
+#include "core/utils/utils_gtest.h"
+#include "core/common/wolf.h"
+#include "core/problem/problem.h"
+#include "core/map/map_base.h"
+#include "core/landmark/landmark_base.h"
+#include "core/state_block/state_block.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() == "LANDMARK 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);
+
+    Eigen::Vector2s p1, p2, p3;
+    Eigen::Vector1s o2, o3;
+    p1 << 1, 2;
+    p2 << 3, 4;
+    p3 << 5, 6;
+    o2 << 2;
+    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(), p1_sb, o1_sb);
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), p2_sb, o2_sb);
+    LandmarkBase::emplace<LandmarkBase>(problem->getMap(), p3_sb, o3_sb);
+
+    std::string wolf_root       = _WOLF_ROOT_DIR;
+    std::string map_path     = wolf_root + "/test/yaml";
+    std::string filename = wolf_config + "/map_2D_problem.yaml";
+    std::cout << "Saving map to file: " << filename << std::endl;
+    problem->saveMap(filename, "2D map saved from Problem::saveMap()");
+
+    std::string filename = wolf_config + "/map_2D_map.yaml";
+    std::cout << "Saving map to file: " << filename << std::endl;
+    problem->getMap()->save(filename, "2D map saved from Map::save()");
+}
+
+TEST(MapYaml, load_2D)
+{
+    ProblemPtr problem = Problem::create("PO", 2);
+
+    std::string wolf_root       = _WOLF_ROOT_DIR;
+    std::string map_path     = wolf_root + "/test/yaml";
+    std::string filename = wolf_config + "/map_2D_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::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()<<3,4).finished(), 1e-12);
+            ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12);
+            ASSERT_TRUE(lmk->getP()->isFixed());
+            ASSERT_TRUE(lmk->getO()->isFixed());
+        }
+    }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}