From f1fdebd9ac3122ad9142a263dc0e493d20c561dc Mon Sep 17 00:00:00 2001 From: Michal Date: Sat, 21 Feb 2026 00:53:29 +0100 Subject: [PATCH] Added proj integration MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Proj is a library for performing cartographic transformations and geodetic computations. This commit integrates proj into the project, allowing for more accurate and flexible handling of geospatial data. The data fron NMEA sequences are projected to local ENU coordinates for GNSS processing. The ellipsoid height is obtained from the NMEA sequences instead of using mean sea level (MSL) height. Added conversion to WPUWG92 and UTM. Old code was kept to prevent breaking changes, but it is marked as deprecated and will be removed in future releases. Signed-off-by: Michał Pełka --- .github/workflows/cmake-linux-deb.yml | 8 +- .github/workflows/cmake-linux.yml | 9 +- .github/workflows/python-bindings-linux.yml | 5 +- .gitignore | 2 + 3rdpartyBinary/Proj/CMakeLists.txt | 102 ++++ CMakeLists.txt | 6 + .../CMakeLists.txt | 3 +- .../multi_view_tls_registration_gui.cpp | 138 +++++- core/CMakeLists.txt | 4 +- core/include/gnss.h | 54 ++- core/src/gnss.cpp | 440 +++++++++++++++--- core/src/pose_graph_loop_closure.cpp | 12 +- ubuntu-24.04-apt-requirements.sh | 2 +- 13 files changed, 688 insertions(+), 97 deletions(-) create mode 100644 3rdpartyBinary/Proj/CMakeLists.txt diff --git a/.github/workflows/cmake-linux-deb.yml b/.github/workflows/cmake-linux-deb.yml index 36a05a87..4727ed79 100644 --- a/.github/workflows/cmake-linux-deb.yml +++ b/.github/workflows/cmake-linux-deb.yml @@ -17,13 +17,13 @@ jobs: # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. # You can convert this to a matrix build if you need cross-platform coverage. # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Load repository cache run: sudo apt-get update --allow-releaseinfo-change - name: Install Libs - run: sudo apt-get install -y --no-install-recommends libeigen3-dev libglew-dev freeglut3-dev libx11-dev libxi-dev libtbb-dev liblaszip-dev libopencv-dev + run: sudo apt-get install -y --no-install-recommends libeigen3-dev libglew-dev freeglut3-dev libx11-dev libxi-dev libtbb-dev liblaszip-dev libopencv-dev libproj-dev - uses: actions/checkout@v4 with: @@ -34,12 +34,12 @@ jobs: wget -O cmake.sh https://github.com/Kitware/CMake/releases/download/v4.0.0/cmake-4.0.0-linux-x86_64.sh sudo sh cmake.sh --skip-license --prefix=/usr/local cmake --version - + - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBUILD_WITH_BUNDLED_FREEGLUT=0 -DBUILD_WITH_BUNDLED_EIGEN=0 -DBUILD_WITH_BUNDLED_LIBLASZIP=0 - + - name: Build # Build your program with the given configuration run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} --target package diff --git a/.github/workflows/cmake-linux.yml b/.github/workflows/cmake-linux.yml index 72103e04..b584782e 100644 --- a/.github/workflows/cmake-linux.yml +++ b/.github/workflows/cmake-linux.yml @@ -17,13 +17,13 @@ jobs: # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. # You can convert this to a matrix build if you need cross-platform coverage. # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Load repository cache run: sudo apt-get update --allow-releaseinfo-change - name: Install Libs - run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev + run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev libproj-dev - uses: actions/checkout@v4 with: @@ -34,7 +34,10 @@ jobs: wget -O cmake.sh https://github.com/Kitware/CMake/releases/download/v4.0.0/cmake-4.0.0-linux-x86_64.sh sudo sh cmake.sh --skip-license --prefix=/usr/local cmake --version - + + - name: Fix pkg-config visibility + run: echo "PKG_CONFIG_PATH=/usr/lib/x86_64-linux-gnu/pkgconfig:/usr/lib/pkgconfig:$PKG_CONFIG_PATH" >> $GITHUB_ENV + - name: Configure CMake # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type diff --git a/.github/workflows/python-bindings-linux.yml b/.github/workflows/python-bindings-linux.yml index 324e7a58..4632e3e3 100644 --- a/.github/workflows/python-bindings-linux.yml +++ b/.github/workflows/python-bindings-linux.yml @@ -11,7 +11,7 @@ env: jobs: build: - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 strategy: matrix: python-version: ["3.12"] @@ -21,7 +21,7 @@ jobs: run: sudo apt-get update --allow-releaseinfo-change - name: Install system libraries - run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev + run: sudo apt-get install -y --no-install-recommends libx11-dev libxi-dev libtbb-dev libegl1-mesa-dev libglu1-mesa-dev libopencv-dev libproj-dev - uses: actions/checkout@v4 with: @@ -58,3 +58,4 @@ jobs: python -c "import sys; sys.path.insert(0, '.'); import multi_view_tls_registration_py; print('multi_view_tls_registration_py imported successfully')" + diff --git a/.gitignore b/.gitignore index 165868ea..fd91924d 100644 --- a/.gitignore +++ b/.gitignore @@ -70,3 +70,5 @@ imgui.ini /out_old/install /.gitmodules +/build2 +/build3 diff --git a/3rdpartyBinary/Proj/CMakeLists.txt b/3rdpartyBinary/Proj/CMakeLists.txt new file mode 100644 index 00000000..a5eccb8f --- /dev/null +++ b/3rdpartyBinary/Proj/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.28) + +# ============================================================ +# PROJ setup +# ============================================================ + + + +# ---------------------------- +# Linux / Unix +# ---------------------------- +if(UNIX AND NOT WIN32) + + find_package(PROJ CONFIG QUIET) + + # Some distros expose lowercase target + if(TARGET PROJ::proj) + set(PROJ_TARGET PROJ::proj) + + elseif(TARGET proj) + set(PROJ_TARGET proj) + + else() + message(FATAL_ERROR + "PROJ not found.\n" + "libproj-dev is installed but CMake could not locate the config package.\n" + "Check CMAKE_PREFIX_PATH or PROJ_DIR." + ) + endif() + +endif() + +# ---------------------------- +# Windows – download prebuilt +# ---------------------------- +if(WIN32) + set(PROJ_FILENAME "proj-windows.zip") + set(PROJ_URL + "https://github.com/MapsHD/proj_win_deploy/releases/download/v9.3.0-5/proj-windows.zip" + ) + + + set(PROJ_SHA256 + "2d0cef6a07ca4d3e83cb6ca89637fb62db1650006876305e8d6a2af21468ae48" + ) + + set(PROJ_DOWNLOAD_PATH "${CMAKE_BINARY_DIR}/${PROJ_FILENAME}") + set(PROJ_EXTRACT_DIR "${CMAKE_BINARY_DIR}/3rd_binary/PROJ") + + # ---------------------------- + # Download + # ---------------------------- + if(NOT EXISTS "${PROJ_DOWNLOAD_PATH}") + message(STATUS "Downloading PROJ ${PROJ_VERSION}...") + file(DOWNLOAD + "${PROJ_URL}" + "${PROJ_DOWNLOAD_PATH}" + EXPECTED_HASH SHA256=${PROJ_SHA256} + SHOW_PROGRESS + TLS_VERIFY ON + ) + endif() + + # ---------------------------- + # Extract + # ---------------------------- + if(NOT EXISTS "${PROJ_EXTRACT_DIR}/include") + message(STATUS "Extracting PROJ...") + file(ARCHIVE_EXTRACT + INPUT "${PROJ_DOWNLOAD_PATH}" + DESTINATION "${PROJ_EXTRACT_DIR}" + ) + endif() + set(PROJ_VERSION v9.3.0) + set(PROJ_ROOT "${PROJ_EXTRACT_DIR}/install_proj") + set (PROJ_INCLUDE_DIR "${PROJ_ROOT}/include") + set (PROJ_LIBRARY "${PROJ_ROOT}/bin/proj_9_3.dll") + set (PROJ_IMPLIB "${PROJ_ROOT}/lib/proj.lib") + set (PROJ_DB "${PROJ_ROOT}/share/proj/proj.db") + + # ---------------------------- + # Create imported target + # ---------------------------- + add_library(PROJ::proj SHARED IMPORTED) + + set_target_properties(PROJ::proj PROPERTIES + IMPORTED_LOCATION "${PROJ_ROOT}/bin/proj_9_3.dll" + IMPORTED_IMPLIB "${PROJ_ROOT}/lib/proj.lib" + INTERFACE_INCLUDE_DIRECTORIES "${PROJ_ROOT}/include" + ) + add_library(PROJ::ZLIB SHARED IMPORTED) + set_target_properties(PROJ::ZLIB PROPERTIES + IMPORTED_LOCATION "${PROJ_ROOT}/bin/z.dll" + IMPORTED_IMPLIB "${PROJ_ROOT}/lib/zs.lib" + INTERFACE_INCLUDE_DIRECTORIES "${PROJ_ROOT}/include" + ) + + set_property(TARGET PROJ::proj APPEND PROPERTY + INTERFACE_LINK_LIBRARIES PROJ::ZLIB + ) + +endif() diff --git a/CMakeLists.txt b/CMakeLists.txt index b91df119..44c930be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -368,6 +368,12 @@ include(${THIRDPARTY_DIRECTORY_BINARY}/OpenCV/CMakeLists.txt) find_package(OpenCV REQUIRED) MESSAGE(STATUS "OpenCV include dir: ${OpenCV_INCLUDE_DIRS}, OpenCV librarys: ${OpenCV_LIBS}") + +# Download or Search for PROJ +include(${THIRDPARTY_DIRECTORY_BINARY}/Proj/CMakeLists.txt) +MESSAGE(STATUS "PROJ include dir: ${PROJ_INCLUDE_DIR}, PROJ library: ${PROJ_LIBRARY}") +MESSAGE(STATUS "PROJ implementation library: ${PROJ_IMPLIB}, PROJ database: ${PROJ_DB}") + # Option to build with bundled oneTBB option(BUILD_WITH_BUNDLED_ONETBB "Build with bundled oneTBB" ON) if (BUILD_WITH_BUNDLED_ONETBB) diff --git a/apps/multi_view_tls_registration/CMakeLists.txt b/apps/multi_view_tls_registration/CMakeLists.txt index e54ec9a6..476b40b7 100644 --- a/apps/multi_view_tls_registration/CMakeLists.txt +++ b/apps/multi_view_tls_registration/CMakeLists.txt @@ -51,7 +51,8 @@ target_include_directories( ${PLATFORM_LASZIP_LIB} ${PLATFORM_MISCELLANEOUS_LIBS} ${CORE_LIBRARIES} - ${GUI_LIBRARIES}) + ${GUI_LIBRARIES} + PROJ::proj) if(WIN32) add_custom_command( diff --git a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp index c74e1012..acae4371 100644 --- a/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp +++ b/apps/multi_view_tls_registration/multi_view_tls_registration_gui.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include "../lidar_odometry_step_1/lidar_odometry_utils.h" #include "multi_view_tls_registration.h" @@ -47,6 +48,7 @@ #include "wgs84_do_puwg92.h" #include +#include #ifdef _WIN32 #include "resource.h" #include @@ -216,6 +218,8 @@ bool manipulate_only_marked_gizmo = false; Session session; PointClouds::PointCloudDimensions session_dims; bool session_loaded = false; +std::vector geoids; +std::string selected_geoid_model; // these functions performs experiment from paper //@article @@ -2998,7 +3002,7 @@ void display() if (input_file_names.size() > 0) { Eigen::Vector3d out_offset(0.0, 0.0, 0.0); - if (!tls_registration.gnss.load(input_file_names, out_offset, gnssWithOffset)) + if (!tls_registration.gnss.load_data_from_gnss_and_convert_to_92(input_file_names, out_offset, gnssWithOffset)) { spdlog::error("Error loading GNSS files!"); } @@ -3013,6 +3017,23 @@ void display() ImGui::Text("Load & convert WGS84 to Cartesian by Mercator projection"); + if (ImGui::MenuItem("Load GNSS (deprecated)")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load gnss files", { "GNSS", "*.gnss" }, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.gnss.load_raw_data_from_gnss(input_file_names)) + { + spdlog::error("Error loading GNSS files!"); + } + if (!tls_registration.gnss.project_to_mercator_projection()) + { + spdlog::error("Error converting WGS84 to Mercator projection!"); + } + } + } if (ImGui::MenuItem("Load GNSS")) { std::vector input_file_names; @@ -3020,28 +3041,53 @@ void display() if (input_file_names.size() > 0) { - if (!tls_registration.gnss.load_mercator_projection(input_file_names)) + if (!tls_registration.gnss.load_raw_data_from_gnss(input_file_names)) { spdlog::error("Error loading GNSS files!"); } + if (!tls_registration.gnss.project_using_proj()) + { + spdlog::error("Error converting WGS84 to PROJ projection!"); + } } } if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Load structured GNSS dataset and decode it into coordinates"); + ImGui::SetTooltip("Load structured GNSS dataset and decode it into coordinates, using PROJ library"); - if (ImGui::MenuItem("Load NMEA")) + if (ImGui::MenuItem("Load NMEA (deprecated)")) { std::vector input_file_names; input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true); if (input_file_names.size() > 0) { - if (!tls_registration.gnss.load_nmea_mercator_projection(input_file_names)) - spdlog::error("Error loading GNSS files!"); + if (!tls_registration.gnss.load_raw_data_from_nmea(input_file_names)) + spdlog::error("Error loading NMEA files!"); + } + if (!tls_registration.gnss.project_to_mercator_projection()) + { + spdlog::error("Error converting WGS84 to Mercator projection!"); } } if (ImGui::IsItemHovered()) ImGui::SetTooltip("Load raw GNSS serial output and decode it into coordinates"); + if (ImGui::MenuItem("Load NMEA")) + { + std::vector input_file_names; + input_file_names = mandeye::fd::OpenFileDialog("Load nmea files", { "NMEA", "*.nmea" }, true); + + if (input_file_names.size() > 0) + { + if (!tls_registration.gnss.load_raw_data_from_nmea(input_file_names)) + spdlog::error("Error loading NMEA files!"); + } + if (!tls_registration.gnss.project_using_proj()) + { + spdlog::error("Error converting WGS84 to PROJ projection!"); + } + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Load raw GNSS serial output and decode it into coordinates, using PROJ library"); ImGui::Separator(); @@ -3058,10 +3104,8 @@ void display() session.point_clouds_container.offset.y(), session.point_clouds_container.offset.z()); } - - if (ImGui::MenuItem("Save metascan points in PUWG92")) + const auto prepareVisibleData = [&]() { - const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); std::vector pointcloud; std::vector intensity; std::vector timestamps; @@ -3086,6 +3130,13 @@ void display() } } } + return std::tuple(pointcloud, intensity, timestamps); + }; + + if (ImGui::MenuItem("Save metascan points in PUWG92(dep!)")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + const auto [pointcloud, intensity, timestamps] = prepareVisibleData(); const auto lat = tls_registration.gnss.WGS84ReferenceLatitude; const auto lon = tls_registration.gnss.WGS84ReferenceLongitude; @@ -3097,6 +3148,70 @@ void display() Eigen::Vector3d offset(Ypuwg92, Xpuwg92, alt); exportLaz(output_file_name, pointcloud, intensity, timestamps, offset.x(), offset.y(), offset.z()); } + ImGui::Separator(); + for (const auto& geoid : geoids) + { + if (ImGui::MenuItem(std::string("Set geoid to " + geoid).c_str(), nullptr, selected_geoid_model == geoid)) + { + selected_geoid_model = geoid; + } + } + ImGui::Separator(); + + if (ImGui::MenuItem("Save metascan points in WGS84 LLA (PROJ)")) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + const auto [pointcloud, intensity, timestamps] = prepareVisibleData(); + + const auto lla_points = tls_registration.gnss.unproject_using_proj(pointcloud); + + exportLaz(output_file_name, lla_points, intensity, timestamps, 0, 0, 0); + } + for (const auto& crtName : CRTs::SupportedCRTs) + { + std::string itemName = "Save metascan points in " + crtName + " (PROJ)"; + if (ImGui::MenuItem(itemName.c_str())) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + + const auto [pointcloud, intensity, timestamps] = prepareVisibleData(); + const auto lla_points = tls_registration.gnss.unproject_using_proj(pointcloud); + auto crt_points = tls_registration.gnss.CRTConvert(lla_points, crtName, selected_geoid_model); + Eigen::Vector3d offset = crt_points.front(); + for (auto& p : crt_points) + { + p = p - offset; + } + exportLaz(output_file_name, crt_points, intensity, timestamps, offset.x(), offset.y(), offset.z()); + } + } + + for (const auto& crtName : CRTs::SupportedCRTs) + { + std::string itemName = "Save GNSS data to las/laz in " + crtName + " (PROJ)"; + if (ImGui::MenuItem(itemName.c_str())) + { + const auto output_file_name = mandeye::fd::SaveFileDialog(out_fn.c_str(), mandeye::fd::LAS_LAZ_filter, ".laz"); + + std::vector lla_points; + std::vector intensity; + std::vector timestamps; + for (const auto& gnss : tls_registration.gnss.gnss_poses) + { + lla_points.emplace_back(gnss.lat, gnss.lon, gnss.alt); + intensity.push_back(gnss.hdop); + timestamps.push_back(gnss.timestamp); + } + + auto crt_points = tls_registration.gnss.CRTConvert(lla_points, crtName, selected_geoid_model); + Eigen::Vector3d offset = crt_points.front(); + for (auto& p : crt_points) + { + p = p - offset; + } + exportLaz(output_file_name, crt_points, intensity, timestamps, offset.x(), offset.y(), offset.z()); + } + } ImGui::EndMenu(); } @@ -3784,6 +3899,9 @@ int main(int argc, char* argv[]) return 0; } + // search for available geoid models in the system and populate the menu + geoids = GNSS::get_available_geoids(); + initGL(&argc, argv, winTitle, display, mouse); if (argc > 1) @@ -3793,7 +3911,7 @@ int main(int argc, char* argv[]) std::string ext = fs::path(argv[i]).extension().string(); std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower); - if (ext == ".mjs") + if (ext == ".mjs" || ext == ".json") { loadSession(argv[i]); diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index a503e255..19969006 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -52,7 +52,9 @@ function(add_core_target target_name with_gui) ${THIRDPARTY_DIRECTORY}/json/include ${THIRDPARTY_DIRECTORY}/observation_equations/codes ${EXTERNAL_LIBRARIES_DIRECTORY}/include - ) + ) + + target_link_libraries(${target_name} PRIVATE PROJ::proj) if(${with_gui}) target_include_directories(${target_name} PRIVATE diff --git a/core/include/gnss.h b/core/include/gnss.h index 6aa06e85..2bae2916 100644 --- a/core/include/gnss.h +++ b/core/include/gnss.h @@ -2,8 +2,18 @@ #include +#include #include #include +namespace CRTs +{ + // List of supported CRTs: + + constexpr const char* PUWG92_ID = "PUWG92"; + constexpr const char* UTM_AUTO_ID = "UTM_AUTO"; + constexpr const char* WEBMERC_ID = "WebMercator"; + const std::set SupportedCRTs{ PUWG92_ID, UTM_AUTO_ID, WEBMERC_ID }; +} // namespace CRTs class GNSS { @@ -20,8 +30,9 @@ class GNSS double age; double time; double fix_quality; - double x; - double y; + double enu_x; // ENU + double enu_y; // ENU + double enu_z; // ENU double dist_xy_along; }; @@ -34,12 +45,44 @@ class GNSS ; }; + //! \brief Get available geoid models from PROJ library + //! \return vector of geoid model names + static std::vector get_available_geoids(); + //! \brief Load GNSS data from file and converts to PUWG92 //! \param input_file_names - vector of file names //! \param localize - if true, the data is moved to the first point - bool load(const std::vector& input_file_names, Eigen::Vector3d& out_offset, bool localize = false); - bool load_mercator_projection(const std::vector& input_file_names); - bool load_nmea_mercator_projection(const std::vector& input_file_names); + bool load_data_from_gnss_and_convert_to_92( + const std::vector& input_file_names, Eigen::Vector3d& out_offset, bool localize = false); + + //! Load data from timestamped NMEA files (timestamp + NMEA sentence) + //! \param input_file_names - vector of file names + bool load_raw_data_from_nmea(const std::vector& input_file_names); + + //! \brief Load *.GNSS files data from file without projection + //! \param input_file_names - vector of file names + //! \ return true if the data was loaded successfully, false otherwise + bool load_raw_data_from_gnss(const std::vector& input_file_names); + + //! Project the loaded GNSS data to Mercator projection using the WGS84 reference point + bool project_to_mercator_projection(); + + //! Project loaded WGS84 data to topocentric coordinates using the WGS84 reference point + //! \see https://proj.org/en/stable/operations/conversions/topocentric.html + bool project_using_proj(); + + //! Unproject the loaded data from topocentric coordinates to WGS84 using the WGS84 reference point + std::vector unproject_using_proj(const std::vector& pointcloud); + + double getGeoidSeparation(double lat_deg, double lon_deg, const std::string& geoidFile); + + //! Converts coordinate system + //! \param llaPointcloud - pointcloud converted to LLA (WGS84) + //! \targetCRT - target coordinate system e.g. PUWG92 or UTM + //! \geoid - name of geoid/ellipsoid, empty for WG84 e + std::vector CRTConvert( + const std::vector& llaPointcloud, const std::string targetCRT, const std::string geoid); + void render(const PointClouds& point_clouds_container); bool save_to_laz(const std::string& output_file_names, double offset_x, double offset_y, double offset_alt); @@ -51,5 +94,6 @@ class GNSS double WGS84ReferenceLatitude = 0; double WGS84ReferenceLongitude = 0; + double geoidSeparation = 0; //! Distance from WGS84 elipsoid to geoid at reference bool setWGS84ReferenceFromFirstPose = true; }; diff --git a/core/src/gnss.cpp b/core/src/gnss.cpp index 49dac8cd..8e0d97ac 100644 --- a/core/src/gnss.cpp +++ b/core/src/gnss.cpp @@ -9,7 +9,70 @@ #include #endif +#include #include +#include +const std::string Elipsooid = "WGS84Ellipsoid"; + +// path to find +const std::vector PathCandidate{ + std::filesystem::current_path().string(), + std::filesystem::current_path().string() + "/proj", + std::filesystem::current_path().string() + "/share/proj", + std::filesystem::current_path().string() + "/data/proj", + "/usr/local/share/proj", + "/usr/share/proj", + "C:/HDMapping", +}; + +std::vector GNSS::get_available_geoids() +{ + std::set unique; + unique.insert(Elipsooid); + + // Respect HD_MAPPING_PROJ environment variable if set. It can point to a directory + // containing geoid grid files (*.gtx). If present, search it first. + std::vector paths = PathCandidate; + const char* env_proj = std::getenv("HD_MAPPING_ASSETS"); + if (env_proj && std::string(env_proj).size() > 0) + { + // insert at front so user-specified path is searched first + paths.insert(paths.begin(), std::string(env_proj)); + std::cout << "HD_MAPPING_ASSETS set: " << env_proj << std::endl; + } + + for (const auto& path : paths) + { + std::filesystem::path proj_path(path); + std::error_code ec; + if (std::filesystem::exists(proj_path, ec) && !ec) + { + std::filesystem::directory_iterator dir_it(proj_path, ec); + if (ec) + { + std::cerr << "cannot open directory " << proj_path << ": " << ec.message() << std::endl; + continue; + } + for (; dir_it != std::filesystem::directory_iterator(); dir_it.increment(ec)) + { + if (ec) + { + std::cerr << "error iterating directory " << proj_path << ": " << ec.message() << std::endl; + break; + } + const auto& entry = *dir_it; + if (entry.is_regular_file(ec) && !ec && entry.path().extension() == ".gtx") + if (entry.is_regular_file() && entry.path().extension() == ".gtx") + { + std::cout << "found geoid model: " << entry.path().string() << std::endl; + unique.insert(entry.path().string()); + } + } + } + } + + return std::vector(unique.begin(), unique.end()); +} #include @@ -25,7 +88,8 @@ inline void split(std::string& str, char delim, std::vector& out) } } -bool GNSS::load(const std::vector& input_file_names, Eigen::Vector3d& out_offset, bool localize) +bool GNSS::load_data_from_gnss_and_convert_to_92( + const std::vector& input_file_names, Eigen::Vector3d& out_offset, bool localize) { // 54651848940 5156.43798828125 2009.1610107421875 122.1999969482421875 1.25 8 35.799999237060546875 nan 14:51:42 1 // timestamp lat lon alt hdop satelites_tracked height age time fix_quality @@ -75,9 +139,9 @@ bool GNSS::load(const std::vector& input_file_names, Eigen::Vector3 double L = gp.lon; double B = gp.lat; - wgs84_do_puwg92(B, L, &gp.y, &gp.x); - - if (Eigen::Vector3d(gp.y, gp.x, gp.alt).norm() > 0) + wgs84_do_puwg92(B, L, &gp.enu_y, &gp.enu_x); + gp.enu_z = gp.alt; + if (Eigen::Vector3d(gp.enu_y, gp.enu_x, gp.alt).norm() > 0) { gnss_poses.push_back(gp); } @@ -100,7 +164,7 @@ bool GNSS::load(const std::vector& input_file_names, Eigen::Vector3 gnss_poses.end(), [](const GNSS::GlobalPose& gp) { - return gp.x != 0 && gp.y != 0 && gp.alt != 0; + return gp.enu_x != 0 && gp.enu_y != 0 && gp.enu_z != 0; }); if (firstGNSSIt == gnss_poses.end()) { @@ -113,33 +177,34 @@ bool GNSS::load(const std::vector& input_file_names, Eigen::Vector3 std::cout << std::setprecision(20); std::cout << "firstGNSS: " << firstGNSS.lat << " " << firstGNSS.lon << " " << firstGNSS.alt << std::endl; - std::cout << "firstGNSS: " << firstGNSS.x << " " << firstGNSS.y << " " << firstGNSS.alt << std::endl; + std::cout << "firstGNSS: " << firstGNSS.enu_x << " " << firstGNSS.enu_y << " " << firstGNSS.enu_z << std::endl; WGS84ReferenceLatitude = firstGNSS.lat; WGS84ReferenceLongitude = firstGNSS.lon; - out_offset.x() = firstGNSS.x; - out_offset.y() = firstGNSS.y; - out_offset.z() = firstGNSS.alt; + out_offset.x() = firstGNSS.enu_x; + out_offset.y() = firstGNSS.enu_y; + out_offset.z() = firstGNSS.enu_z; if (localize) { for (auto& pose : gnss_poses) { - pose.x = pose.x - firstGNSS.x; - pose.y = pose.y - firstGNSS.y; - pose.alt = pose.alt - firstGNSS.alt; + pose.enu_x = pose.enu_x - firstGNSS.enu_x; + pose.enu_y = pose.enu_y - firstGNSS.enu_y; + pose.enu_z = pose.enu_z - firstGNSS.enu_z; + // std::cout << "pose.x: " << pose.x << " pose.y: " << pose.y << " pose.alt: " << pose.alt << std::endl; } } std::cout << std::setprecision(20); std::cout << "firstGNSS: " << firstGNSS.lat << " " << firstGNSS.lon << " " << firstGNSS.alt << std::endl; - std::cout << "firstGNSS: " << firstGNSS.x << " " << firstGNSS.y << " " << firstGNSS.alt << std::endl; + std::cout << "firstGNSS: " << firstGNSS.enu_x << " " << firstGNSS.enu_y << " " << firstGNSS.enu_z << std::endl; return true; } -bool GNSS::load_mercator_projection(const std::vector& input_file_names) +bool GNSS::load_raw_data_from_gnss(const std::vector& input_file_names) { gnss_poses.clear(); @@ -177,22 +242,9 @@ bool GNSS::load_mercator_projection(const std::vector& input_file_n std::istringstream(strs[7]) >> gp.age; std::istringstream(strs[8]) >> gp.time; std::istringstream(strs[9]) >> gp.fix_quality; - - if (gp.lat == gp.lat) + if (std::isfinite(gp.lat) && std::isfinite(gp.lon) && std::isfinite(gp.alt) && gp.lat != 0.0 && gp.lon != 0.0) { - if (gp.lon == gp.lon) - { - if (gp.alt == gp.alt) - { - if (gp.lat != 0) - { - if (gp.lon != 0) - { - gnss_poses.push_back(gp); - } - } - } - } + gnss_poses.push_back(gp); } } } @@ -206,7 +258,11 @@ bool GNSS::load_mercator_projection(const std::vector& input_file_n { return (a.timestamp < b.timestamp); }); + return true; +} +bool GNSS::project_to_mercator_projection() +{ std::array WGS84Reference{ 0, 0 }; if (gnss_poses.size() > 0) @@ -224,15 +280,91 @@ bool GNSS::load_mercator_projection(const std::vector& input_file_n WGS84Reference[1] = WGS84ReferenceLongitude; } } - for (int i = 0; i < gnss_poses.size(); i++) { std::array WGS84Position{ gnss_poses[i].lat, gnss_poses[i].lon }; std::array result{ wgs84::toCartesian(WGS84Reference, WGS84Position) }; - gnss_poses[i].x = result[0]; - gnss_poses[i].y = result[1]; + gnss_poses[i].enu_x = result[0]; + gnss_poses[i].enu_y = result[1]; + gnss_poses[i].enu_z = gnss_poses[i].alt; + } + + return true; +} + +double get_ellipsoid_height(const GNSS::GlobalPose& pose) +{ + return pose.alt + pose.height; +} + +bool GNSS::project_using_proj() +{ + if (gnss_poses.empty()) + return false; + + // ------------------------------------------------- + // Determine reference + // ------------------------------------------------- + double refLat = 0.0; + double refLon = 0.0; + double refAlt = 0.0; // assume 0 if you don't store altitude + + if (setWGS84ReferenceFromFirstPose) + { + refLat = gnss_poses.front().lat; + refLon = gnss_poses.front().lon; + refAlt = get_ellipsoid_height(gnss_poses.front()); + + WGS84ReferenceLatitude = refLat; + WGS84ReferenceLongitude = refLon; + } + else + { + refLat = WGS84ReferenceLatitude; + refLon = WGS84ReferenceLongitude; + refAlt = 0.0; + } + + // ------------------------------------------------- + // Create PROJ context + pipeline + // ------------------------------------------------- + PJ_CONTEXT* ctx = proj_context_create(); + + std::string pipeline = "+proj=pipeline " + "+step +proj=cart +ellps=WGS84 " + "+step +proj=topocentric " + "+ellps=WGS84 " + "+lat_0=" + + std::to_string(refLat) + " +lon_0=" + std::to_string(refLon) + " +h_0=" + std::to_string(refAlt); + + PJ* P = proj_create(ctx, pipeline.c_str()); + if (!P) + { + proj_context_destroy(ctx); + return false; + } + + // ------------------------------------------------- + // Transform all GNSS poses + // ------------------------------------------------- + for (auto& pose : gnss_poses) + { + PJ_COORD geo = proj_coord( + pose.lon * M_PI / 180.0, // longitude in radians + pose.lat * M_PI / 180.0, // latitude in radians + get_ellipsoid_height(pose), + 0); + + PJ_COORD enu = proj_trans(P, PJ_FWD, geo); + + pose.enu_x = enu.xyz.x; // East (meters) + pose.enu_y = enu.xyz.y; // North (meters) + pose.enu_z = enu.xyz.z; // } + proj_destroy(P); + proj_context_destroy(ctx); + return true; } @@ -253,7 +385,59 @@ double dm_to_dd(const std::string& dm, char direction, bool is_latitude) return dd; } -bool GNSS::load_nmea_mercator_projection(const std::vector& input_file_names) +std::vector GNSS::unproject_using_proj(const std::vector& pointcloud) +{ + PJ_CONTEXT* ctx = proj_context_create(); + + const double alt = get_ellipsoid_height(gnss_poses.front()); + + std::string pipeline = "+proj=pipeline " + "+step +inv +proj=topocentric " + "+lat_0=" + + std::to_string(WGS84ReferenceLatitude) + + " " + "+lon_0=" + + std::to_string(WGS84ReferenceLongitude) + + " " + "+h_0=" + + std::to_string(alt) + + " " + "+ellps=WGS84 " + "+step +inv +proj=cart +ellps=WGS84"; + PJ* P = proj_create(ctx, pipeline.c_str()); + if (!P) + { + proj_context_destroy(ctx); + std::cerr << "Failed to create projection pipeline " << pipeline << std::endl; + return {}; + } + + std::vector lla_points; + lla_points.reserve(pointcloud.size()); + + for (const auto& p : pointcloud) + { + PJ_COORD c; + c.xyz.x = p.x(); // East + c.xyz.y = p.y(); // North + c.xyz.z = p.z(); // Up + + PJ_COORD r = proj_trans(P, PJ_FWD, c); + + double lon = r.lpzt.lam * 180.0 / M_PI; + double lat = r.lpzt.phi * 180.0 / M_PI; + double h_msl = r.lpzt.z; + + lla_points.emplace_back(lat, lon, h_msl); + } + + proj_destroy(P); + proj_context_destroy(ctx); + + return lla_points; +} + +bool GNSS::load_raw_data_from_nmea(const std::vector& input_file_names) { gnss_poses.clear(); @@ -328,33 +512,161 @@ bool GNSS::load_nmea_mercator_projection(const std::vector& input_f return (a.timestamp < b.timestamp); }); - std::array WGS84Reference{ 0, 0 }; + return true; +} - if (gnss_poses.size() > 0) +std::vector GNSS::CRTConvert( + const std::vector& llaPointcloud, const std::string targetCRT, const std::string geoid) +{ + std::vector result; + result.reserve(llaPointcloud.size()); + + if (llaPointcloud.empty()) + return result; + if (!CRTs::SupportedCRTs.contains(targetCRT)) { - if (setWGS84ReferenceFromFirstPose) + std::cerr << "Unsuported CRT " << targetCRT << std::endl; + return result; + } + + std::string target = ""; + + if (targetCRT == CRTs::PUWG92_ID) + { + target = "EPSG:2180"; + } + else if (targetCRT == CRTs::WEBMERC_ID) + { + target = "EPSG:3857"; + } + else if (targetCRT == CRTs::UTM_AUTO_ID) + { + double lat = llaPointcloud[0].x(); + double lon = llaPointcloud[0].y(); + + int zone = int(std::floor((lon + 180.0) / 6.0)) + 1; + bool north = (lat >= 0.0); + + target = "+proj=utm +zone=" + std::to_string(zone) + (north ? " +north " : " +south ") + "+datum=WGS84 +units=m +no_defs"; + } + + std::cout << "target CRT " << target << std::endl; + + PJ_CONTEXT* ctx = proj_context_create(); + PJ* P_geoid = nullptr; + + if (!(geoid.empty() || geoid == Elipsooid)) + { + std::string pipeline = "+proj=pipeline " + "+step +proj=unitconvert +xy_in=deg +xy_out=rad " + "+step +proj=vgridshift +grids=" + + geoid + + " " + "+step +proj=unitconvert +xy_in=rad +xy_out=deg"; + + P_geoid = proj_create(ctx, pipeline.c_str()); + + if (!P_geoid) { - WGS84Reference[0] = gnss_poses[0].lat; - WGS84Reference[1] = gnss_poses[0].lon; - WGS84ReferenceLatitude = gnss_poses[0].lat; - WGS84ReferenceLongitude = gnss_poses[0].lon; + std::cerr << "Failed to create geoid transformation " << pipeline << "\n"; + proj_context_destroy(ctx); + return result; } - else + } + + // --------------------------- + // CRS transform (LLA to target) + // --------------------------- + PJ* P_crs = proj_create_crs_to_crs( + ctx, + "EPSG:4979", // WGS84 3D + target.c_str(), + nullptr); + + if (!P_crs) + { + std::cerr << "Failed to create CRS transformation\n"; + if (P_geoid) + proj_destroy(P_geoid); + proj_context_destroy(ctx); + return result; + } + + P_crs = proj_normalize_for_visualization(ctx, P_crs); + if (!P_crs) + { + std::cerr << "Failed to normalize CRS transformation for visualization\n"; + proj_destroy(P_crs); + return {}; + } + + // --------------------------- + // Transform loop + // --------------------------- + for (const auto& p : llaPointcloud) + { + double lat = p.x(); + double lon = p.y(); + double h = p.z(); + + // Geoid correction (if requested) + if (P_geoid) { - WGS84Reference[0] = WGS84ReferenceLatitude; - WGS84Reference[1] = WGS84ReferenceLongitude; + PJ_COORD c_geo = proj_coord(lon, lat, h, 0); + PJ_COORD r_geo = proj_trans(P_geoid, PJ_FWD, c_geo); + + lon = r_geo.lpzt.lam; + lat = r_geo.lpzt.phi; + h = r_geo.lpzt.z; } + + // CRS conversion + PJ_COORD c = proj_coord(lon, lat, h, 0); + PJ_COORD r = proj_trans(P_crs, PJ_FWD, c); + + result.emplace_back( + r.xy.x, // Easting / X + r.xy.y, // Northing / Y + r.xyz.z // height (after geoid if applied) + ); } - for (int i = 0; i < gnss_poses.size(); i++) + // --------------------------- + // Cleanup + // --------------------------- + proj_destroy(P_crs); + if (P_geoid) + proj_destroy(P_geoid); + proj_context_destroy(ctx); + + return result; +} + +double GNSS::getGeoidSeparation(double lat_deg, double lon_deg, const std::string& geoidFile) +{ + PJ_CONTEXT* ctx = proj_context_create(); + + std::string pipeline = "+proj=pipeline " + "+step +proj=unitconvert +xy_in=deg +xy_out=rad " + "+step +proj=vgridshift +grids=" + + geoidFile + + " " + "+step +proj=unitconvert +xy_in=rad +xy_out=deg"; + + PJ* P = proj_create(ctx, pipeline.c_str()); + if (!P) { - std::array WGS84Position{ gnss_poses[i].lat, gnss_poses[i].lon }; - std::array result{ wgs84::toCartesian(WGS84Reference, WGS84Position) }; - gnss_poses[i].x = result[0]; - gnss_poses[i].y = result[1]; + proj_context_destroy(ctx); + return 0.0; } - return true; + PJ_COORD c = proj_coord(lon_deg, lat_deg, 0.0, 0); + PJ_COORD r = proj_trans(P, PJ_FWD, c); + + proj_destroy(P); + proj_context_destroy(ctx); + + return -r.lpzt.z; } #if WITH_GUI == 1 @@ -365,9 +677,9 @@ void GNSS::render(const PointClouds& point_clouds_container) for (int i = 0; i < gnss_poses.size(); i++) { glVertex3f( - gnss_poses[i].x - point_clouds_container.offset.x(), - gnss_poses[i].y - point_clouds_container.offset.y(), - gnss_poses[i].alt - point_clouds_container.offset.z()); + gnss_poses[i].enu_x - point_clouds_container.offset.x(), + gnss_poses[i].enu_y - point_clouds_container.offset.y(), + gnss_poses[i].enu_z - point_clouds_container.offset.z()); } glEnd(); @@ -400,9 +712,9 @@ void GNSS::render(const PointClouds& point_clouds_container) glVertex3f(m(0, 3), m(1, 3), m(2, 3)); glVertex3f( - gnss_poses[i].x - point_clouds_container.offset.x(), - gnss_poses[i].y - point_clouds_container.offset.y(), - gnss_poses[i].alt - point_clouds_container.offset.z()); + gnss_poses[i].enu_x - point_clouds_container.offset.x(), + gnss_poses[i].enu_y - point_clouds_container.offset.y(), + gnss_poses[i].enu_z - point_clouds_container.offset.z()); } } } @@ -422,13 +734,13 @@ bool GNSS::save_to_laz(const std::string& output_file_names, double offset_x, do for (int i = 0; i < gnss_poses.size(); i++) { - max.x() = std::max(max.x(), gnss_poses[i].x); - max.y() = std::max(max.y(), gnss_poses[i].y); - max.z() = std::max(max.z(), gnss_poses[i].alt); + max.x() = std::max(max.x(), gnss_poses[i].enu_x); + max.y() = std::max(max.y(), gnss_poses[i].enu_y); + max.z() = std::max(max.z(), gnss_poses[i].enu_z); - min.x() = std::min(min.x(), gnss_poses[i].x); - min.y() = std::min(min.y(), gnss_poses[i].y); - min.z() = std::min(min.z(), gnss_poses[i].alt); + min.x() = std::min(min.x(), gnss_poses[i].enu_x); + min.y() = std::min(min.y(), gnss_poses[i].enu_y); + min.z() = std::min(min.z(), gnss_poses[i].enu_z); } // create the writer @@ -500,9 +812,9 @@ bool GNSS::save_to_laz(const std::string& output_file_names, double offset_x, do const auto& p = gnss_poses[i]; p_count++; - coordinates[0] = p.x; - coordinates[1] = p.y; - coordinates[2] = p.alt; + coordinates[0] = p.enu_x; + coordinates[1] = p.enu_y; + coordinates[2] = p.enu_z; if (laszip_set_coordinates(laszip_writer, coordinates)) { spdlog::error("DLL ERROR: setting coordinates for point {}", p_count); diff --git a/core/src/pose_graph_loop_closure.cpp b/core/src/pose_graph_loop_closure.cpp index a9ad98e8..1d80f7c6 100644 --- a/core/src/pose_graph_loop_closure.cpp +++ b/core/src/pose_graph_loop_closure.cpp @@ -309,9 +309,9 @@ void PoseGraphLoopClosure::graph_slam(PointClouds& point_clouds_container, GNSS& // Eigen::Vector3d p_t(gnss.gnss_poses[i].x - gnss.offset_x, gnss.gnss_poses[i].y - gnss.offset_y, // gnss.gnss_poses[i].alt - gnss.offset_alt); Eigen::Vector3d p_t( - gnss.gnss_poses[i].x - point_clouds_container.offset.x(), - gnss.gnss_poses[i].y - point_clouds_container.offset.y(), - gnss.gnss_poses[i].alt - point_clouds_container.offset.z()); + gnss.gnss_poses[i].enu_x - point_clouds_container.offset.x(), + gnss.gnss_poses[i].enu_y - point_clouds_container.offset.y(), + gnss.gnss_poses[i].enu_z - point_clouds_container.offset.z()); point_to_point_source_to_target_tait_bryan_wc( delta_x, @@ -998,9 +998,9 @@ void PoseGraphLoopClosure::FuseTrajectoryWithGNSS(PointClouds& point_clouds_cont double delta_y; double delta_z; Eigen::Vector3d p_t( - gnss.gnss_poses[index].x - point_clouds_container.offset.x(), - gnss.gnss_poses[index].y - point_clouds_container.offset.y(), - gnss.gnss_poses[index].alt - point_clouds_container.offset.z()); + gnss.gnss_poses[index].enu_x - point_clouds_container.offset.x(), + gnss.gnss_poses[index].enu_y - point_clouds_container.offset.y(), + gnss.gnss_poses[index].enu_z - point_clouds_container.offset.z()); point_to_point_source_to_target_tait_bryan_wc( delta_x, diff --git a/ubuntu-24.04-apt-requirements.sh b/ubuntu-24.04-apt-requirements.sh index e23d30d2..698f9955 100755 --- a/ubuntu-24.04-apt-requirements.sh +++ b/ubuntu-24.04-apt-requirements.sh @@ -1 +1 @@ -sudo apt update && sudo apt upgrade && sudo apt install g++ cmake libglx-dev libopengl-dev libgl-dev libglut-dev libopencv-dev libopencv-contrib-dev \ No newline at end of file +sudo apt update && sudo apt upgrade && sudo apt install g++ cmake libglx-dev libopengl-dev libgl-dev libglut-dev libopencv-dev libopencv-contrib-dev libproj-dev \ No newline at end of file