From 82e2e3668c0a2370159f06c4743bddf81cf3151b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 5 Feb 2025 13:02:41 +0100
Subject: [PATCH] [skip ci] working on gtest landmark
---
schema/state/StateO2d.schema | 2 +-
schema/state/StateO3d.schema | 2 +-
schema/state/StateP3d.schema | 2 +-
schema/state/StateV3d.schema | 2 +-
test/CMakeLists.txt | 2 +
test/gtest_landmark.cpp | 899 +++++++++++++++++++++++++++++++++++
6 files changed, 905 insertions(+), 4 deletions(-)
create mode 100644 test/gtest_landmark.cpp
diff --git a/schema/state/StateO2d.schema b/schema/state/StateO2d.schema
index e7473a5a6..3a79d011c 100644
--- a/schema/state/StateO2d.schema
+++ b/schema/state/StateO2d.schema
@@ -1,6 +1,6 @@
value:
_type: Vector1d
- _mandatory: false
+ _mandatory: true
_doc: A vector containing the orientation, yaw [rad].
prior:
diff --git a/schema/state/StateO3d.schema b/schema/state/StateO3d.schema
index a11e36a8c..e5b1b4fcd 100644
--- a/schema/state/StateO3d.schema
+++ b/schema/state/StateO3d.schema
@@ -1,6 +1,6 @@
value:
_type: Vector4d
- _mandatory: false
+ _mandatory: true
_doc: A vector containing the quaternion values (x, y, z, w)
prior:
diff --git a/schema/state/StateP3d.schema b/schema/state/StateP3d.schema
index 84121396e..b52f7fea6 100644
--- a/schema/state/StateP3d.schema
+++ b/schema/state/StateP3d.schema
@@ -1,6 +1,6 @@
value:
_type: Vector3d
- _mandatory: false
+ _mandatory: true
_doc: A vector containing the position (x, y, z) [m].
prior:
diff --git a/schema/state/StateV3d.schema b/schema/state/StateV3d.schema
index c16350c2d..8638f74fb 100644
--- a/schema/state/StateV3d.schema
+++ b/schema/state/StateV3d.schema
@@ -1,6 +1,6 @@
value:
_type: Vector3d
- _mandatory: false
+ _mandatory: true
_doc: A vector containing the velocity (x, y, z) [m/s].
prior:
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c6e89aadf..9cac5510f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -82,6 +82,8 @@ wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
# GraphSearch class test
wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
+# Landmark classes test
+wolf_add_gtest(gtest_landmark gtest_landmark.cpp)
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_loader gtest_loader.cpp)
diff --git a/test/gtest_landmark.cpp b/test/gtest_landmark.cpp
new file mode 100644
index 000000000..1e78b39a4
--- /dev/null
+++ b/test/gtest_landmark.cpp
@@ -0,0 +1,899 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024,2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#include <core/utils/utils_gtest.h>
+
+#include "core/landmark/landmark.h"
+
+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;
+
+VectorComposite vectors_p_2d({{'P', p_2d}});
+VectorComposite vectors_po_2d({{'P', p_2d}, {'O', o_2d}});
+VectorComposite vectors_p_3d({{'P', p_3d}});
+VectorComposite vectors_po_3d({{'P', p_3d}, {'O', o_3d}});
+
+PriorComposite priors_p_ig{{'P', Prior("initial_guess")}};
+PriorComposite priors_p_fix{{'P', Prior("fix")}};
+PriorComposite priors_p_fac_2d{{'P', Prior("factor", Vector2d::Ones())}};
+PriorComposite priors_p_fac_3d{{'P', Prior("factor", Vector3d::Ones())}};
+PriorComposite priors_po_ig{{'P', Prior("initial_guess")}, {'O', Prior("initial_guess")}};
+PriorComposite priors_po_fix{{'P', Prior("fix")}, {'O', Prior("fix")}};
+PriorComposite priors_po_fac_2d{{'P', Prior("factor", Vector2d::Ones())}, {'O', Prior("factor", Vector1d::Ones())}};
+PriorComposite priors_po_fac_3d{{'P', Prior("factor", Vector3d::Ones())}, {'O', Prior("factor", Vector3d::Ones())}};
+PriorComposite priors_po_fix_ig{{'P', Prior("fix")}, {'O', Prior("initial_guess")}};
+PriorComposite priors_po_fix_fac_2d{{'P', Prior("fix")}, {'O', Prior("factor", Vector1d::Ones())}};
+PriorComposite priors_po_fix_fac_3d{{'P', Prior("fix")}, {'O', Prior("factor", Vector3d::Ones())}};
+
+YAML::Node params_p_2d_ig, params_p_2d_fix, params_p_2d_fac, params_p_3d_ig, params_p_3d_fix, params_p_3d_fac,
+ params_po_2d_ig, params_po_2d_fix, params_po_2d_fac, params_po_2d_fix_ig, params_po_2d_fix_fac, params_po_3d_ig,
+ params_po_3d_fix, params_po_3d_fac, params_po_3d_fix_ig, params_po_3d_fix_fac;
+
+void fillYamlNodes()
+{
+ // P 2D
+ params_p_2d_ig["type"] = "Landmark2d";
+ params_p_2d_ig["plugin"] = "core";
+ params_p_2d_ig["id"] = 1;
+ params_p_2d_ig["states"]["P"]["value"] = p_2d;
+ params_p_2d_ig["states"]["P"]["prior"]["mode"] = "initial_guess";
+
+ params_p_2d_fix = Clone(params_p_2d_ig);
+ params_p_2d_fix["states"]["P"]["prior"]["mode"] = "fix";
+
+ params_p_2d_fac = Clone(params_p_2d_fix);
+ params_p_2d_fac["states"]["P"]["prior"]["mode"] = "factor";
+ params_p_2d_fac["states"]["P"]["prior"]["factor_std"] = Vector2d::Ones();
+
+ // PO 2D
+ params_po_2d_ig = Clone(params_p_2d_ig);
+ params_po_2d_ig["states"]["O"]["value"] = o_2d;
+ params_po_2d_ig["states"]["O"]["prior"]["mode"] = "initial_guess";
+
+ params_po_2d_fix = Clone(params_p_2d_fix);
+ params_po_2d_fix["states"]["O"]["value"] = o_2d;
+ params_po_2d_fix["states"]["O"]["prior"]["mode"] = "fix";
+
+ params_po_2d_fac = Clone(params_p_2d_fac);
+ params_po_2d_fac["states"]["O"]["value"] = o_2d;
+ params_po_2d_fac["states"]["O"]["prior"]["mode"] = "factor";
+ params_po_2d_fac["states"]["O"]["prior"]["factor_std"] = Vector1d::Ones();
+
+ params_po_2d_fix_ig = Clone(params_p_2d_fix);
+ params_po_2d_fix_ig["states"]["O"]["value"] = o_2d;
+ params_po_2d_fix_ig["states"]["O"]["prior"]["mode"] = "initial_guess";
+
+ params_po_2d_fix_fac = Clone(params_p_2d_fix);
+ params_po_2d_fix_fac["states"]["O"]["value"] = o_2d;
+ params_po_2d_fix_fac["states"]["O"]["prior"]["mode"] = "factor";
+ params_po_2d_fix_fac["states"]["O"]["prior"]["factor_std"] = Vector1d::Ones();
+
+ // P 3D
+ params_p_3d_ig = Clone(params_p_2d_ig);
+ params_p_3d_ig["type"] = "Landmark3d";
+ params_p_3d_ig["states"]["P"]["value"] = p_3d;
+
+ params_p_3d_fix = Clone(params_p_3d_ig);
+ params_p_3d_fix["states"]["P"]["prior"]["mode"] = "fix";
+
+ params_p_3d_fac = Clone(params_p_3d_fix);
+ params_p_3d_fac["states"]["P"]["prior"]["mode"] = "factor";
+ params_p_3d_fac["states"]["P"]["prior"]["factor_std"] = Vector3d::Ones();
+
+ // PO 3D
+ params_po_3d_ig = Clone(params_p_3d_ig);
+ params_po_3d_ig["states"]["O"]["value"] = o_3d;
+ params_po_3d_ig["states"]["O"]["prior"]["mode"] = "initial_guess";
+
+ params_po_3d_fix = Clone(params_p_3d_fix);
+ params_po_3d_fix["states"]["O"]["value"] = o_3d;
+ params_po_3d_fix["states"]["O"]["prior"]["mode"] = "fix";
+
+ params_po_3d_fac = Clone(params_p_3d_fac);
+ params_po_3d_fac["states"]["O"]["value"] = o_3d;
+ params_po_3d_fac["states"]["O"]["prior"]["mode"] = "factor";
+ params_po_3d_fac["states"]["O"]["prior"]["factor_std"] = Vector3d::Ones();
+
+ params_po_3d_fix_ig = Clone(params_p_3d_fix);
+ params_po_3d_fix_ig["states"]["O"]["value"] = o_3d;
+ params_po_3d_fix_ig["states"]["O"]["prior"]["mode"] = "initial_guess";
+
+ params_po_3d_fix_fac = Clone(params_p_3d_fix);
+ params_po_3d_fix_fac["states"]["O"]["value"] = o_3d;
+ params_po_3d_fix_fac["states"]["O"]["prior"]["mode"] = "factor";
+ params_po_3d_fix_fac["states"]["O"]["prior"]["factor_std"] = Vector3d::Ones();
+}
+
+////////////////////////////////////////////////////////////////////
+//////////////////// CONSTRUCTOR COMPOSITES ////////////////////////
+////////////////////////////////////////////////////////////////////
+TEST(LandmarkTest, ConstructorComposite_P2D)
+{
+ // P 2D
+ auto lmk = std::make_shared<Landmark2d>(vectors_p_2d);
+ auto lmk_ig = std::make_shared<Landmark2d>(vectors_p_2d, priors_p_ig);
+ auto lmk_fix = std::make_shared<Landmark2d>(vectors_p_2d, priors_p_fix);
+ auto lmk_fac = std::make_shared<Landmark2d>(vectors_p_2d, priors_p_fac_2d);
+
+ // constructor does not emplace priors
+ lmk->emplacePriors();
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ 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());
+}
+
+TEST(LandmarkTest, ConstructorComposite_PO2D)
+{
+ // PO 2D
+ auto lmk = std::make_shared<Landmark2d>(vectors_po_2d);
+ auto lmk_ig = std::make_shared<Landmark2d>(vectors_po_2d, priors_po_ig);
+ auto lmk_fix = std::make_shared<Landmark2d>(vectors_po_2d, priors_po_fix);
+ auto lmk_fac = std::make_shared<Landmark2d>(vectors_po_2d, priors_po_fac_2d);
+ auto lmk_fix_ig = std::make_shared<Landmark2d>(vectors_po_2d, priors_po_fix_ig);
+ auto lmk_fix_fac = std::make_shared<Landmark2d>(vectors_po_2d, priors_po_fix_fac_2d);
+
+ // constructor does not emplace priors
+ lmk->emplacePriors();
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ lmk_fac->emplacePriors();
+ lmk_fix_ig->emplacePriors();
+ 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());
+}
+
+TEST(LandmarkTest, ConstructorComposite_P3D)
+{
+ // P 3D
+ auto lmk = std::make_shared<Landmark3d>(vectors_p_3d);
+ auto lmk_ig = std::make_shared<Landmark3d>(vectors_p_3d, priors_p_ig);
+ auto lmk_fix = std::make_shared<Landmark3d>(vectors_p_3d, priors_p_fix);
+ auto lmk_fac = std::make_shared<Landmark3d>(vectors_p_3d, priors_p_fac_3d);
+
+ // constructor does not emplace priors
+ lmk->emplacePriors();
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ 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());
+}
+
+TEST(LandmarkTest, ConstructorComposite_PO3D)
+{
+ // PO 3D
+ auto lmk = std::make_shared<Landmark3d>(vectors_po_3d);
+ auto lmk_ig = std::make_shared<Landmark3d>(vectors_po_3d, priors_po_ig);
+ auto lmk_fix = std::make_shared<Landmark3d>(vectors_po_3d, priors_po_fix);
+ auto lmk_fac = std::make_shared<Landmark3d>(vectors_po_3d, priors_po_fac_3d);
+ auto lmk_fix_ig = std::make_shared<Landmark3d>(vectors_po_3d, priors_po_fix_ig);
+ auto lmk_fix_fac = std::make_shared<Landmark3d>(vectors_po_3d, priors_po_fix_fac_3d);
+
+ // constructor does not emplace priors
+ lmk->emplacePriors();
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ lmk_fac->emplacePriors();
+ lmk_fix_ig->emplacePriors();
+ 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());
+}
+
+////////////////////////////////////////////////////////////////////
+//////////////////// CONSTRUCTOR YAML NODE /////////////////////////
+////////////////////////////////////////////////////////////////////
+TEST(LandmarkTest, ConstructorYamlNode_P2D)
+{
+ fillYamlNodes();
+
+ auto lmk_ig = std::make_shared<Landmark2d>(params_p_2d_ig);
+ auto lmk_fix = std::make_shared<Landmark2d>(params_p_2d_fix);
+ auto lmk_fac = std::make_shared<Landmark2d>(params_p_2d_fac);
+
+ // constructor does not emplace priors
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ 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());
+}
+
+TEST(LandmarkTest, ConstructorYamlNode_PO2D)
+{
+ auto lmk_ig = std::make_shared<Landmark2d>(params_po_2d_ig);
+ auto lmk_fix = std::make_shared<Landmark2d>(params_po_2d_fix);
+ auto lmk_fac = std::make_shared<Landmark2d>(params_po_2d_fac);
+ auto lmk_fix_ig = std::make_shared<Landmark2d>(params_po_2d_fix_ig);
+ auto lmk_fix_fac = std::make_shared<Landmark2d>(params_po_2d_fix_fac);
+
+ // constructor does not emplace priors
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ lmk_fac->emplacePriors();
+ lmk_fix_ig->emplacePriors();
+ 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());
+}
+
+TEST(LandmarkTest, ConstructorYamlNode_P3D)
+{
+ auto lmk_ig = std::make_shared<Landmark3d>(params_p_3d_ig);
+ auto lmk_fix = std::make_shared<Landmark3d>(params_p_3d_fix);
+ auto lmk_fac = std::make_shared<Landmark3d>(params_p_3d_fac);
+
+ // constructor does not emplace priors
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ 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());
+}
+
+TEST(LandmarkTest, ConstructorYamlNode_PO3D)
+{
+ auto lmk_ig = std::make_shared<Landmark3d>(params_po_3d_ig);
+ auto lmk_fix = std::make_shared<Landmark3d>(params_po_3d_fix);
+ auto lmk_fac = std::make_shared<Landmark3d>(params_po_3d_fac);
+ auto lmk_fix_ig = std::make_shared<Landmark3d>(params_po_3d_fix_ig);
+ auto lmk_fix_fac = std::make_shared<Landmark3d>(params_po_3d_fix_fac);
+
+ // constructor does not emplace priors
+ lmk_ig->emplacePriors();
+ lmk_fix->emplacePriors();
+ lmk_fac->emplacePriors();
+ lmk_fix_ig->emplacePriors();
+ 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());
+}
+
+////////////////////////////////////////////////////////////////////
+///////////////////// EMPLACE COMPOSITE ////////////////////////////
+////////////////////////////////////////////////////////////////////
+TEST(LandmarkTest, EmplaceComposite_P2D)
+{
+ // P 2D
+ auto lmk = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_p_2d);
+ auto lmk_ig = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_p_2d, priors_p_ig);
+ auto lmk_fix = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_p_2d, priors_p_fix);
+ 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());
+}
+
+TEST(LandmarkTest, EmplaceComposite_PO2D)
+{
+ // PO 2D
+ auto lmk = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_po_2d);
+ auto lmk_ig = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_po_2d, priors_po_ig);
+ auto lmk_fix = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_po_2d, priors_po_fix);
+ auto lmk_fac = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_po_2d, priors_po_fac_2d);
+ auto lmk_fix_ig = LandmarkBase::emplace<Landmark2d>(nullptr, vectors_po_2d, priors_po_fix_ig);
+ 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());
+}
+
+TEST(LandmarkTest, EmplaceComposite_P3D)
+{
+ // P 3D
+ auto lmk = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_p_3d);
+ auto lmk_ig = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_p_3d, priors_p_ig);
+ auto lmk_fix = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_p_3d, priors_p_fix);
+ 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());
+}
+
+TEST(LandmarkTest, EmplaceComposite_PO3D)
+{
+ // PO 3D
+ auto lmk = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_po_3d);
+ auto lmk_ig = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_po_3d, priors_po_ig);
+ auto lmk_fix = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_po_3d, priors_po_fix);
+ auto lmk_fac = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_po_3d, priors_po_fac_3d);
+ auto lmk_fix_ig = LandmarkBase::emplace<Landmark3d>(nullptr, vectors_po_3d, priors_po_fix_ig);
+ 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());
+}
+
+////////////////////////////////////////////////////////////////////
+///////////////////// EMPLACE YAML NODES ///////////////////////////
+////////////////////////////////////////////////////////////////////
+
+// TODO
+TEST(Landmark2dTest, emplaceYamlNode) {}
+
+////////////////////////////////////////////////////////////////////
+/////////////////////////// CREATE /////////////////////////////////
+////////////////////////////////////////////////////////////////////
+
+// TODO
+TEST(Landmark2dTest, create) {}
+
+////////////////////////////////////////////////////////////////////
+/////////////////////////// OTHERS /////////////////////////////////
+////////////////////////////////////////////////////////////////////
+
+TEST(Landmark2dTest, ExternalIdAndType)
+{
+ // TODO
+}
+
+TEST(Landmark2dTest, ToYaml)
+{
+ // CHECK THAT FROM YAML NODES --> same Node
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
\ No newline at end of file
--
GitLab