wave_geography interface update

CMakeLists.txt

@@ -106,17 +106,21 @@ INSTALL(TARGETS wave EXPORT waveTargets)
 # Add each module to the project
 # Modules with missing dependencies are not built, and circular dependencies are
 # not supported. Thus modules must be listed after their dependencies, for now.
-ADD_SUBDIRECTORY(wave_utils)
-ADD_SUBDIRECTORY(wave_geometry)
-ADD_SUBDIRECTORY(wave_containers)
-ADD_SUBDIRECTORY(wave_benchmark)
-ADD_SUBDIRECTORY(wave_controls)
-ADD_SUBDIRECTORY(wave_kinematics)
-ADD_SUBDIRECTORY(wave_matching)
-ADD_SUBDIRECTORY(wave_vision)
-ADD_SUBDIRECTORY(wave_optimization)
-ADD_SUBDIRECTORY(wave_gtsam)
-ADD_SUBDIRECTORY(wave_geography)
+SET(WAVE_MODULES
+        wave_utils
+        wave_geometry
+        wave_containers
+        wave_benchmark
+        wave_controls
+        wave_kinematics
+        wave_matching
+        wave_vision
+        wave_optimization
+        wave_gtsam
+        wave_geography)
+FOREACH(MODULE ${WAVE_MODULES})
+    ADD_SUBDIRECTORY(${MODULE})
+ENDFOREACH()
 
 # Documentation
 SET(WAVE_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR})

docs/CMakeLists.txt

@@ -4,12 +4,11 @@ project(wave_docs)
 set(WAVE_DOXY_INPUT
     "${WAVE_SOURCE_DIR}/README.md ${CMAKE_CURRENT_SOURCE_DIR}/ref")
 
-foreach(dir ${LIBWAVE_MODULES})
-    include_directories(${dir}/include)
-    set(WAVE_DOXY_INPUT "${WAVE_DOXY_INPUT} ${WAVE_SOURCE_DIR}/${dir}")
+foreach(module ${WAVE_MODULES})
+    include_directories(${module}/include)
+    set(WAVE_DOXY_INPUT "${WAVE_DOXY_INPUT} ${WAVE_SOURCE_DIR}/${module}")
 endforeach()
 
-
 find_package(Doxygen)
 if(DOXYGEN_FOUND)
     configure_file(Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY)

wave_geography/CMakeLists.txt

@@ -3,9 +3,7 @@ PROJECT(wave_geography)
 WAVE_ADD_MODULE(${PROJECT_NAME}
     DEPENDS
     Eigen3::Eigen
-    GeographicLib
-    SOURCES
-    src/world_frame_conversions.cpp)
+    GeographicLib)
 
 # Unit tests
 IF(BUILD_TESTING)

wave_geography/include/wave/geography/world_frame_conversions.hpp

@@ -1,133 +1,166 @@
-/* Copyright (c) 2017, Waterloo Autonomous Vehicles Laboratory (WAVELab),
- * Waterloo Intelligent Systems Engineering Lab (WISELab),
- * University of Waterloo.
- *
- * Refer to the accompanying LICENSE file for license information.
- *
- * ############################################################################
- ******************************************************************************
- |                                                                            |
- |                         /\/\__/\_/\      /\_/\__/\/\                       |
- |                         \          \____/          /                       |
- |                          '----________________----'                        |
- |                              /                \                            |
- |                            O/_____/_______/____\O                          |
- |                            /____________________\                          |
- |                           /    (#UNIVERSITY#)    \                         |
- |                           |[**](#OFWATERLOO#)[**]|                         |
- |                           \______________________/                         |
- |                            |_""__|_,----,_|__""_|                          |
- |                            ! !                ! !                          |
- |                            '-'                '-'                          |
- |       __    _   _  _____  ___  __  _  ___  _    _  ___  ___   ____  ____   |
- |      /  \  | | | ||_   _|/ _ \|  \| |/ _ \| \  / |/ _ \/ _ \ /     |       |
- |     / /\ \ | |_| |  | |  ||_||| |\  |||_|||  \/  |||_||||_|| \===\ |====   |
- |    /_/  \_\|_____|  |_|  \___/|_| \_|\___/|_|\/|_|\___/\___/ ____/ |____   |
- |                                                                            |
- ******************************************************************************
- * ############################################################################
- *
- * File: world_frame_conversions.hpp
- * Desc: Header file for world frame conversion functions
- * Auth: Michael Smart <[email protected]>
- *
- * ############################################################################
-*/
+/** @file
+ * @ingroup geography
+ */
 
 #ifndef WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP
 #define WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP
 
 #include <cmath>
+#include <Eigen/Core>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
 
 namespace wave {
+/** @addtogroup geography
+ *  @{ */
 
 /** Converts a point from LLH (Latitude [deg], Longitude [deg], Height[m]) to
  *  ECEF.
  *
  *  @param[in] llh the input llh point as (Latitude, Longitude, Height). Height
  *  is relative to the WGS84 ellipsoid.
- *  @param[out] ecef the corresponding point in the geocentric ECEF frame.
+ *  @return the corresponding point in the geocentric ECEF frame.
  */
-void ecefPointFromLLH(const double llh[3], double ecef[3]);
+template <typename Derived>
+Eigen::Vector3d ecefPointFromLLH(const Eigen::MatrixBase<Derived> &llh) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+
+    double latitude = llh(0), longitude = llh(1), height = llh(2);
+
+    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+
+    double X, Y, Z;
+    earth.Forward(latitude, longitude, height, X, Y, Z);
+
+    return Eigen::Vector3d(X, Y, Z);
+};
 
 /** Converts a point from LLH (Latitude [deg], Longitude [deg], Height[m]) to
  *  ECEF.
  *
  *  @param[in] ecef the input point in the geocentric ECEF frame.
- *  @param[out] llh the corresponding llh point as (Latitude, Longitude,
+ *  @return the corresponding llh point as (Latitude, Longitude,
  *  Height). Height is relative to the WGS84 ellipsoid.
  */
-void llhPointFromECEF(const double ecef[3], double llh[3]);
+template <typename Derived>
+Eigen::Vector3d llhPointFromECEF(const Eigen::MatrixBase<Derived> &ecef) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
 
-/** Computes the 3D Affine transform from ECEF to a local datum-defined ENU
- *  frame as a 4x4 row-major matrix.
- *
- *  @param[in] datum the LLH datum point defining the transform. If
- *  /p datum_is_LLH is set to false, then the datum values are taken as ECEF
- *  instead.
- *  @param[out] T_enu_ecef the 4x4 row-major transformation matrix converting
- *  column-vector points from ECEF to the local ENU frame defined by the datum
- *  point.
- *  @param[in] datum_is_llh \b true: The given datum values are LLH
- *  (default). <BR>
- *  \b false: The given datum values are ECEF
- */
-void enuFromECEFTransformMatrix(const double datum[3],
-                                double T_enu_ecef[4][4],
-                                bool datum_is_llh = true);
+    double X = ecef(0), Y = ecef(1), Z = ecef(2);
+
+    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+
+    double latitude, longitude, height;
+    earth.Reverse(X, Y, Z, latitude, longitude, height);
+
+    return Eigen::Vector3d(latitude, longitude, height);
+};
 
 /** Computes the 3D Affine transform from a local datum-defined ENU frame to
  *  ECEF as a 4x4 row-major matrix.
  *
- *  @param[in] datum the LLH datum point defining the transform. If
- *  /p datum_is_LLH is set to false, then the datum values are taken as ECEF
- *  instead.
- *  @param[out] T_ecef_enu the 4x4 row-major transformation matrix converting
+ *  @param[in] datum the ecef datum point defining the transform.
+ *  @return the 4x4 row-major transformation matrix converting
  *  column-vector points from the local ENU frame defined by the datum point to
  *  ECEF.
- *  @param[in] datum_is_llh \b true: The given datum values are LLH
- *  (default). <BR>
- *  \b false: The given datum values are ECEF
  */
-void ecefFromENUTransformMatrix(const double datum[3],
-                                double T_ecef_enu[4][4],
-                                bool datum_is_llh = true);
+template <typename Derived>
+Eigen::Matrix4d ecefEnuTransformFromEcef(
+  const Eigen::MatrixBase<Derived> &datum) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+
+    // Both Forward() and Reverse() return the same rotation matrix from ENU
+    // to ECEF
+    std::vector<double> R_ecef_enu(9, 0.0);
+    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+
+    double latitude, longitude, height;
+    earth.Reverse(
+      datum(0), datum(1), datum(2), latitude, longitude, height, R_ecef_enu);
+
+    Eigen::Matrix4d transform;
+    transform << R_ecef_enu[0], R_ecef_enu[1], R_ecef_enu[2], datum(0),
+      R_ecef_enu[3], R_ecef_enu[4], R_ecef_enu[5], datum(1), R_ecef_enu[6],
+      R_ecef_enu[7], R_ecef_enu[8], datum(2), 0.0, 0.0, 0.0, 1.0;
+    return transform;
+};
+
+/** Computes the 3D Affine transform from ECEF to a local datum-defined ENU
+ *  frame as a 4x4 row-major matrix.
+ *
+ *  @param[in] datum the ECEF datum point defining the transform.
+ *  @return the 4x4 row-major transformation matrix converting
+ *  column-vector points from ECEF to the local ENU frame defined by the datum
+ *  point.
+ */
+template <typename Derived>
+Eigen::Matrix4d enuEcefTransformFromEcef(
+  const Eigen::MatrixBase<Derived> &datum) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+
+    // Get T_ecef_enu and then invert it
+    Eigen::Matrix4d T_ecef_enu = ecefEnuTransformFromEcef(datum);
+
+    Eigen::Matrix4d T_enu_ecef;
+    // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t]
+    T_enu_ecef.topLeftCorner(3, 3) = T_ecef_enu.topLeftCorner(3, 3).transpose();
+    // Affine inverse translation component: -R_inverse * b
+    //    with b as the 4th column of T_ecef_enu
+    T_enu_ecef.topRightCorner(3, 1) =
+      -T_ecef_enu.topLeftCorner(3, 3).transpose() *
+      T_ecef_enu.topRightCorner(3, 1);
+    T_enu_ecef.row(3) = Eigen::MatrixXd::Zero(1, 4);
+    T_enu_ecef(3, 3) = 1.0;
+
+    return T_enu_ecef;
+};
 
 /** Converts a source point from LLH to a target ENU point in the local
  *  Cartesian ENU frame defined by the provided datum.
  *
  *  @param[in] point_llh the source LLH point to be converted to an ENU point.
- *  @param[in] enu_datum the LLH datum point defining the local ENU frame. If
- *  /p datum_is_LLH is set to false, then the datum values are taken as ECEF
- *  instead.
- *  @param[out] point_enu the corresponding target point in the local ENU frame.
- *  @param[in] datum_is_llh \b true: The given datum values are LLH
- *  (default). <BR>
- *  \b false: The given datum values are ECEF
+ *  @param[in] enu_datum the LLH datum point defining the local ENU frame.
+ *  @return the corresponding target point in the local ENU frame.
  */
-void enuPointFromLLH(const double point_llh[3],
-                     const double enu_datum[3],
-                     double point_enu[3],
-                     bool datum_is_llh = true);
+template <typename DerivedA, typename DerivedB>
+Eigen::Vector3d enuPointFromLLH(const Eigen::MatrixBase<DerivedA> &point_llh,
+                                const Eigen::MatrixBase<DerivedB> &enu_datum) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3);
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3);
+
+    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+    GeographicLib::LocalCartesian localENU(
+      enu_datum(0), enu_datum(1), enu_datum(2), earth);
+
+    double A, B, C;
+    localENU.Forward(point_llh(0), point_llh(1), point_llh(2), A, B, C);
+
+    return Eigen::Vector3d(A, B, C);
+};
 
 /** Converts a source point from ENU in the local Cartesian ENU frame
  *  defined by the provided datum to a target LLH point.
  *
  *  @param[in] point_enu the source ENU point to be converted to an LLH point.
- *  @param[in] enu_datum the LLH datum point defining the local ENU frame. If
- *  /p datum_is_LLH is set to false, then the datum values are taken as ECEF
- *  instead.
- *  @param[out] point_llh the corresponding target point in LLH.
- *  @param[in] datum_is_llh \b true: The given datum values are LLH
- *  (default). <BR>
- *  \b false: The given datum values are ECEF
+ *  @param[in] enu_datum the LLH datum point defining the local ENU frame.
+ *  @return the corresponding target point in LLH.
  */
-void llhPointFromENU(const double point_enu[3],
-                     const double enu_datum[3],
-                     double point_llh[3],
-                     bool datum_is_llh = true);
+template <typename DerivedA, typename DerivedB>
+Eigen::Vector3d llhPointFromENU(const Eigen::MatrixBase<DerivedA> &point_enu,
+                                const Eigen::MatrixBase<DerivedB> &enu_datum) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3);
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3);
+
+    GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84();
+    GeographicLib::LocalCartesian localENU(
+      enu_datum(0), enu_datum(1), enu_datum(2), earth);
+
+    double A, B, C;
+    localENU.Reverse(point_enu(0), point_enu(1), point_enu(2), A, B, C);
+
+    return Eigen::Vector3d(A, B, C);
+};
 
+/** @} group geography */
 }  // namespace wave
 #endif  // WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP