diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp index a9d843c55157603dd8f3ee08dc0290ca33aa5385..60ac575e2a13591b22a8ce0c0217d049124cc9e2 100644 --- a/test/gtest_map_yaml.cpp +++ b/test/gtest_map_yaml.cpp @@ -12,6 +12,7 @@ #include "core/landmark/landmark_base.h" #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" +#include "core/state_block/local_parametrization_quaternion.h" #include "core/yaml/yaml_conversion.h" #include <iostream> @@ -207,7 +208,7 @@ TEST(MapYaml, load_3D) 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 + ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); } else if (lmk->id() == 3) { @@ -217,7 +218,7 @@ TEST(MapYaml, load_3D) 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 + ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); } } @@ -253,7 +254,7 @@ TEST(MapYaml, load_3D) 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 + ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); } else if (lmk->id() == 3) { @@ -263,7 +264,7 @@ TEST(MapYaml, load_3D) 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 + ASSERT_TRUE(lmk->getO()->hasLocalParametrization()); } } }