From 501b8f08313e4394ac8585167b74374e2ae3da09 Mon Sep 17 00:00:00 2001 From: fjacomme <56129513+fjacomme@users.noreply.github.com> Date: Tue, 10 Nov 2020 14:45:01 +0100 Subject: [PATCH] 1.1: fix mercator frame --- CMakeLists.txt | 13 ++++-- doc/frames.md | 2 +- include/sico/frames/mercator.hpp | 20 ++++++--- include/sico/types/enu.hpp | 2 +- src/conversions/lla_mercator.cpp | 14 +++---- tests/conversions.lla_mercator.t.cpp | 2 +- tests/frames.mercator.t.cpp | 63 +++++++++++++++++++++++++++- 7 files changed, 96 insertions(+), 20 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6df8c21..c88161b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,9 @@ include(FetchContent) include(CMakePackageConfigHelpers) set(CMAKE_CXX_STANDARD 17) -project(sico VERSION 1.0.0) +project(sico VERSION 1.1.0) + +set(CMAKE_INSTALL_PREFIX ${CMAKE_CURRENT_BINARY_DIR}/dist) option(SICO_USE_EIGEN "Use the Eigen library for linear algebra" OFF) option(SICO_USE_HOLTHAUS_UNITS "Use Holthaus Units library" OFF) @@ -21,6 +23,9 @@ else() set(SICO_COMP_OPTS -Wall -Wextra -pedantic -Werror -frounding-math) endif() +set(CMAKE_DEBUG_POSTFIX "d") +set(CMAKE_RELWITHDEBINFO_POSTFIX "rd") + add_library(sico STATIC include/sico/sico.hpp src/sico.cpp @@ -94,7 +99,9 @@ target_compile_options(sico PRIVATE ${SICO_COM_OPTS}) install(TARGETS sico DESTINATION lib EXPORT sico-config) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include DESTINATION ".") -install(DIRECTORY ${Units_DIR}/units DESTINATION "include/units") +if (Units_DIR) + install(DIRECTORY ${Units_DIR}/units DESTINATION "include/units") +endif() export(TARGETS sico FILE sico-config.cmake) install(EXPORT sico-config DESTINATION cmake) write_basic_package_version_file(sico-config-version.cmake COMPATIBILITY SameMajorVersion) @@ -142,7 +149,7 @@ if (SICO_BUILD_TESTS) ${Catch2_DIR}/single_include ) target_compile_options(sicotests PUBLIC ${SICO_COM_OPTS}) - if(SICO_TEST_COVERAGE) + if(SICO_TEST_COVERAGE AND NOT MSVC) target_compile_options(sicotests PUBLIC -O0 -g -fprofile-arcs -ftest-coverage) target_link_options(sicotests PUBLIC -fprofile-arcs -ftest-coverage) endif() diff --git a/doc/frames.md b/doc/frames.md index 0c315aa..5dc924d 100644 --- a/doc/frames.md +++ b/doc/frames.md @@ -12,7 +12,7 @@ See [frames/local_tangent.hpp](../include/sico/frames/local_tangent.hpp). ## mercator -This frames converts a geodetic *LLA* position to an *east-north-up* local position, relative to mercator projection centered on the reference longitude. +This frames converts a geodetic *LLA* position to an *east-north-up* local position, relative to mercator projection centered on the reference position. See [frames/mercator.hpp](../include/sico/frames/mercator.hpp). diff --git a/include/sico/frames/mercator.hpp b/include/sico/frames/mercator.hpp index 3023534..0e12f15 100644 --- a/include/sico/frames/mercator.hpp +++ b/include/sico/frames/mercator.hpp @@ -5,16 +5,26 @@ namespace sico { /// Mercator projection from a reference longitude class frame_mercator { - radians lon0; + pos_lla ref; + meters north_offset; public: - frame_mercator(radians lon0) - : lon0(lon0) + frame_mercator(pos_lla const& ref) + : ref(ref) { + north_offset = sico::to_enu(ref.lon, ref).north; } - pos_enu_m to_enu(pos_lla const& p) const { return sico::to_enu(lon0, p); } - pos_lla to_lla(pos_enu_m const& p) const { return sico::to_lla(lon0, p); } + pos_enu_m to_enu(pos_lla const& p) const + { + auto const enu = sico::to_enu(ref.lon, p); + return { enu.east, enu.north - north_offset, enu.up - ref.alt }; + } + + pos_lla to_lla(pos_enu_m const& p) const + { + return sico::to_lla(ref.lon, { p.east, p.north + north_offset, p.up + ref.alt }); + } }; } // namespace sico diff --git a/include/sico/types/enu.hpp b/include/sico/types/enu.hpp index da541d4..f57de81 100644 --- a/include/sico/types/enu.hpp +++ b/include/sico/types/enu.hpp @@ -73,7 +73,7 @@ bool approx_eq(vec_enu const& p1, vec_enu const& p2, U p) template bool operator==(vec_enu const& p1, vec_enu const& p2) { - return approx_eq(p1, p2, U(0.001)); // millimeter precision is usually good enough + return approx_eq(p1, p2, U(0.01)); // centimeter precision is usually good enough } template bool operator!=(vec_enu const& p1, vec_enu const& p2) diff --git a/src/conversions/lla_mercator.cpp b/src/conversions/lla_mercator.cpp index 43cb363..7ab86fa 100644 --- a/src/conversions/lla_mercator.cpp +++ b/src/conversions/lla_mercator.cpp @@ -128,12 +128,10 @@ pos_enu_m sico::to_enu(radians lon0, pos_lla const& p) bool const backside = lonDiff > SICO_PI2; double const lon = backside ? SICO_PI - lonDiff : lonDiff; - double const latsign = (backside && lat == 0) ? -1.0 : std::copysign(1.0, lat); - double const lonsign = std::copysign(1.0, lon); - double const sphi = sin(lat); - double const cphi = cos(lat); - double const slam = sin(lon); - double const clam = cos(lon); + double const sphi = sin(lat); + double const cphi = cos(lat); + double const slam = sin(lon); + double const clam = cos(lon); double const tau = sphi / cphi, taup = taupf(tau); double const xip = atan2(taup, clam); @@ -173,8 +171,8 @@ pos_enu_m sico::to_enu(radians lon0, pos_lla const& p) // Gauss-Krueger TM. double const xi = y1.real(); double const eta = y1.imag(); - double const y = Earth.a1 * Earth.k0 * xi * latsign; - double const x = Earth.a1 * Earth.k0 * eta * lonsign; + double const y = Earth.a1 * Earth.k0 * xi; + double const x = Earth.a1 * Earth.k0 * eta; return pos_enu_m { meters(x), meters(y), p.alt }; } diff --git a/tests/conversions.lla_mercator.t.cpp b/tests/conversions.lla_mercator.t.cpp index 4419518..71b70e4 100644 --- a/tests/conversions.lla_mercator.t.cpp +++ b/tests/conversions.lla_mercator.t.cpp @@ -19,7 +19,7 @@ void test(radians lonref, pos_lla const& lla, pos_enu_m const& ref) void testzero(double lat, double lon, double x, double y) { - INFO("test: " << lat << " " << lon << " " << x << " " << y); + INFO("testo: " << lat << " " << lon << " " << x << " " << y); auto const lonref(0_rad); pos_lla const lla { degrees(lat), degrees(lon), 0_m }; pos_enu_m const enu { meters(x), meters(y), 0_m }; diff --git a/tests/frames.mercator.t.cpp b/tests/frames.mercator.t.cpp index 15682cf..49874e0 100644 --- a/tests/frames.mercator.t.cpp +++ b/tests/frames.mercator.t.cpp @@ -9,7 +9,68 @@ using namespace sico; using namespace Catch::literals; using namespace sico::literals; + +using std::vector; + +#define eqp(p1, p2) REQUIRE(abs((p1 - p2)) < 0.1) + +static void test_enu( + pos_lla const& reflla, double lat, double lon, double alt, double e, double n, double u) +{ + frame_mercator frame(reflla); + + auto const lla = pos_lla { degrees(lat), degrees(lon), meters(alt) }; + + auto const enu = frame.to_enu(lla); + + INFO(reflla << "\n" << lla << "\n" << enu); + + eqp(enu.east, e); + eqp(enu.north, n); + eqp(enu.up, u); + + auto const lla2 = frame.to_lla(enu); + + eqp(lla2.lat, lla.lat); + eqp(lla2.lon, lla.lon); + eqp(lla2.alt, lla.alt); +} + +static void test_locs(double lat, double lon, double alt, vector> const& vals) +{ + pos_lla reflla { degrees(lat), degrees(lon), meters(alt) }; + + for (auto& v : vals) { + test_enu(reflla, v[0], v[1], v[2], v[3], v[4], v[5]); + } +} + TEST_CASE("Frames/Mercator") { - + test_locs(0, 0, 0, + { { 0, 0.1, 0, 11127.5, 0, 0 }, + { 0, -0.1, 0, -11127.5, 0, 0 }, + { 0.1, 0, 0, 0, 11053, 0 }, + { -0.1, 0, 0, 0, -11053, 0 }, + { 0, 0, 100, 0, 0, 100 }, + { 0.1, 0.1, 100, 11127.5, 11053, 100 } }); + + test_locs(1, 1, 0, + { { 1, 1.1, 0, 11125.8, 0.2, 0 }, + { 1, 0.9, 0, -11125.8, 0.2, 0 }, + { 1.1, 1, 0, 0, 11053, 0 }, + { 0.9, 1, 0, 0, -11053, 0 }, + { 1, 1, 100, 0, 0, 100 }, + { 1.1, 1.1, 100, 11125.5, 11053.2, 100 } }); + + test_locs(-1, -1, 0, + { { -1, -1.1, 0, -11125.8, -0.2, 0 }, + { -1, -0.9, 0, 11125.8, -0.2, 0 }, + { -1.1, -1, 0, 0, -11053, 0 }, + { -0.9, -1, 0, 0, 11053, 0 }, + { -1, -1, 100, 0, 0, 100 }, + { -1.1, -1.1, 100, -11125.5, -11053.2, 100 } }); + + test_locs(0, 0, 100, + { { 0, 0, 0, 0, 0, -100 }, { 0, 0, 100, 0, 0, 0 }, { 0, 0, 200, 0, 0, 100 } }); } \ No newline at end of file