From 96eb1f67d7e0b41a86efdaf43a8be4a3298cbb8a Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 5 Feb 2025 16:28:17 +0100
Subject: [PATCH] landmark test completed and working

---
 test/gtest_landmark.cpp | 766 +++++++++++-----------------------------
 1 file changed, 216 insertions(+), 550 deletions(-)

diff --git a/test/gtest_landmark.cpp b/test/gtest_landmark.cpp
index 1e78b39a4..11745d166 100644
--- a/test/gtest_landmark.cpp
+++ b/test/gtest_landmark.cpp
@@ -25,11 +25,12 @@
 using namespace wolf;
 using namespace Eigen;
 
-Vector2d p_2d = Vector2d::Random();
-Vector3d p_3d = Vector3d::Random();
-Vector1d o_2d = Vector1d::Random();
-Vector4d o_3d = Quaterniond::UnitRandom().coeffs();
-int      id   = 0;
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+VectorXd p_2d = Vector2d::Random();
+VectorXd p_3d = Vector3d::Random();
+VectorXd o_2d = Vector1d::Random();
+VectorXd o_3d = Quaterniond::UnitRandom().coeffs();
 
 VectorComposite vectors_p_2d({{'P', p_2d}});
 VectorComposite vectors_po_2d({{'P', p_2d}, {'O', o_2d}});
@@ -127,6 +128,25 @@ void fillYamlNodes()
     params_po_3d_fix_fac["states"]["O"]["prior"]["factor_std"] = Vector3d::Ones();
 }
 
+void checkLandmark(LandmarkBasePtr lmk, int dim, bool orientation, std::string p_prior, std::string o_prior = "")
+{
+    ASSERT_TRUE(lmk);
+    ASSERT_TRUE(lmk->has('P'));
+    ASSERT_EQ(lmk->has('O'), orientation);
+    ASSERT_EQ(lmk->getKeys(), orientation ? "OP" : "P");
+
+    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), dim == 2 ? p_2d : p_3d, 1e-12);
+    ASSERT_EQ(lmk->isFixed(), p_prior == "fix" and (not orientation or o_prior == "fix"));
+    ASSERT_EQ(lmk->getP()->isFixed(), p_prior == "fix");
+    ASSERT_EQ(lmk->getFactoredBySet().size(), (p_prior == "factor" ? 1 : 0) + (o_prior == "factor" ? 1 : 0));
+
+    if (orientation)
+    {
+        ASSERT_MATRIX_APPROX(lmk->getO()->getState(), dim == 2 ? o_2d : o_3d, 1e-12);
+        ASSERT_EQ(lmk->getO()->isFixed(), o_prior == "fix");
+    }
+}
+
 ////////////////////////////////////////////////////////////////////
 //////////////////// CONSTRUCTOR COMPOSITES ////////////////////////
 ////////////////////////////////////////////////////////////////////
@@ -145,37 +165,10 @@ TEST(LandmarkTest, ConstructorComposite_P2D)
     lmk_fac->emplacePriors();
 
     // checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_FALSE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_FALSE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_FALSE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_FALSE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_FALSE(lmk_fac->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fac->isFixed());
+    checkLandmark(lmk, 2, false, "initial_guess");
+    checkLandmark(lmk_ig, 2, false, "initial_guess");
+    checkLandmark(lmk_fix, 2, false, "fix");
+    checkLandmark(lmk_fac, 2, false, "factor");
 }
 
 TEST(LandmarkTest, ConstructorComposite_PO2D)
@@ -197,71 +190,12 @@ TEST(LandmarkTest, ConstructorComposite_PO2D)
     lmk_fix_fac->emplacePriors();
 
     // Checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_TRUE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-    ASSERT_FALSE(lmk->getP()->isFixed());
-    ASSERT_FALSE(lmk->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_TRUE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_ig->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-    ASSERT_FALSE(lmk_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_TRUE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-    ASSERT_TRUE(lmk_fix->getP()->isFixed());
-    ASSERT_TRUE(lmk_fix->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_TRUE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fac->getO()->getState(), o_2d, 1e-12);
-    ASSERT_EQ(lmk_fac->getFactoredBySet().size(), 2);
-    ASSERT_FALSE(lmk_fac->isFixed());
-    ASSERT_FALSE(lmk_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fac->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_ig);
-    ASSERT_TRUE(lmk_fix_ig->has('P'));
-    ASSERT_TRUE(lmk_fix_ig->has('O'));
-    ASSERT_EQ(lmk_fix_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fix_ig->isFixed());
-    ASSERT_TRUE(lmk_fix_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_fac);
-    ASSERT_TRUE(lmk_fix_fac->has('P'));
-    ASSERT_TRUE(lmk_fix_fac->has('O'));
-    ASSERT_EQ(lmk_fix_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getO()->getState(), o_2d, 1e-12);
-    ASSERT_EQ(lmk_fix_fac->getFactoredBySet().size(), 1);
-    ASSERT_FALSE(lmk_fix_fac->isFixed());
-    ASSERT_TRUE(lmk_fix_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_fac->getO()->isFixed());
+    checkLandmark(lmk, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_ig, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 2, true, "fix", "fix");
+    checkLandmark(lmk_fac, 2, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 2, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 2, true, "fix", "factor");
 }
 
 TEST(LandmarkTest, ConstructorComposite_P3D)
@@ -279,37 +213,10 @@ TEST(LandmarkTest, ConstructorComposite_P3D)
     lmk_fac->emplacePriors();
 
     // checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_FALSE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_FALSE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_FALSE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_FALSE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_FALSE(lmk_fac->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fac->isFixed());
+    checkLandmark(lmk, 3, false, "initial_guess");
+    checkLandmark(lmk_ig, 3, false, "initial_guess");
+    checkLandmark(lmk_fix, 3, false, "fix");
+    checkLandmark(lmk_fac, 3, false, "factor");
 }
 
 TEST(LandmarkTest, ConstructorComposite_PO3D)
@@ -331,71 +238,12 @@ TEST(LandmarkTest, ConstructorComposite_PO3D)
     lmk_fix_fac->emplacePriors();
 
     // Checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_TRUE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-    ASSERT_FALSE(lmk->getP()->isFixed());
-    ASSERT_FALSE(lmk->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_TRUE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_ig->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-    ASSERT_FALSE(lmk_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_TRUE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-    ASSERT_TRUE(lmk_fix->getP()->isFixed());
-    ASSERT_TRUE(lmk_fix->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_TRUE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fac->getO()->getState(), o_3d, 1e-12);
-    ASSERT_EQ(lmk_fac->getFactoredBySet().size(), 2);
-    ASSERT_FALSE(lmk_fac->isFixed());
-    ASSERT_FALSE(lmk_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fac->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_ig);
-    ASSERT_TRUE(lmk_fix_ig->has('P'));
-    ASSERT_TRUE(lmk_fix_ig->has('O'));
-    ASSERT_EQ(lmk_fix_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fix_ig->isFixed());
-    ASSERT_TRUE(lmk_fix_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_fac);
-    ASSERT_TRUE(lmk_fix_fac->has('P'));
-    ASSERT_TRUE(lmk_fix_fac->has('O'));
-    ASSERT_EQ(lmk_fix_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getO()->getState(), o_3d, 1e-12);
-    ASSERT_EQ(lmk_fix_fac->getFactoredBySet().size(), 1);
-    ASSERT_FALSE(lmk_fix_fac->isFixed());
-    ASSERT_TRUE(lmk_fix_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_fac->getO()->isFixed());
+    checkLandmark(lmk, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_ig, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 3, true, "fix", "fix");
+    checkLandmark(lmk_fac, 3, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 3, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 3, true, "fix", "factor");
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -415,29 +263,9 @@ TEST(LandmarkTest, ConstructorYamlNode_P2D)
     lmk_fac->emplacePriors();
 
     // checks
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_FALSE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_FALSE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_FALSE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_FALSE(lmk_fac->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fac->isFixed());
+    checkLandmark(lmk_ig, 2, false, "initial_guess");
+    checkLandmark(lmk_fix, 2, false, "fix");
+    checkLandmark(lmk_fac, 2, false, "factor");
 }
 
 TEST(LandmarkTest, ConstructorYamlNode_PO2D)
@@ -456,60 +284,11 @@ TEST(LandmarkTest, ConstructorYamlNode_PO2D)
     lmk_fix_fac->emplacePriors();
 
     // Checks
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_TRUE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_ig->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-    ASSERT_FALSE(lmk_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_TRUE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-    ASSERT_TRUE(lmk_fix->getP()->isFixed());
-    ASSERT_TRUE(lmk_fix->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_TRUE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fac->getO()->getState(), o_2d, 1e-12);
-    ASSERT_EQ(lmk_fac->getFactoredBySet().size(), 2);
-    ASSERT_FALSE(lmk_fac->isFixed());
-    ASSERT_FALSE(lmk_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fac->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_ig);
-    ASSERT_TRUE(lmk_fix_ig->has('P'));
-    ASSERT_TRUE(lmk_fix_ig->has('O'));
-    ASSERT_EQ(lmk_fix_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fix_ig->isFixed());
-    ASSERT_TRUE(lmk_fix_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_fac);
-    ASSERT_TRUE(lmk_fix_fac->has('P'));
-    ASSERT_TRUE(lmk_fix_fac->has('O'));
-    ASSERT_EQ(lmk_fix_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getO()->getState(), o_2d, 1e-12);
-    ASSERT_EQ(lmk_fix_fac->getFactoredBySet().size(), 1);
-    ASSERT_FALSE(lmk_fix_fac->isFixed());
-    ASSERT_TRUE(lmk_fix_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_fac->getO()->isFixed());
+    checkLandmark(lmk_ig, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 2, true, "fix", "fix");
+    checkLandmark(lmk_fac, 2, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 2, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 2, true, "fix", "factor");
 }
 
 TEST(LandmarkTest, ConstructorYamlNode_P3D)
@@ -524,29 +303,9 @@ TEST(LandmarkTest, ConstructorYamlNode_P3D)
     lmk_fac->emplacePriors();
 
     // checks
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_FALSE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_FALSE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_FALSE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_FALSE(lmk_fac->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fac->isFixed());
+    checkLandmark(lmk_ig, 3, false, "initial_guess");
+    checkLandmark(lmk_fix, 3, false, "fix");
+    checkLandmark(lmk_fac, 3, false, "factor");
 }
 
 TEST(LandmarkTest, ConstructorYamlNode_PO3D)
@@ -565,60 +324,11 @@ TEST(LandmarkTest, ConstructorYamlNode_PO3D)
     lmk_fix_fac->emplacePriors();
 
     // Checks
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_TRUE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_ig->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-    ASSERT_FALSE(lmk_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_TRUE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-    ASSERT_TRUE(lmk_fix->getP()->isFixed());
-    ASSERT_TRUE(lmk_fix->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_TRUE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fac->getO()->getState(), o_3d, 1e-12);
-    ASSERT_EQ(lmk_fac->getFactoredBySet().size(), 2);
-    ASSERT_FALSE(lmk_fac->isFixed());
-    ASSERT_FALSE(lmk_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fac->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_ig);
-    ASSERT_TRUE(lmk_fix_ig->has('P'));
-    ASSERT_TRUE(lmk_fix_ig->has('O'));
-    ASSERT_EQ(lmk_fix_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fix_ig->isFixed());
-    ASSERT_TRUE(lmk_fix_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_fac);
-    ASSERT_TRUE(lmk_fix_fac->has('P'));
-    ASSERT_TRUE(lmk_fix_fac->has('O'));
-    ASSERT_EQ(lmk_fix_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getO()->getState(), o_3d, 1e-12);
-    ASSERT_EQ(lmk_fix_fac->getFactoredBySet().size(), 1);
-    ASSERT_FALSE(lmk_fix_fac->isFixed());
-    ASSERT_TRUE(lmk_fix_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_fac->getO()->isFixed());
+    checkLandmark(lmk_ig, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 3, true, "fix", "fix");
+    checkLandmark(lmk_fac, 3, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 3, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 3, true, "fix", "factor");
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -633,37 +343,10 @@ TEST(LandmarkTest, EmplaceComposite_P2D)
     auto lmk_fac = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_p_2d, priors_p_fac_2d);
 
     // checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_FALSE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_FALSE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_FALSE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_FALSE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_FALSE(lmk_fac->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fac->isFixed());
+    checkLandmark(lmk, 2, false, "initial_guess");
+    checkLandmark(lmk_ig, 2, false, "initial_guess");
+    checkLandmark(lmk_fix, 2, false, "fix");
+    checkLandmark(lmk_fac, 2, false, "factor");
 }
 
 TEST(LandmarkTest, EmplaceComposite_PO2D)
@@ -677,71 +360,12 @@ TEST(LandmarkTest, EmplaceComposite_PO2D)
     auto lmk_fix_fac = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_po_2d, priors_po_fix_fac_2d);
 
     // Checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_TRUE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-    ASSERT_FALSE(lmk->getP()->isFixed());
-    ASSERT_FALSE(lmk->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_TRUE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_ig->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-    ASSERT_FALSE(lmk_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_TRUE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-    ASSERT_TRUE(lmk_fix->getP()->isFixed());
-    ASSERT_TRUE(lmk_fix->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_TRUE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fac->getO()->getState(), o_2d, 1e-12);
-    ASSERT_EQ(lmk_fac->getFactoredBySet().size(), 2);
-    ASSERT_FALSE(lmk_fac->isFixed());
-    ASSERT_FALSE(lmk_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fac->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_ig);
-    ASSERT_TRUE(lmk_fix_ig->has('P'));
-    ASSERT_TRUE(lmk_fix_ig->has('O'));
-    ASSERT_EQ(lmk_fix_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getO()->getState(), o_2d, 1e-12);
-    ASSERT_TRUE(lmk_fix_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fix_ig->isFixed());
-    ASSERT_TRUE(lmk_fix_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_fac);
-    ASSERT_TRUE(lmk_fix_fac->has('P'));
-    ASSERT_TRUE(lmk_fix_fac->has('O'));
-    ASSERT_EQ(lmk_fix_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getP()->getState(), p_2d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getO()->getState(), o_2d, 1e-12);
-    ASSERT_EQ(lmk_fix_fac->getFactoredBySet().size(), 1);
-    ASSERT_FALSE(lmk_fix_fac->isFixed());
-    ASSERT_TRUE(lmk_fix_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_fac->getO()->isFixed());
+    checkLandmark(lmk, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_ig, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 2, true, "fix", "fix");
+    checkLandmark(lmk_fac, 2, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 2, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 2, true, "fix", "factor");
 }
 
 TEST(LandmarkTest, EmplaceComposite_P3D)
@@ -753,37 +377,10 @@ TEST(LandmarkTest, EmplaceComposite_P3D)
     auto lmk_fac = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_p_3d, priors_p_fac_3d);
 
     // checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_FALSE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_FALSE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_FALSE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_FALSE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "P");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_FALSE(lmk_fac->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fac->isFixed());
+    checkLandmark(lmk, 3, false, "initial_guess");
+    checkLandmark(lmk_ig, 3, false, "initial_guess");
+    checkLandmark(lmk_fix, 3, false, "fix");
+    checkLandmark(lmk_fac, 3, false, "factor");
 }
 
 TEST(LandmarkTest, EmplaceComposite_PO3D)
@@ -797,86 +394,135 @@ TEST(LandmarkTest, EmplaceComposite_PO3D)
     auto lmk_fix_fac = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_po_3d, priors_po_fix_fac_3d);
 
     // Checks
-    ASSERT_TRUE(lmk);
-    ASSERT_TRUE(lmk->has('P'));
-    ASSERT_TRUE(lmk->has('O'));
-    ASSERT_EQ(lmk->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk->isFixed());
-    ASSERT_FALSE(lmk->getP()->isFixed());
-    ASSERT_FALSE(lmk->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_ig);
-    ASSERT_TRUE(lmk_ig->has('P'));
-    ASSERT_TRUE(lmk_ig->has('O'));
-    ASSERT_EQ(lmk_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_ig->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_ig->isFixed());
-    ASSERT_FALSE(lmk_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix);
-    ASSERT_TRUE(lmk_fix->has('P'));
-    ASSERT_TRUE(lmk_fix->has('O'));
-    ASSERT_EQ(lmk_fix->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix->getFactoredBySet().empty());
-    ASSERT_TRUE(lmk_fix->isFixed());
-    ASSERT_TRUE(lmk_fix->getP()->isFixed());
-    ASSERT_TRUE(lmk_fix->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fac);
-    ASSERT_TRUE(lmk_fac->has('P'));
-    ASSERT_TRUE(lmk_fac->has('O'));
-    ASSERT_EQ(lmk_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fac->getO()->getState(), o_3d, 1e-12);
-    ASSERT_EQ(lmk_fac->getFactoredBySet().size(), 2);
-    ASSERT_FALSE(lmk_fac->isFixed());
-    ASSERT_FALSE(lmk_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fac->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_ig);
-    ASSERT_TRUE(lmk_fix_ig->has('P'));
-    ASSERT_TRUE(lmk_fix_ig->has('O'));
-    ASSERT_EQ(lmk_fix_ig->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_ig->getO()->getState(), o_3d, 1e-12);
-    ASSERT_TRUE(lmk_fix_ig->getFactoredBySet().empty());
-    ASSERT_FALSE(lmk_fix_ig->isFixed());
-    ASSERT_TRUE(lmk_fix_ig->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_ig->getO()->isFixed());
-
-    ASSERT_TRUE(lmk_fix_fac);
-    ASSERT_TRUE(lmk_fix_fac->has('P'));
-    ASSERT_TRUE(lmk_fix_fac->has('O'));
-    ASSERT_EQ(lmk_fix_fac->getKeys(), "OP");
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getP()->getState(), p_3d, 1e-12);
-    ASSERT_MATRIX_APPROX(lmk_fix_fac->getO()->getState(), o_3d, 1e-12);
-    ASSERT_EQ(lmk_fix_fac->getFactoredBySet().size(), 1);
-    ASSERT_FALSE(lmk_fix_fac->isFixed());
-    ASSERT_TRUE(lmk_fix_fac->getP()->isFixed());
-    ASSERT_FALSE(lmk_fix_fac->getO()->isFixed());
+    checkLandmark(lmk, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_ig, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 3, true, "fix", "fix");
+    checkLandmark(lmk_fac, 3, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 3, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 3, true, "fix", "factor");
 }
 
 ////////////////////////////////////////////////////////////////////
 ///////////////////// EMPLACE YAML NODES ///////////////////////////
 ////////////////////////////////////////////////////////////////////
+TEST(LandmarkTest, EmplaceYamlNode_P2D)
+{
+    fillYamlNodes();
+
+    auto lmk_ig  = LandmarkBase::emplace<Landmark2d>(nullptr, params_p_2d_ig);
+    auto lmk_fix = LandmarkBase::emplace<Landmark2d>(nullptr, params_p_2d_fix);
+    auto lmk_fac = LandmarkBase::emplace<Landmark2d>(nullptr, params_p_2d_fac);
+
+    // checks
+    checkLandmark(lmk_ig, 2, false, "initial_guess");
+    checkLandmark(lmk_fix, 2, false, "fix");
+    checkLandmark(lmk_fac, 2, false, "factor");
+}
+
+TEST(LandmarkTest, EmplaceYamlNode_PO2D)
+{
+    auto lmk_ig      = LandmarkBase::emplace<Landmark2d>(nullptr, params_po_2d_ig);
+    auto lmk_fix     = LandmarkBase::emplace<Landmark2d>(nullptr, params_po_2d_fix);
+    auto lmk_fac     = LandmarkBase::emplace<Landmark2d>(nullptr, params_po_2d_fac);
+    auto lmk_fix_ig  = LandmarkBase::emplace<Landmark2d>(nullptr, params_po_2d_fix_ig);
+    auto lmk_fix_fac = LandmarkBase::emplace<Landmark2d>(nullptr, params_po_2d_fix_fac);
+
+    // Checks
+    checkLandmark(lmk_ig, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 2, true, "fix", "fix");
+    checkLandmark(lmk_fac, 2, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 2, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 2, true, "fix", "factor");
+}
+
+TEST(LandmarkTest, EmplaceYamlNode_P3D)
+{
+    auto lmk_ig  = LandmarkBase::emplace<Landmark3d>(nullptr, params_p_3d_ig);
+    auto lmk_fix = LandmarkBase::emplace<Landmark3d>(nullptr, params_p_3d_fix);
+    auto lmk_fac = LandmarkBase::emplace<Landmark3d>(nullptr, params_p_3d_fac);
+
+    // checks
+    checkLandmark(lmk_ig, 3, false, "initial_guess");
+    checkLandmark(lmk_fix, 3, false, "fix");
+    checkLandmark(lmk_fac, 3, false, "factor");
+}
+
+TEST(LandmarkTest, EmplaceYamlNode_PO3D)
+{
+    auto lmk_ig      = LandmarkBase::emplace<Landmark3d>(nullptr, params_po_3d_ig);
+    auto lmk_fix     = LandmarkBase::emplace<Landmark3d>(nullptr, params_po_3d_fix);
+    auto lmk_fac     = LandmarkBase::emplace<Landmark3d>(nullptr, params_po_3d_fac);
+    auto lmk_fix_ig  = LandmarkBase::emplace<Landmark3d>(nullptr, params_po_3d_fix_ig);
+    auto lmk_fix_fac = LandmarkBase::emplace<Landmark3d>(nullptr, params_po_3d_fix_fac);
 
-// TODO
-TEST(Landmark2dTest, emplaceYamlNode) {}
+    // Checks
+    checkLandmark(lmk_ig, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 3, true, "fix", "fix");
+    checkLandmark(lmk_fac, 3, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 3, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 3, true, "fix", "factor");
+}
 
 ////////////////////////////////////////////////////////////////////
 /////////////////////////// CREATE /////////////////////////////////
 ////////////////////////////////////////////////////////////////////
+TEST(LandmarkTest, Create_P2D)
+{
+    fillYamlNodes();
+
+    auto lmk_ig  = FactoryLandmark::create("Landmark2d", params_p_2d_ig, {wolf_dir});
+    auto lmk_fix = FactoryLandmark::create("Landmark2d", params_p_2d_fix, {wolf_dir});
+    auto lmk_fac = FactoryLandmark::create("Landmark2d", params_p_2d_fac, {wolf_dir});
+
+    // checks
+    checkLandmark(lmk_ig, 2, false, "initial_guess");
+    checkLandmark(lmk_fix, 2, false, "fix");
+    checkLandmark(lmk_fac, 2, false, "factor");
+}
 
-// TODO
-TEST(Landmark2dTest, create) {}
+TEST(LandmarkTest, Create_PO2D)
+{
+    auto lmk_ig      = FactoryLandmark::create("Landmark2d", params_po_2d_ig, {wolf_dir});
+    auto lmk_fix     = FactoryLandmark::create("Landmark2d", params_po_2d_fix, {wolf_dir});
+    auto lmk_fac     = FactoryLandmark::create("Landmark2d", params_po_2d_fac, {wolf_dir});
+    auto lmk_fix_ig  = FactoryLandmark::create("Landmark2d", params_po_2d_fix_ig, {wolf_dir});
+    auto lmk_fix_fac = FactoryLandmark::create("Landmark2d", params_po_2d_fix_fac, {wolf_dir});
+
+    // Checks
+    checkLandmark(lmk_ig, 2, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 2, true, "fix", "fix");
+    checkLandmark(lmk_fac, 2, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 2, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 2, true, "fix", "factor");
+}
+
+TEST(LandmarkTest, Create_P3D)
+{
+    auto lmk_ig  = FactoryLandmark::create("Landmark3d", params_p_3d_ig, {wolf_dir});
+    auto lmk_fix = FactoryLandmark::create("Landmark3d", params_p_3d_fix, {wolf_dir});
+    auto lmk_fac = FactoryLandmark::create("Landmark3d", params_p_3d_fac, {wolf_dir});
+
+    // checks
+    checkLandmark(lmk_ig, 3, false, "initial_guess");
+    checkLandmark(lmk_fix, 3, false, "fix");
+    checkLandmark(lmk_fac, 3, false, "factor");
+}
+
+TEST(LandmarkTest, Create_PO3D)
+{
+    auto lmk_ig      = FactoryLandmark::create("Landmark3d", params_po_3d_ig, {wolf_dir});
+    auto lmk_fix     = FactoryLandmark::create("Landmark3d", params_po_3d_fix, {wolf_dir});
+    auto lmk_fac     = FactoryLandmark::create("Landmark3d", params_po_3d_fac, {wolf_dir});
+    auto lmk_fix_ig  = FactoryLandmark::create("Landmark3d", params_po_3d_fix_ig, {wolf_dir});
+    auto lmk_fix_fac = FactoryLandmark::create("Landmark3d", params_po_3d_fix_fac, {wolf_dir});
+
+    // Checks
+    checkLandmark(lmk_ig, 3, true, "initial_guess", "initial_guess");
+    checkLandmark(lmk_fix, 3, true, "fix", "fix");
+    checkLandmark(lmk_fac, 3, true, "factor", "factor");
+    checkLandmark(lmk_fix_ig, 3, true, "fix", "initial_guess");
+    checkLandmark(lmk_fix_fac, 3, true, "fix", "factor");
+}
 
 ////////////////////////////////////////////////////////////////////
 /////////////////////////// OTHERS /////////////////////////////////
@@ -884,12 +530,32 @@ TEST(Landmark2dTest, create) {}
 
 TEST(Landmark2dTest, ExternalIdAndType)
 {
-    // TODO
+    params_p_3d_ig["external_id"] = 1;
+    params_p_3d_ig["external_type"] = 2;
+    params_p_3d_fix["external_id"] = 3;
+    params_p_3d_fix["external_type"] = 4;
+    params_p_3d_fac["external_id"] = 5;
+    params_p_3d_fac["external_type"] = 6;
+
+    auto lmk_ig  = FactoryLandmark::create("Landmark3d", params_p_3d_ig, {wolf_dir});
+    auto lmk_fix = FactoryLandmark::create("Landmark3d", params_p_3d_fix, {wolf_dir});
+    auto lmk_fac = FactoryLandmark::create("Landmark3d", params_p_3d_fac, {wolf_dir});
+
+    ASSERT_EQ(lmk_ig->getExternalId(), 1);
+    ASSERT_EQ(lmk_ig->getExternalType(), 2);
+    ASSERT_EQ(lmk_fix->getExternalId(), 3);
+    ASSERT_EQ(lmk_fix->getExternalType(), 4);
+    ASSERT_EQ(lmk_fac->getExternalId(), 5);
+    ASSERT_EQ(lmk_fac->getExternalType(), 6);
 }
 
-TEST(Landmark2dTest, ToYaml)
+TEST(Landmark2dTest, ToYamlAndCreate)
 {
-    // CHECK THAT FROM YAML NODES --> same Node
+    auto lmk_fix_fac = FactoryLandmark::create("Landmark3d", params_po_3d_fix_fac, {wolf_dir});
+
+    YAML::Node lmk_node = lmk_fix_fac->toYaml();
+
+    auto lmk2 = FactoryLandmark::create("Landmark3d", lmk_node, {wolf_dir});
 }
 
 int main(int argc, char **argv)
-- 
GitLab