Skip to content
Snippets Groups Projects
Commit 96eb1f67 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

landmark test completed and working

parent 82e2e366
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #20592 failed
Stage: license
    Stage: clang
      Stage: yaml_templates
        Stage: build_and_test
          Stage: deploy_plugins
            Stage: deploy_ros
              Stage: deploy_ros2
                Hide whitespace changes
                Inline Side-by-side
                test/gtest_landmark.cpp
                + 216
                − 550
                View file @ 96eb1f67
                ......@@ -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)
                ......
                0% or .
                You are about to add 0 people to the discussion. Proceed with caution.
                Finish editing this message first!
                Please register or to comment