diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index efb89ade080ff6f322fca52d4476587f1578f894..fe61ea7930cdf1b12aeaf23c806774ac4c3b8364 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -148,6 +148,13 @@ ENDIF(vision_utils_FOUND)
wolf_add_gtest(gtest_constraint_gnss_fix_2D gtest_constraint_gnss_fix_2D.cpp)
target_link_libraries(gtest_constraint_gnss_fix_2D ${PROJECT_NAME})
+# ConstraintGnssFix2D test
+wolf_add_gtest(gtest_constraint_gnss_fix_2D_1 gtest_constraint_gnss_fix_2D_1.cpp)
+target_link_libraries(gtest_constraint_gnss_fix_2D_1 ${PROJECT_NAME})
+# ConstraintGnssFix2D test
+wolf_add_gtest(gtest_constraint_gnss_fix_2D_2 gtest_constraint_gnss_fix_2D_2.cpp)
+target_link_libraries(gtest_constraint_gnss_fix_2D_2 ${PROJECT_NAME})
+
# ConstraintGnssFix2D test
wolf_add_gtest(gtest_constraint_gnss_fix_2D_simple gtest_constraint_gnss_fix_2D_simple.cpp)
target_link_libraries(gtest_constraint_gnss_fix_2D_simple ${PROJECT_NAME})
diff --git a/test/gtest_constraint_gnss_fix_2D_1.cpp b/test/gtest_constraint_gnss_fix_2D_1.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3eac818ce578912b00e2461d5b8b37eb2afcafb5
--- /dev/null
+++ b/test/gtest_constraint_gnss_fix_2D_1.cpp
@@ -0,0 +1,155 @@
+/**
+ * \file gtest_constraint_gnss_fix_2D.cpp
+ *
+ * Created on: Aug 1, 2018
+ * \author: jvallve
+ */
+
+
+#include "base/constraint/constraint_gnss_fix_2D.h"
+#include "utils_gtest.h"
+
+#include "base/capture/capture_motion.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_gnss.h"
+#include "base/processor/processor_gnss_fix.h"
+
+#include "base/ceres_wrapper/ceres_manager.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+
+class ConstraintGnssFix2DTest : public testing::Test
+{
+ public:
+
+ // groundtruth transformations
+ Vector3s t_ecef_enu;
+ Matrix3s R_ecef_enu, R_enu_map, R_map_base;
+ Vector3s t_base_antena, t_ecef_antena;
+ Vector1s o_enu_map;
+ Vector3s t_enu_map;
+ Vector3s t_map_base;
+ Vector1s o_map_base;
+
+ // WOLF
+ ProblemPtr problem_ptr;
+ CeresManagerPtr ceres_mgr_ptr;
+ SensorGnssPtr gnss_sensor_ptr;
+
+ ConstraintGnssFix2DTest()
+ {
+ o_enu_map << 2.6;
+ t_enu_map << 12, -34, 4;
+ t_map_base << 32, -34, 0; // z=0
+ o_map_base << -0.4;
+ t_base_antena << 3,-2,8;// Antena extrinsics
+ t_ecef_enu << 100,30,50;
+ R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX())
+ *AngleAxis<Scalar>(-2.2, Vector3s::UnitY())
+ *AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
+ R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
+ R_map_base = Matrix3s::Identity();
+ R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
+ t_ecef_antena = R_ecef_enu * R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_ecef_enu;
+
+ // ----------------------------------------------------
+ // Problem and solver
+ problem_ptr = Problem::create("PO 2D");
+ ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
+ ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
+
+ // gnss sensor & processor
+ gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
+ gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map);
+ gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map);
+ gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
+ gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
+ gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+ std::cout << "gnss sensor installed" << std::endl;
+ ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
+ gnss_params_ptr->time_tolerance = -1.0;
+ gnss_params_ptr->voting_active = true;
+ problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
+ std::cout << "gnss processor installed" << std::endl;
+
+ // set prior (FIXED)
+ Eigen::Vector3s frame_pose;
+ frame_pose << t_map_base.head(2), o_map_base;
+ FrameBasePtr prior_frame_ptr = problem_ptr->setPrior(frame_pose, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
+ prior_frame_ptr->fix();
+ };
+
+ virtual void SetUp()
+ {
+ // reset grountruth parameters
+ R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
+ R_map_base = Matrix3s::Identity();
+ R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
+ t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+
+ // Reset antena extrinsics
+ gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
+
+ // Reset ENU_ECEF
+ gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+ }
+};
+
+////////////////////////////////////////////////////////
+
+TEST_F(ConstraintGnssFix2DTest, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST_F(ConstraintGnssFix2DTest, gnss_1_map_base_position)
+{
+ // Create GNSS Fix capture
+ CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
+ gnss_sensor_ptr->process(cap_gnss_ptr);
+
+ // fixing things
+ gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix();// enu-map yaw translation
+ gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();// enu-map yaw orientation
+ cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base); // frame orientation
+ cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
+
+ // distort frm position
+ Vector3s frm_dist = cap_gnss_ptr->getFramePtr()->getState();
+ frm_dist(0) += 0.18;
+ frm_dist(1) += -0.58;
+ cap_gnss_ptr->getFramePtr()->setState(frm_dist);
+
+ // solve for frm
+ std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 2);
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+
+ ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey());
+ ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getState().head(2), t_map_base.head(2), 1e-6);
+
+ cap_gnss_ptr->getFramePtr()->remove();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_constraint_gnss_fix_2D_2.cpp b/test/gtest_constraint_gnss_fix_2D_2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3eac818ce578912b00e2461d5b8b37eb2afcafb5
--- /dev/null
+++ b/test/gtest_constraint_gnss_fix_2D_2.cpp
@@ -0,0 +1,155 @@
+/**
+ * \file gtest_constraint_gnss_fix_2D.cpp
+ *
+ * Created on: Aug 1, 2018
+ * \author: jvallve
+ */
+
+
+#include "base/constraint/constraint_gnss_fix_2D.h"
+#include "utils_gtest.h"
+
+#include "base/capture/capture_motion.h"
+#include "base/problem.h"
+#include "base/sensor/sensor_gnss.h"
+#include "base/processor/processor_gnss_fix.h"
+
+#include "base/ceres_wrapper/ceres_manager.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+
+class ConstraintGnssFix2DTest : public testing::Test
+{
+ public:
+
+ // groundtruth transformations
+ Vector3s t_ecef_enu;
+ Matrix3s R_ecef_enu, R_enu_map, R_map_base;
+ Vector3s t_base_antena, t_ecef_antena;
+ Vector1s o_enu_map;
+ Vector3s t_enu_map;
+ Vector3s t_map_base;
+ Vector1s o_map_base;
+
+ // WOLF
+ ProblemPtr problem_ptr;
+ CeresManagerPtr ceres_mgr_ptr;
+ SensorGnssPtr gnss_sensor_ptr;
+
+ ConstraintGnssFix2DTest()
+ {
+ o_enu_map << 2.6;
+ t_enu_map << 12, -34, 4;
+ t_map_base << 32, -34, 0; // z=0
+ o_map_base << -0.4;
+ t_base_antena << 3,-2,8;// Antena extrinsics
+ t_ecef_enu << 100,30,50;
+ R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX())
+ *AngleAxis<Scalar>(-2.2, Vector3s::UnitY())
+ *AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
+ R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
+ R_map_base = Matrix3s::Identity();
+ R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
+ t_ecef_antena = R_ecef_enu * R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_ecef_enu;
+
+ // ----------------------------------------------------
+ // Problem and solver
+ problem_ptr = Problem::create("PO 2D");
+ ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
+ ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
+
+ // gnss sensor & processor
+ gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
+ gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map);
+ gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map);
+ gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
+ gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
+ gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+ std::cout << "gnss sensor installed" << std::endl;
+ ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
+ gnss_params_ptr->time_tolerance = -1.0;
+ gnss_params_ptr->voting_active = true;
+ problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
+ std::cout << "gnss processor installed" << std::endl;
+
+ // set prior (FIXED)
+ Eigen::Vector3s frame_pose;
+ frame_pose << t_map_base.head(2), o_map_base;
+ FrameBasePtr prior_frame_ptr = problem_ptr->setPrior(frame_pose, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
+ prior_frame_ptr->fix();
+ };
+
+ virtual void SetUp()
+ {
+ // reset grountruth parameters
+ R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
+ R_map_base = Matrix3s::Identity();
+ R_map_base.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base(0)).matrix();
+ t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+
+ // Reset antena extrinsics
+ gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
+
+ // Reset ENU_ECEF
+ gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+ }
+};
+
+////////////////////////////////////////////////////////
+
+TEST_F(ConstraintGnssFix2DTest, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+/*
+ * Test with one GNSS fix capture.
+ *
+ * Estimating: MAP_BASE position.
+ * Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
+ */
+TEST_F(ConstraintGnssFix2DTest, gnss_1_map_base_position)
+{
+ // Create GNSS Fix capture
+ CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
+ gnss_sensor_ptr->process(cap_gnss_ptr);
+
+ // fixing things
+ gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix();// enu-map yaw translation
+ gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();// enu-map yaw orientation
+ cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base); // frame orientation
+ cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
+
+ // distort frm position
+ Vector3s frm_dist = cap_gnss_ptr->getFramePtr()->getState();
+ frm_dist(0) += 0.18;
+ frm_dist(1) += -0.58;
+ cap_gnss_ptr->getFramePtr()->setState(frm_dist);
+
+ // solve for frm
+ std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
+
+ std::cout << report << std::endl;
+
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 2);
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
+ ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+
+ ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey());
+ ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getState().head(2), t_map_base.head(2), 1e-6);
+
+ cap_gnss_ptr->getFramePtr()->remove();
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+