From a5b6ac843d006a26c74a1122bb24e5608a5c5495 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 4 Mar 2020 11:14:30 +0100
Subject: [PATCH] WIP
---
CMakeLists.txt | 15 ++-
include/gnss_utils/gnss_utils.h | 9 +-
src/CMakeLists.txt | 9 +-
src/examples/gnss_utils_test.cpp | 4 +-
src/gnss_utils.cpp | 134 +++++++++++++++++++++++++-
src/test/CMakeLists.txt | 17 ++++
src/test/gtest/CMakeLists.txt | 65 +++++++++++++
src/test/gtest/utils_gtest.h | 146 +++++++++++++++++++++++++++++
src/test/gtest_example.cpp | 20 ++++
src/test/gtest_transformations.cpp | 33 +++++++
10 files changed, 441 insertions(+), 11 deletions(-)
create mode 100644 src/test/CMakeLists.txt
create mode 100644 src/test/gtest/CMakeLists.txt
create mode 100644 src/test/gtest/utils_gtest.h
create mode 100644 src/test/gtest_example.cpp
create mode 100644 src/test/gtest_transformations.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0da1a8..89e9567 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,8 +39,21 @@ endif()
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fpermissive")
+IF(NOT BUILD_TESTS)
+ OPTION(BUILD_TESTS "Build Unit tests" ON)
+ENDIF(NOT BUILD_TESTS)
+
ADD_SUBDIRECTORY(src)
+if(BUILD_TESTS)
+ # Enables testing for this directory and below.
+ # Note that ctest expects to find a test file in the build directory root.
+ # Therefore, this command should be in the source directory root.
+ #include(CTest) # according to http://public.kitware.com/pipermail/cmake/2012-June/050853.html
+ enable_testing()
+ MESSAGE("Building tests.")
+endif()
+
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
@@ -81,4 +94,4 @@ ELSE(UNIX)
COMMENT "uninstall only implemented in unix"
TARGET uninstall
)
-ENDIF(UNIX)
+ENDIF(UNIX)
\ No newline at end of file
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 1a555b0..e90e6cc 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -56,8 +56,13 @@ namespace GNSSUtils
const prcopt_t *opt, sol_t *sol, double *azel, int *vsat,
double *resp, char *msg);
- Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
- Eigen::Vector3d latLonToEcef(const Eigen::Vector3d & _latlon);
+ Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
+ Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
+ Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
+ Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
+
+ void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
+ void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 948e41e..50ebf67 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -61,4 +61,11 @@ INSTALL(TARGETS ${PROJECT_NAME}
INSTALL(FILES ${HEADERS} DESTINATION include/iri-algorithms/gnss_utils)
INSTALL(FILES ../Findgnss_utils.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
-ADD_SUBDIRECTORY(examples)
+# Testing
+if(BUILD_TESTS)
+ MESSAGE("Building tests.")
+ add_subdirectory(test)
+endif()
+
+# Examples
+ADD_SUBDIRECTORY(examples)
\ No newline at end of file
diff --git a/src/examples/gnss_utils_test.cpp b/src/examples/gnss_utils_test.cpp
index ab9c2cf..fb0792a 100644
--- a/src/examples/gnss_utils_test.cpp
+++ b/src/examples/gnss_utils_test.cpp
@@ -88,7 +88,7 @@ int main(int argc, char *argv[])
std::cout << "msg: " << msg << std::endl;
std::cout << "sol.stat: " << int(solb.stat) << std::endl;
std::cout << "Position: " << solb.rr[0] << ", " << solb.rr[1] << ", " << solb.rr[2] << std::endl;
- std::cout << "Position (GT): " << latLonToEcef(lla_gt).transpose() << std::endl;
- std::cout << "Position LLA: " << ecefToLatLon(Eigen::Vector3d(solb.rr[0],solb.rr[1],solb.rr[2])).transpose() << std::endl;
+ std::cout << "Position (GT): " << latLonAltToEcef(lla_gt).transpose() << std::endl;
+ std::cout << "Position LLA: " << ecefToLatLonAlt(Eigen::Vector3d(solb.rr[0],solb.rr[1],solb.rr[2])).transpose() << std::endl;
std::cout << "Position LLA (GT): " << lla_gt.transpose() << std::endl;
}
diff --git a/src/gnss_utils.cpp b/src/gnss_utils.cpp
index 5f55ef8..e4336e5 100644
--- a/src/gnss_utils.cpp
+++ b/src/gnss_utils.cpp
@@ -1,6 +1,5 @@
#include "gnss_utils/gnss_utils.h"
-
namespace GNSSUtils
{
@@ -48,7 +47,7 @@ namespace GNSSUtils
output.ns = sol.ns;
output.age = sol.age;
output.ratio = sol.ratio;
- output.lat_lon = ecefToLatLon(output.pos);
+ output.lat_lon = ecefToLatLonAlt(output.pos);
return output;
}
@@ -97,7 +96,7 @@ namespace GNSSUtils
output.ns = sol.ns;
output.age = sol.age;
output.ratio = sol.ratio;
- output.lat_lon = ecefToLatLon(output.pos);
+ output.lat_lon = ecefToLatLonAlt(output.pos);
return output;
}
@@ -247,7 +246,7 @@ namespace GNSSUtils
return 0;
}
-Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
+Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef)
{
Eigen::Vector3d pos;
ecef2pos(_ecef.data(), pos.data());
@@ -255,7 +254,7 @@ Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
return pos;
}
-Eigen::Vector3d latLonToEcef(const Eigen::Vector3d & _pos)
+Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _pos)
{
Eigen::Vector3d ecef;
pos2ecef(_pos.data(), ecef.data());
@@ -263,6 +262,131 @@ Eigen::Vector3d latLonToEcef(const Eigen::Vector3d & _pos)
return ecef;
}
+
+Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef)
+{
+ Eigen::Matrix3d cov_enu;
+
+ /* RTKLIB transform covariance to local tangental coordinate --------------------------
+ * transform ecef covariance to local tangental coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *P I covariance in ecef coordinate
+ * double *Q O covariance in local tangental coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ //extern void covenu(const double *pos, const double *P, double *Q);
+ covenu(_latlon.data(), _cov_ecef.data(), cov_enu.data());
+
+ return cov_enu;
+}
+
+Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu)
+{
+ Eigen::Matrix3d cov_ecef;
+
+ /* RTKLIB transform local enu coordinate covariance to xyz-ecef -----------------------
+ * transform local enu covariance to xyz-ecef coordinate
+ * args : double *pos I geodetic position {lat,lon} (rad)
+ * double *Q I covariance in local enu coordinate
+ * double *P O covariance in xyz-ecef coordinate
+ * return : none
+ *-----------------------------------------------------------------------------*/
+ //extern void covecef(const double *pos, const double *Q, double *P)
+ covecef(_latlon.data(), _cov_enu.data(), cov_ecef.data());
+
+ return cov_ecef;
+}
+
+void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF)
+{
+ // Convert ECEF coordinates to geodetic coordinates.
+ // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
+ // to geodetic coordinates," IEEE Transactions on Aerospace and
+ // Electronic Systems, vol. 30, pp. 957-961, 1994.
+
+// double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+// double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+// double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+// double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
+// double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
+// double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+// double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+// double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
+// double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+// + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
+// - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
+// double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2));
+// double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+//
+// double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+// double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+//
+//
+// double sLat = sin(latitude);
+// double cLat = cos(latitude);
+// double sLon = sin(longitude);
+// double cLon = cos(longitude);
+//
+// R_ENU_ECEF(0,0) = -sLon;
+// R_ENU_ECEF(0,1) = cLon;
+// R_ENU_ECEF(0,2) = 0.0;
+//
+// R_ENU_ECEF(1,0) = -sLat*cLon;
+// R_ENU_ECEF(1,1) = -sLat * sLon;
+// R_ENU_ECEF(1,2) = cLat;
+//
+// R_ENU_ECEF(2,0) = cLat * cLon;
+// R_ENU_ECEF(2,1) = cLat * sLon;
+// R_ENU_ECEF(2,2) = sLat;
+//
+// t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
+
+ Eigen::Vector3d ENU_lat_lon_alt = GNSSUtils::ecefToLatLonAlt(_t_ECEF_ENU);
+
+ double sLat = sin(ENU_lat_lon_alt(0));
+ double cLat = cos(ENU_lat_lon_alt(0));
+ double sLon = sin(ENU_lat_lon_alt(1));
+ double cLon = cos(ENU_lat_lon_alt(1));
+
+ R_ENU_ECEF(0,0) = -sLon;
+ R_ENU_ECEF(0,1) = cLon;
+ R_ENU_ECEF(0,2) = 0.0;
+
+ R_ENU_ECEF(1,0) = -sLat*cLon;
+ R_ENU_ECEF(1,1) = -sLat * sLon;
+ R_ENU_ECEF(1,2) = cLat;
+
+ R_ENU_ECEF(2,0) = cLat * cLon;
+ R_ENU_ECEF(2,1) = cLat * sLon;
+ R_ENU_ECEF(2,2) = sLat;
+
+ t_ENU_ECEF = -R_ENU_ECEF * _t_ECEF_ENU;
+}
+
+void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF)
+{
+ double sLat = sin(_ENU_latlonalt(0));
+ double cLat = cos(_ENU_latlonalt(0));
+ double sLon = sin(_ENU_latlonalt(1));
+ double cLon = cos(_ENU_latlonalt(1));
+
+ R_ENU_ECEF(0,0) = -sLon;
+ R_ENU_ECEF(0,1) = cLon;
+ R_ENU_ECEF(0,2) = 0.0;
+
+ R_ENU_ECEF(1,0) = -sLat*cLon;
+ R_ENU_ECEF(1,1) = -sLat * sLon;
+ R_ENU_ECEF(1,2) = cLat;
+
+ R_ENU_ECEF(2,0) = cLat * cLon;
+ R_ENU_ECEF(2,1) = cLat * sLon;
+ R_ENU_ECEF(2,2) = sLat;
+
+ Eigen::Vector3d t_ECEF_ENU = latLonAltToEcef(_ENU_latlonalt);
+
+ t_ENU_ECEF = -R_ENU_ECEF * t_ECEF_ENU;
+}
+
double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef)
{
// ecef 2 geodetic
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
new file mode 100644
index 0000000..dcf7deb
--- /dev/null
+++ b/src/test/CMakeLists.txt
@@ -0,0 +1,17 @@
+# Retrieve googletest from github & compile
+add_subdirectory(gtest)
+
+# Include gtest directory.
+include_directories(${GTEST_INCLUDE_DIRS})
+
+############# USE THIS TEST AS AN EXAMPLE ####################
+# #
+# Create a specific test executable for gtest_example #
+# gnss_utils_add_gtest(gtest_example gtest_example.cpp) #
+# target_link_libraries(gtest_example ${PROJECT_NAME}) #
+# #
+##############################################################
+
+# Transformations test
+gnss_utils_add_gtest(gtest_transformations gtest_transformations.cpp)
+target_link_libraries(gtest_transformations ${PROJECT_NAME})
diff --git a/src/test/gtest/CMakeLists.txt b/src/test/gtest/CMakeLists.txt
new file mode 100644
index 0000000..3c3c48c
--- /dev/null
+++ b/src/test/gtest/CMakeLists.txt
@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 2.8.8)
+project(gtest_builder C CXX)
+
+# We need thread support
+#find_package(Threads REQUIRED)
+
+# Enable ExternalProject CMake module
+include(ExternalProject)
+
+set(GTEST_FORCE_SHARED_CRT ON)
+set(GTEST_DISABLE_PTHREADS OFF)
+
+# For some reason I need to disable PTHREADS
+# with g++ (Ubuntu 4.9.3-8ubuntu2~14.04) 4.9.3
+# This is a known issue for MinGW :
+# https://github.com/google/shaderc/pull/174
+#if(MINGW)
+ set(GTEST_DISABLE_PTHREADS ON)
+#endif()
+
+# Download GoogleTest
+ExternalProject_Add(googletest
+ GIT_REPOSITORY https://github.com/google/googletest.git
+ GIT_TAG v1.8.x
+ # TIMEOUT 1 # We'll try this
+ CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
+ -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
+ -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
+ -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
+ -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
+ -DBUILD_GTEST=ON
+ PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
+ # Disable install step
+ INSTALL_COMMAND ""
+ UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
+)
+
+# Get GTest source and binary directories from CMake project
+
+# Specify include dir
+ExternalProject_Get_Property(googletest source_dir)
+set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
+
+# Specify MainTest's link libraries
+ExternalProject_Get_Property(googletest binary_dir)
+set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
+
+# Create a libgtest target to be used as a dependency by test programs
+add_library(libgtest IMPORTED STATIC GLOBAL)
+add_dependencies(libgtest googletest)
+
+# Set libgtest properties
+set_target_properties(libgtest PROPERTIES
+ "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
+ "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
+)
+
+function(gnss_utils_add_gtest target)
+ add_executable(${target} ${ARGN})
+ add_dependencies(${target} libgtest)
+ target_link_libraries(${target} libgtest)
+
+ #WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/bin
+ add_test(NAME ${target} COMMAND ${target})
+endfunction()
diff --git a/src/test/gtest/utils_gtest.h b/src/test/gtest/utils_gtest.h
new file mode 100644
index 0000000..09c2907
--- /dev/null
+++ b/src/test/gtest/utils_gtest.h
@@ -0,0 +1,146 @@
+/**
+ * \file utils_gtest.h
+ * \brief Some utils for gtest
+ * \author Jeremie Deray
+ * Created on: 26/09/2016
+ * Eigen macros extension by: Joan Sola on 26/04/2017
+ */
+
+#ifndef GNSSUTILS_UTILS_GTEST_H
+#define GNSSUTILS_UTILS_GTEST_H
+
+#include <gtest/gtest.h>
+
+// Macros for testing equalities and inequalities.
+//
+// * {ASSERT|EXPECT}_EQ(expected, actual): Tests that expected == actual
+// * {ASSERT|EXPECT}_NE(v1, v2): Tests that v1 != v2
+// * {ASSERT|EXPECT}_LT(v1, v2): Tests that v1 < v2
+// * {ASSERT|EXPECT}_LE(v1, v2): Tests that v1 <= v2
+// * {ASSERT|EXPECT}_GT(v1, v2): Tests that v1 > v2
+// * {ASSERT|EXPECT}_GE(v1, v2): Tests that v1 >= v2
+//
+// C String Comparisons. All tests treat NULL and any non-NULL string
+// as different. Two NULLs are equal.
+//
+// * {ASSERT|EXPECT}_STREQ(s1, s2): Tests that s1 == s2
+// * {ASSERT|EXPECT}_STRNE(s1, s2): Tests that s1 != s2
+// * {ASSERT|EXPECT}_STRCASEEQ(s1, s2): Tests that s1 == s2, ignoring case
+// * {ASSERT|EXPECT}_STRCASENE(s1, s2): Tests that s1 != s2, ignoring case
+//
+// Macros for comparing floating-point numbers.
+//
+// * {ASSERT|EXPECT}_FLOAT_EQ(expected, actual):
+// Tests that two float values are almost equal.
+// * {ASSERT|EXPECT}_DOUBLE_EQ(expected, actual):
+// Tests that two double values are almost equal.
+// * {ASSERT|EXPECT}_NEAR(v1, v2, abs_error):
+// Tests that v1 and v2 are within the given distance to each other.
+//
+// These predicate format functions work on floating-point values, and
+// can be used in {ASSERT|EXPECT}_PRED_FORMAT2*(), e.g.
+//
+// EXPECT_PRED_FORMAT2(testing::DoubleLE, Foo(), 5.0);
+//
+// Macros that execute statement and check that it doesn't generate new fatal
+// failures in the current thread.
+//
+// * {ASSERT|EXPECT}_NO_FATAL_FAILURE(statement);
+
+// http://stackoverflow.com/a/29155677
+
+namespace testing
+{
+namespace internal
+{
+enum GTestColor
+{
+ COLOR_DEFAULT,
+ COLOR_RED,
+ COLOR_GREEN,
+ COLOR_YELLOW
+};
+
+extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
+
+#define PRINTF(...) \
+ do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN,\
+ "[ ] "); \
+ testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } \
+ while(0)
+
+#define PRINT_TEST_FINISHED \
+{ \
+ const ::testing::TestInfo* const test_info = \
+ ::testing::UnitTest::GetInstance()->current_test_info(); \
+ PRINTF(std::string("Finished test case ").append(test_info->test_case_name()).\
+ append(" of test ").append(test_info->name()).append(".\n").c_str()); \
+}
+
+// C++ stream interface
+class TestCout : public std::stringstream
+{
+public:
+ ~TestCout()
+ {
+ PRINTF("%s\n", str().c_str());
+ }
+};
+
+/* Usage :
+
+TEST(Test, Foo)
+{
+ // the following works but prints default stream
+ EXPECT_TRUE(false) << "Testing Stream.";
+
+ // or you can play with AINSI color code
+ EXPECT_TRUE(false) << "\033[1;31m" << "Testing Stream.";
+
+ // or use the above defined macros
+
+ PRINTF("Hello world");
+
+ // or
+
+ TEST_COUT << "Hello world";
+}
+
+*/
+#define TEST_COUT testing::internal::TestCout()
+
+} // namespace internal
+
+/* Macros related to testing Eigen classes:
+ */
+#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ return (lhs - rhs).isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ return (lhs - rhs).isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+
+#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
+ er(2) = pi2pi((double)er(2)); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+} // namespace testing
+
+#endif /* GNSSUTILS_UTILS_GTEST_H */
diff --git a/src/test/gtest_example.cpp b/src/test/gtest_example.cpp
new file mode 100644
index 0000000..7dd3b9c
--- /dev/null
+++ b/src/test/gtest_example.cpp
@@ -0,0 +1,20 @@
+#include "gtest/utils_gtest.h"
+
+TEST(TestTest, DummyTestExample)
+{
+ EXPECT_FALSE(false);
+
+ ASSERT_TRUE(true);
+
+ int my_int = 5;
+
+ ASSERT_EQ(my_int, 5);
+
+// PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/src/test/gtest_transformations.cpp b/src/test/gtest_transformations.cpp
new file mode 100644
index 0000000..193cb06
--- /dev/null
+++ b/src/test/gtest_transformations.cpp
@@ -0,0 +1,33 @@
+#include "gtest/utils_gtest.h"
+#include "gnss_utils/gnss_utils.h"
+
+/* functions being tested:
+ *
+ * Eigen::Vector3d ecefToLatLonAlt(const Eigen::Vector3d & _ecef);
+ * Eigen::Vector3d latLonAltToEcef(const Eigen::Vector3d & _latlon);
+ * Eigen::Matrix3d ecefToEnuCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_ecef);
+ * Eigen::Matrix3d enuToEcefCov(const Eigen::Vector3d & _latlon, const Eigen::Matrix3d _cov_enu);
+ * void computeEnuEcefFromEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
+ * void computeEnuEcefFromLatLonAlt(const Eigen::Vector3d& _ENU_latlonalt, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF);
+ */
+
+using namespace GNSSUtils;
+
+TEST(TransformationsTest, ecefToLatLonAlt)
+{
+ Eigen::Vector3d ecef, latlonalt;
+ ecef << 0, 0, 1e3;
+
+ latlonalt = ecefToLatLonAlt(ecef);
+
+ std::cout << latlonalt.transpose();
+
+ ASSERT_DOUBLE_EQ(latlonalt(0),0.0);
+ ASSERT_DOUBLE_EQ(latlonalt(1),0.0);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
--
GitLab