Add support for custom geodetic datums and orthometric height

fixposition_driver_lib/CMakeLists.txt

@@ -17,7 +17,6 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
 endif()
 
 set(_unused "${FPSDK_BUILD_TESTING}") # suppress warning
-set(_unused "${FPSDK_USE_PROJ}") # suppress warning
 set(_unused "${FPSDK_USE_FFMPEG}") # suppress warning
 set(_unused "${FPSDK_USE_BZ2}") # suppress warning
 set(_unused "${CMAKE_C_COMPILER}") # suppress warning
@@ -29,6 +28,14 @@ find_package(Eigen3 REQUIRED)
 find_package(fpsdk_common REQUIRED)
 find_package(Threads REQUIRED)
 
+# Respect an explicit SDK-wide disable. Otherwise use PROJ when it is available.
+if(DEFINED FPSDK_USE_PROJ AND NOT FPSDK_USE_PROJ)
+  set(FIXPOSITION_DRIVER_USE_PROJ OFF)
+else()
+  find_package(PROJ 9.4 QUIET)
+  set(FIXPOSITION_DRIVER_USE_PROJ ${PROJ_FOUND})
+endif()
+
 include_directories(include
   ${EIGEN3_INCLUDE_DIR}
   ${Boost_INCLUDE_DIR}
@@ -42,10 +49,38 @@ add_library(
   ${PROJECT_NAME} SHARED
   src/fixposition_driver.cpp
   src/helper.cpp
+  src/llh_transformer.cpp
   src/params.cpp
 )
 
-target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${fpsdk_common_LIBRARIES} pthread)
+target_compile_definitions(${PROJECT_NAME} PUBLIC
+  FIXPOSITION_DRIVER_USE_PROJ=$<BOOL:${FIXPOSITION_DRIVER_USE_PROJ}>
+)
+
+target_link_libraries(${PROJECT_NAME}
+  ${Boost_LIBRARIES}
+  ${fpsdk_common_LIBRARIES}
+  $<$<BOOL:${FIXPOSITION_DRIVER_USE_PROJ}>:PROJ::proj>
+  pthread
+)
+
+
+# TESTS =================================================================================================================
+
+if(FPSDK_BUILD_TESTING)
+  enable_testing()
+  find_package(GTest REQUIRED)
+
+  add_executable(${PROJECT_NAME}_llh_transformer_test
+    test/llh_transformer_test.cpp
+  )
+  target_link_libraries(${PROJECT_NAME}_llh_transformer_test
+    PRIVATE
+      ${PROJECT_NAME}
+      GTest::gtest_main
+  )
+  add_test(NAME ${PROJECT_NAME}_llh_transformer_test COMMAND ${PROJECT_NAME}_llh_transformer_test)
+endif()
 
 
 # INSTALL ==============================================================================================================

fixposition_driver_lib/cmake/fixposition_driver_lib-config.cmake.in

@@ -1,5 +1,14 @@
 @PACKAGE_INIT@
 
+include(CMakeFindDependencyMacro)
+set(_fixposition_driver_lib_package_prefix_dir "${PACKAGE_PREFIX_DIR}")
+find_dependency(fpsdk_common)
+if(@FIXPOSITION_DRIVER_USE_PROJ@)
+    find_dependency(PROJ 9.4)
+endif()
+set(PACKAGE_PREFIX_DIR "${_fixposition_driver_lib_package_prefix_dir}")
+unset(_fixposition_driver_lib_package_prefix_dir)
+
 set(@PROJECT_NAME@_FOUND ON)
 set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include")
 set_and_check(@PROJECT_NAME@_LIBRARY_DIRS "${PACKAGE_PREFIX_DIR}/lib")

fixposition_driver_lib/include/fixposition_driver_lib/llh_transformer.hpp

@@ -0,0 +1,65 @@
+/**
+ * \verbatim
+ * ___    ___
+ * \  \  /  /
+ *  \  \/  /   Copyright (c) Fixposition AG (www.fixposition.com) and contributors
+ *  /  /\  \   License: see the LICENSE file
+ * /__/  \__\
+ * \endverbatim
+ *
+ * @file
+ * @brief Optional PROJ-backed ECEF to geodetic coordinate conversion
+ */
+
+#ifndef __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__
+#define __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__
+
+/* LIBC/STL */
+#include <string>
+
+/* EXTERNAL */
+#include <fpsdk_common/ext/eigen_core.hpp>
+
+namespace fixposition {
+/* ****************************************************************************************************************** */
+
+/**
+ * @brief Convert ECEF coordinates to latitude, longitude and height
+ *
+ * The configured target must be either a three-dimensional geographic CRS or a compound CRS consisting of a
+ * two-dimensional geographic CRS and a vertical CRS. Output is always latitude [rad], longitude [rad], height [m].
+ * PROJ state is created per calling thread because a PROJ context must not be shared across threads.
+ */
+class LlhTransformer {
+   public:
+    /**
+     * @brief Configure and validate the transformation
+     *
+     * @param[in]  enabled     Use PROJ when true; use the built-in WGS84 conversion when false
+     * @param[in]  ecef_crs    Geocentric source CRS, normally "EPSG:4978"
+     * @param[in]  llh_crs     Geographic 3D or geographic 2D plus vertical target CRS
+     *
+     * @returns true if disabled or if the PROJ transformation is available and valid
+     */
+    bool Init(bool enabled, const std::string& ecef_crs, const std::string& llh_crs);
+
+    /**
+     * @brief Convert ECEF [m] to latitude [rad], longitude [rad], height [m]
+     *
+     * @returns true on success, false if the configured PROJ operation cannot transform the coordinate
+     */
+    bool EcefToLlhRad(const Eigen::Vector3d& ecef, Eigen::Vector3d& llh_rad) const;
+
+    bool enabled() const { return proj_enabled_; }
+    const std::string& error() const { return error_; }
+
+   private:
+    bool proj_enabled_ = false;
+    std::string ecef_crs_;
+    std::string llh_crs_;
+    std::string error_;
+};
+
+/* ****************************************************************************************************************** */
+}  // namespace fixposition
+#endif  // __FIXPOSITION_DRIVER_LIB_LLH_TRANSFORMER_HPP__

fixposition_driver_lib/include/fixposition_driver_lib/params.hpp

@@ -41,6 +41,9 @@ struct DriverParams {
     bool raw_output_ = false;
     bool cov_warning_ = false;
     bool nav2_mode_ = false;
+    bool datum_llh_enabled_ = false;
+    std::string datum_llh_ecef_crs_ = "EPSG:4978";
+    std::string datum_llh_llh_crs_ = "EPSG:4979";
 
     enum class VelTopicType { UNSPECIFIED, TWIST, TWISTWITHCOV, ODOMETRY };
     bool converter_enabled_ = false;

fixposition_driver_lib/src/helper.cpp

@@ -161,13 +161,12 @@ bool OdometryData::ConvertToEnu(const TfData& tf_ecef_enu0) {
     const Eigen::Matrix<double, 6, 6> cov_ecef = pose.cov;
 
     // Extract data from the TF message (using the arrow operator)
-    const Eigen::Vector3d t_ecef_enu0 = tf_ecef_enu0.translation;
     const Eigen::Quaterniond q_ecef_enu0 = tf_ecef_enu0.rotation;
     const Eigen::Matrix3d rot_ecef_enu0 = q_ecef_enu0.toRotationMatrix();
 
     // Convert position in ECEF into position in ENU
-    const Eigen::Vector3d t_enu_body =
-        fpsdk::common::trafo::TfEnuEcef(t_ecef_body, fpsdk::common::trafo::TfWgs84LlhEcef(t_ecef_enu0));
+    const Eigen::Vector3d wgs84llh_ref = fpsdk::common::trafo::TfWgs84LlhEcef(tf_ecef_enu0.translation);
+    const Eigen::Vector3d t_enu_body = fpsdk::common::trafo::TfEnuEcef(t_ecef_body, wgs84llh_ref);
     const Eigen::Quaterniond q_enu_body = q_ecef_enu0.inverse() * q_ecef_body;
 
     // Convert covariance matrix to ENU