diff --git a/CMakeLists.txt b/CMakeLists.txt index 76084e5c..458524aa 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/docs/CMakeLists.txt b/docs/CMakeLists.txt index 5d5d8f5b..35ef3424 100644 --- a/docs/CMakeLists.txt +++ b/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) diff --git a/wave_geography/CMakeLists.txt b/wave_geography/CMakeLists.txt index 070afc0e..30e6e6d1 100644 --- a/wave_geography/CMakeLists.txt +++ b/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) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index d4fe060b..ba7af74d 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/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 - * - * ############################################################################ -*/ +/** @file + * @ingroup geography + */ #ifndef WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP #define WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP #include +#include #include #include 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 +Eigen::Vector3d ecefPointFromLLH(const Eigen::MatrixBase &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 +Eigen::Vector3d llhPointFromECEF(const Eigen::MatrixBase &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).
- * \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).
- * \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 +Eigen::Matrix4d ecefEnuTransformFromEcef( + const Eigen::MatrixBase &datum) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); + + // Both Forward() and Reverse() return the same rotation matrix from ENU + // to ECEF + std::vector 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 +Eigen::Matrix4d enuEcefTransformFromEcef( + const Eigen::MatrixBase &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).
- * \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 +Eigen::Vector3d enuPointFromLLH(const Eigen::MatrixBase &point_llh, + const Eigen::MatrixBase &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).
- * \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 +Eigen::Vector3d llhPointFromENU(const Eigen::MatrixBase &point_enu, + const Eigen::MatrixBase &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 diff --git a/wave_geography/src/world_frame_conversions.cpp b/wave_geography/src/world_frame_conversions.cpp deleted file mode 100644 index 7bcb463d..00000000 --- a/wave_geography/src/world_frame_conversions.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/* 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.cpp - * Desc: Implementation file for world frame conversion functions - * Auth: Michael Smart - * - * ############################################################################ -*/ - -#include -#include "wave/geography/world_frame_conversions.hpp" - -namespace wave { - -void ecefPointFromLLH(const double llh[3], double ecef[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); - - ecef[0] = X; - ecef[1] = Y; - ecef[2] = Z; -} - -void llhPointFromECEF(const double ecef[3], double llh[3]) { - 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); - - llh[0] = latitude; - llh[1] = longitude; - llh[2] = height; -} - -void ecefFromENUTransformMatrix(const double datum[3], - double T_ecef_enu[4][4], - bool datum_is_llh) { - // Both Forward() and Reverse() return the same rotation matrix from ENU - // to ECEF - std::vector R_ecef_enu(9, 0.0); - GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84(); - double datum_X, datum_Y, datum_Z; - - if (datum_is_llh) { - double latitude = datum[0], longitude = datum[1], height = datum[2]; - - earth.Forward( - latitude, longitude, height, datum_X, datum_Y, datum_Z, R_ecef_enu); - } else { - // Datum is already given in ECEF - datum_X = datum[0]; - datum_Y = datum[1]; - datum_Z = datum[2]; - - double latitude, longitude, height; - earth.Reverse( - datum_X, datum_Y, datum_Z, latitude, longitude, height, R_ecef_enu); - } - - T_ecef_enu[0][0] = R_ecef_enu[0]; - T_ecef_enu[0][1] = R_ecef_enu[1]; - T_ecef_enu[0][2] = R_ecef_enu[2]; - T_ecef_enu[0][3] = datum_X; - T_ecef_enu[1][0] = R_ecef_enu[3]; - T_ecef_enu[1][1] = R_ecef_enu[4]; - T_ecef_enu[1][2] = R_ecef_enu[5]; - T_ecef_enu[1][3] = datum_Y; - T_ecef_enu[2][0] = R_ecef_enu[6]; - T_ecef_enu[2][1] = R_ecef_enu[7]; - T_ecef_enu[2][2] = R_ecef_enu[8]; - T_ecef_enu[2][3] = datum_Z; - T_ecef_enu[3][0] = 0.0; - T_ecef_enu[3][1] = 0.0; - T_ecef_enu[3][2] = 0.0; - T_ecef_enu[3][3] = 1.0; -} - -void enuFromECEFTransformMatrix(const double datum[3], - double T_enu_ecef[4][4], - bool datum_is_llh) { - // Get T_ecef_enu and then invert it - double T_ecef_enu[4][4]; - ecefFromENUTransformMatrix(datum, T_ecef_enu, datum_is_llh); - - // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t] - // TODO(msmart/benskikos) - Move these functions to somewhere where we have - // matrix classes and ownership since it is ours. Manual transposition is - // undesirable. The below should be 2 lines: - // R_new = R.transpose(); t_new = - R.transpose()*t; - T_enu_ecef[0][0] = T_ecef_enu[0][0]; - T_enu_ecef[0][1] = T_ecef_enu[1][0]; - T_enu_ecef[0][2] = T_ecef_enu[2][0]; - T_enu_ecef[1][0] = T_ecef_enu[0][1]; - T_enu_ecef[1][1] = T_ecef_enu[1][1]; - T_enu_ecef[1][2] = T_ecef_enu[2][1]; - T_enu_ecef[2][0] = T_ecef_enu[0][2]; - T_enu_ecef[2][1] = T_ecef_enu[1][2]; - T_enu_ecef[2][2] = T_ecef_enu[2][2]; - - // Affine inverse translation component: -R_inverse * b - // with b as the 4th column of T_ecef_enu - T_enu_ecef[0][3] = -T_enu_ecef[0][0] * T_ecef_enu[0][3] // - - T_enu_ecef[0][1] * T_ecef_enu[1][3] // - - T_enu_ecef[0][2] * T_ecef_enu[2][3]; // - - T_enu_ecef[1][3] = -T_enu_ecef[1][0] * T_ecef_enu[0][3] // - - T_enu_ecef[1][1] * T_ecef_enu[1][3] // - - T_enu_ecef[1][2] * T_ecef_enu[2][3]; // - - T_enu_ecef[2][3] = -T_enu_ecef[2][0] * T_ecef_enu[0][3] // - - T_enu_ecef[2][1] * T_ecef_enu[1][3] // - - T_enu_ecef[2][2] * T_ecef_enu[2][3]; // - - // Last row is the same - T_enu_ecef[3][0] = 0.0; - T_enu_ecef[3][1] = 0.0; - T_enu_ecef[3][2] = 0.0; - T_enu_ecef[3][3] = 1.0; -} - -void enuPointFromLLH(const double point_llh[3], - const double enu_datum[3], - double point_enu[3], - bool datum_is_llh) { - double enu_datum_llh[3]; - if (datum_is_llh) { - enu_datum_llh[0] = enu_datum[0]; - enu_datum_llh[1] = enu_datum[1]; - enu_datum_llh[2] = enu_datum[2]; - } else { - // Datum is ECEF - llhPointFromECEF(enu_datum, enu_datum_llh); - } - - GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84(); - GeographicLib::LocalCartesian localENU( - enu_datum_llh[0], enu_datum_llh[1], enu_datum_llh[2], earth); - - localENU.Forward(point_llh[0], - point_llh[1], - point_llh[2], - point_enu[0], - point_enu[1], - point_enu[2]); -} - -void llhPointFromENU(const double point_enu[3], - const double enu_datum[3], - double point_llh[3], - bool datum_is_llh) { - double enu_datum_llh[3]; - if (datum_is_llh) { - enu_datum_llh[0] = enu_datum[0]; - enu_datum_llh[1] = enu_datum[1]; - enu_datum_llh[2] = enu_datum[2]; - } else { - // Datum is ECEF - llhPointFromECEF(enu_datum, enu_datum_llh); - } - - GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84(); - GeographicLib::LocalCartesian localENU( - enu_datum_llh[0], enu_datum_llh[1], enu_datum_llh[2], earth); - - localENU.Reverse(point_enu[0], - point_enu[1], - point_enu[2], - point_llh[0], - point_llh[1], - point_llh[2]); -} - -} // namespace wave diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index 787cd6e6..95338c72 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -1,41 +1,6 @@ -/* 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: test_ecef_enu_transforms.cpp - * Desc: Tests for ECEF <-> ENU transform matrices - * Auth: Michael Smart - * - * ############################################################################ -*/ - #include #include "wave/geography/world_frame_conversions.hpp" +#include "wave/wave_test.hpp" namespace wave { @@ -46,75 +11,57 @@ class ECEFtoENUTest : public ::testing::Test { double rotation_check_threshold = 1e-6; double earth_radius_approx = 6.371e6; // +/- 21km between WGS84 max/min - void checkT_ECEF_ENU(double T_ECEF_ENU[4][4], - double expected_R_ENU_ECEF[3][3]) { + void checkT_ECEF_ENU(Eigen::Matrix4d T_ECEF_ENU, + Eigen::Matrix3d expected_R_ENU_ECEF) { // Compare R vs R^T - for (int i_row = 0; i_row < 3; ++i_row) { - for (int j_col = 0; j_col < 3; ++j_col) { - EXPECT_NEAR(expected_R_ENU_ECEF[i_row][j_col], - T_ECEF_ENU[j_col][i_row], - rotation_check_threshold); - } - } + EXPECT_PRED3(MatricesNearPrec, + expected_R_ENU_ECEF, + T_ECEF_ENU.topLeftCorner(3, 3).transpose(), + rotation_check_threshold); // Check translation: For ECEF from ENU the norm is used - double R = sqrt(pow(T_ECEF_ENU[0][3], 2) + pow(T_ECEF_ENU[1][3], 2) + - pow(T_ECEF_ENU[2][3], 2)); + double R = sqrt(pow(T_ECEF_ENU(0, 3), 2) + pow(T_ECEF_ENU(1, 3), 2) + + pow(T_ECEF_ENU(2, 3), 2)); EXPECT_NEAR(earth_radius_approx, R, 21e3); // Check last row - EXPECT_NEAR(0.0, T_ECEF_ENU[3][0], 1e-6); - EXPECT_NEAR(0.0, T_ECEF_ENU[3][1], 1e-6); - EXPECT_NEAR(0.0, T_ECEF_ENU[3][2], 1e-6); - EXPECT_NEAR(1.0, T_ECEF_ENU[3][3], 1e-6); + EXPECT_NEAR(0.0, T_ECEF_ENU(3, 0), 1e-6); + EXPECT_NEAR(0.0, T_ECEF_ENU(3, 1), 1e-6); + EXPECT_NEAR(0.0, T_ECEF_ENU(3, 2), 1e-6); + EXPECT_NEAR(1.0, T_ECEF_ENU(3, 3), 1e-6); } - void checkT_ENU_ECEF(double T_ENU_ECEF[4][4], - double expected_R_ENU_ECEF[3][3]) { + void checkT_ENU_ECEF(Eigen::Matrix4d T_ENU_ECEF, + Eigen::Matrix3d expected_R_ENU_ECEF) { // Compare R's directly - for (int i_row = 0; i_row < 3; ++i_row) { - for (int j_col = 0; j_col < 3; ++j_col) { - EXPECT_NEAR(expected_R_ENU_ECEF[i_row][j_col], - T_ENU_ECEF[i_row][j_col], - rotation_check_threshold); - } - } + EXPECT_PRED3(MatricesNearPrec, + expected_R_ENU_ECEF, + T_ENU_ECEF.topLeftCorner(3, 3), + rotation_check_threshold); // Check translation: E == 0, N == 0, U == -earth radius - EXPECT_NEAR(0.0, T_ENU_ECEF[0][3], 1e3); - EXPECT_NEAR(0.0, T_ENU_ECEF[1][3], 1e3); - EXPECT_NEAR(-earth_radius_approx, T_ENU_ECEF[2][3], 21e3); + EXPECT_NEAR(0.0, T_ENU_ECEF(0, 3), 1e3); + EXPECT_NEAR(0.0, T_ENU_ECEF(1, 3), 1e3); + EXPECT_NEAR(-earth_radius_approx, T_ENU_ECEF(2, 3), 21e3); // Check last row - EXPECT_NEAR(0.0, T_ENU_ECEF[3][0], 1e-6); - EXPECT_NEAR(0.0, T_ENU_ECEF[3][1], 1e-6); - EXPECT_NEAR(0.0, T_ENU_ECEF[3][2], 1e-6); - EXPECT_NEAR(1.0, T_ENU_ECEF[3][3], 1e-6); + EXPECT_NEAR(0.0, T_ENU_ECEF(3, 0), 1e-6); + EXPECT_NEAR(0.0, T_ENU_ECEF(3, 1), 1e-6); + EXPECT_NEAR(0.0, T_ENU_ECEF(3, 2), 1e-6); + EXPECT_NEAR(1.0, T_ENU_ECEF(3, 3), 1e-6); } - void checkTransformInverse(double T1[4][4], double T2[4][4]) { + void checkTransformInverse(Eigen::Matrix4d T1, Eigen::Matrix4d T2) { // Multiplication should produce identity - for (int i_row = 0; i_row < 3; ++i_row) { - for (int j_col = 0; j_col < 3; ++j_col) { - double result_i_j = T1[i_row][0] * T2[0][j_col] + - T1[i_row][1] * T2[1][j_col] + - T1[i_row][2] * T2[2][j_col]; - if (i_row == j_col) { - EXPECT_NEAR(1.0, result_i_j, 1e-6); - } else { - EXPECT_NEAR(0.0, result_i_j, 1e-6); - } - } - } + EXPECT_PRED3( + MatricesNearPrec, T1 * T2, Eigen::Matrix4d::Identity(), 1e-6); } - void checkLLHDatumPipeline(double datum_llh[3], - double expected_R_ENU_ECEF[3][3]) { - double T_ENU_ECEF[4][4]; - double T_ECEF_ENU[4][4]; - - ecefFromENUTransformMatrix(datum_llh, T_ECEF_ENU); - enuFromECEFTransformMatrix(datum_llh, T_ENU_ECEF); + void checkLLHDatumPipeline(Eigen::Vector3d datum_llh, + Eigen::Matrix3d expected_R_ENU_ECEF) { + Eigen::Vector3d datum_ECEF = ecefPointFromLLH(datum_llh); + Eigen::Matrix4d T_ENU_ECEF = enuEcefTransformFromEcef(datum_ECEF); + Eigen::Matrix4d T_ECEF_ENU = ecefEnuTransformFromEcef(datum_ECEF); checkT_ENU_ECEF(T_ENU_ECEF, expected_R_ENU_ECEF); checkT_ECEF_ENU(T_ECEF_ENU, expected_R_ENU_ECEF); @@ -123,13 +70,10 @@ class ECEFtoENUTest : public ::testing::Test { checkTransformInverse(T_ECEF_ENU, T_ENU_ECEF); } - void checkECEFDatumPipeline(double datum_ECEF[3], - double expected_R_ENU_ECEF[3][3]) { - double T_ENU_ECEF[4][4]; - double T_ECEF_ENU[4][4]; - - ecefFromENUTransformMatrix(datum_ECEF, T_ECEF_ENU, false); - enuFromECEFTransformMatrix(datum_ECEF, T_ENU_ECEF, false); + void checkECEFDatumPipeline(Eigen::Vector3d datum_ECEF, + Eigen::Matrix3d expected_R_ENU_ECEF) { + Eigen::Matrix4d T_ENU_ECEF = enuEcefTransformFromEcef(datum_ECEF); + Eigen::Matrix4d T_ECEF_ENU = ecefEnuTransformFromEcef(datum_ECEF); checkT_ENU_ECEF(T_ENU_ECEF, expected_R_ENU_ECEF); checkT_ECEF_ENU(T_ECEF_ENU, expected_R_ENU_ECEF); @@ -138,13 +82,13 @@ class ECEFtoENUTest : public ::testing::Test { checkTransformInverse(T_ECEF_ENU, T_ENU_ECEF); } - void checkResult(double datum_llh[3], double expected_R_ENU_ECEF[3][3]) { + void checkResult(Eigen::Vector3d datum_llh, + Eigen::Matrix3d expected_R_ENU_ECEF) { // Check LLH datum version of pipeline checkLLHDatumPipeline(datum_llh, expected_R_ENU_ECEF); // Get ECEF datum from LLH and check ECEF version of pipeline - double datum_ecef[3]; - ecefPointFromLLH(datum_llh, datum_ecef); + Eigen::Vector3d datum_ecef = ecefPointFromLLH(datum_llh); checkECEFDatumPipeline(datum_ecef, expected_R_ENU_ECEF); } }; @@ -163,11 +107,10 @@ Z = [0.0, 0.0, 1.0]^T R = [X|Y|Z] */ TEST_F(ECEFtoENUTest, LatNear90LongAt0) { - double datum_llh[3] = {near90, 0.0, 0.0}; + Eigen::Vector3d datum_llh(near90, 0.0, 0.0); - double R_ENU_ECEF_result[3][3] = {{0.0, 1.0, 0.0}, // - {-1.0, 0.0, 0.0}, // - {0.0, 0.0, 1.0}}; + Eigen::Matrix3d R_ENU_ECEF_result; + R_ENU_ECEF_result << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0; checkResult(datum_llh, R_ENU_ECEF_result); } @@ -186,11 +129,11 @@ Z = [0.0, 0.0, 1.0]^T R = [X|Y|Z] */ TEST_F(ECEFtoENUTest, LatNear90LongNear180) { - double datum_llh[3] = {near90, near180, 0.0}; + // double datum_llh[3] = {near90, near180, 0.0}; + Eigen::Vector3d datum_llh(near90, near180, 0.0); - double R_ENU_ECEF_result[3][3] = {{0.0, -1.0, 0.0}, // - {1.0, 0.0, 0.0}, // - {0.0, 0.0, 1.0}}; + Eigen::Matrix3d R_ENU_ECEF_result; + R_ENU_ECEF_result << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0; checkResult(datum_llh, R_ENU_ECEF_result); } @@ -210,12 +153,10 @@ Z = [0.0, 1.0, 0.0]^T R = [X|Y|Z] */ TEST_F(ECEFtoENUTest, LatNear0LongNear90) { - double datum_llh[3] = {0.0, 90.0, 0.0}; - - double R_ENU_ECEF_result[3][3] = {{-1.0, 0.0, 0.0}, // - {0.0, 0.0, 1.0}, // - {0.0, 1.0, 0.0}}; + Eigen::Vector3d datum_llh(0.0, 90.0, 0.0); + Eigen::Matrix3d R_ENU_ECEF_result; + R_ENU_ECEF_result << -1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0; checkResult(datum_llh, R_ENU_ECEF_result); } @@ -235,11 +176,10 @@ Z = [0.0, 1.0, 0.0]^T R = [X|Y|Z] */ TEST_F(ECEFtoENUTest, LatNear0LongNearN90) { - double datum_llh[3] = {0.0, -90.0, 0.0}; + Eigen::Vector3d datum_llh(0.0, -90.0, 0.0); - double R_ENU_ECEF_result[3][3] = {{1.0, 0.0, 0.0}, // - {0.0, 0.0, 1.0}, // - {0.0, -1.0, 0.0}}; + Eigen::Matrix3d R_ENU_ECEF_result; + R_ENU_ECEF_result << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0; checkResult(datum_llh, R_ENU_ECEF_result); } @@ -259,11 +199,10 @@ Z = [0.0, 0.0, 1.0]^T R = [X|Y|Z] */ TEST_F(ECEFtoENUTest, IdentityPoint) { - double datum_llh[3] = {near90, -90.0, 0.0}; + Eigen::Vector3d datum_llh(near90, -90.0, 0.0); - double R_ENU_ECEF_result[3][3] = {{1.0, 0.0, 0.0}, // - {0.0, 1.0, 0.0}, // - {0.0, 0.0, 1.0}}; + Eigen::Matrix3d R_ENU_ECEF_result; + R_ENU_ECEF_result << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; checkResult(datum_llh, R_ENU_ECEF_result); } diff --git a/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp b/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp index 1632f8b2..53f06fbc 100644 --- a/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp @@ -1,39 +1,3 @@ -/* 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: test_ecef_llh_point_conversions.cpp - * Desc: Tests for ECEF <-> LLH conversions - * Auth: Michael Smart - * - * ############################################################################ -*/ - #include #include "wave/geography/world_frame_conversions.hpp" @@ -46,26 +10,24 @@ TEST(ecefAndLLHPointConversionTest, everestTest) { // ECEF[km] ~ (302.3, 5636, 2980) // ECEF[m] ~ (3.023e5, 5.636e6, 2.980e6) - double point_llh[3] = {27.99, 86.93, 8820}; - double point_ECEF[3] = {3.023e5, 5.636e6, 2.980e6}; + Eigen::Vector3d point_llh(27.99, 86.93, 8820); + Eigen::Vector3d point_ECEF(3.023e5, 5.636e6, 2.980e6); // LLH to ECEF - double point_ECEF_Results[3]; - ecefPointFromLLH(point_llh, point_ECEF_Results); + Eigen::Vector3d point_ECEF_Results = ecefPointFromLLH(point_llh); // Precisions are based on the sig-fig data above -> therefore different // checks have different bounds based on their order of magnitude. - ASSERT_NEAR(point_ECEF[0], point_ECEF_Results[0], 1e2); - ASSERT_NEAR(point_ECEF[1], point_ECEF_Results[1], 1e3); - ASSERT_NEAR(point_ECEF[2], point_ECEF_Results[2], 1e3); + ASSERT_NEAR(point_ECEF(0), point_ECEF_Results(0), 1e2); + ASSERT_NEAR(point_ECEF(1), point_ECEF_Results(1), 1e3); + ASSERT_NEAR(point_ECEF(2), point_ECEF_Results(2), 1e3); // ECEF back to LLH - double point_llh_results[3]; - llhPointFromECEF(point_ECEF_Results, point_llh_results); + Eigen::Vector3d point_llh_results = llhPointFromECEF(point_ECEF_Results); - ASSERT_NEAR(point_llh[0], point_llh_results[0], 1e-2); - ASSERT_NEAR(point_llh[1], point_llh_results[1], 1e-2); - ASSERT_NEAR(point_llh[2], point_llh_results[2], 1.0); + ASSERT_NEAR(point_llh(0), point_llh_results(0), 1e-2); + ASSERT_NEAR(point_llh(1), point_llh_results(1), 1e-2); + ASSERT_NEAR(point_llh(2), point_llh_results(2), 1.0); } TEST(ecefAndLLHPointConversionTest, grandCanyonTest) { @@ -75,24 +37,22 @@ TEST(ecefAndLLHPointConversionTest, grandCanyonTest) { // ECEF[km] ~ (-2323.892, -4595.374, 3750.572) // ECEF[m] ~ (-2.323892e6, -4.595374e6, 3.750572e6) - double point_llh[3] = {36.250278, -116.825833, -85.9536}; - double point_ECEF[3] = {-2.323892e6, -4.595374e6, 3.750572e6}; + Eigen::Vector3d point_llh(36.250278, -116.825833, -85.9536); + Eigen::Vector3d point_ECEF(-2.323892e6, -4.595374e6, 3.750572e6); - double point_ECEF_Results[3]; - ecefPointFromLLH(point_llh, point_ECEF_Results); + Eigen::Vector3d point_ECEF_Results = ecefPointFromLLH(point_llh); // Higher precision inputs should have tighter check - ASSERT_NEAR(point_ECEF[0], point_ECEF_Results[0], 1e1); - ASSERT_NEAR(point_ECEF[1], point_ECEF_Results[1], 1e1); - ASSERT_NEAR(point_ECEF[2], point_ECEF_Results[2], 1e1); + ASSERT_NEAR(point_ECEF(0), point_ECEF_Results(0), 1e1); + ASSERT_NEAR(point_ECEF(1), point_ECEF_Results(1), 1e1); + ASSERT_NEAR(point_ECEF(2), point_ECEF_Results(2), 1e1); // ECEF back to LLH - double point_llh_results[3]; - llhPointFromECEF(point_ECEF_Results, point_llh_results); + Eigen::Vector3d point_llh_results = llhPointFromECEF(point_ECEF_Results); - ASSERT_NEAR(point_llh[0], point_llh_results[0], 1e-2); - ASSERT_NEAR(point_llh[1], point_llh_results[1], 1e-2); - ASSERT_NEAR(point_llh[2], point_llh_results[2], 1.0); + ASSERT_NEAR(point_llh(0), point_llh_results(0), 1e-2); + ASSERT_NEAR(point_llh(1), point_llh_results(1), 1e-2); + ASSERT_NEAR(point_llh(2), point_llh_results(2), 1.0); } } // namespace wave diff --git a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp index 350a5982..8429d880 100644 --- a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp @@ -1,39 +1,3 @@ -/* 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: test_enu_llh_point_conversions.cpp - * Desc: Tests for LLH <-> ENU point conversions - * Auth: Michael Smart - * - * ############################################################################ -*/ - #include #include "wave/geography/world_frame_conversions.hpp" @@ -70,67 +34,47 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // NOTE: Assumes test cases aren't at the poles where the ENU singularity // breaks the expected resulting orientations/locations for the square - double datum_A[3], datum_B[3], datum_C[3], datum_D[3]; - - double llh_A[3], llh_B[3], llh_C[3], llh_D[3]; - double enu_B_wrt_A[3], enu_C_wrt_A[3], enu_D_wrt_A[3]; - double enu_A_wrt_B[3], enu_C_wrt_B[3], enu_D_wrt_B[3]; - double enu_A_wrt_C[3], enu_B_wrt_C[3], enu_D_wrt_C[3]; - double enu_A_wrt_D[3], enu_B_wrt_D[3], enu_C_wrt_D[3]; + Eigen::Vector3d datum_A, datum_B, datum_C, datum_D; - double calculateNormDiff(const double input[3], const double expected[3]) { - double result = - sqrt(pow(input[0] - expected[0], 2) + pow(input[1] - expected[1], 2) + - pow(input[2] - expected[2], 2)); - return result; - } + Eigen::Vector3d llh_A, llh_B, llh_C, llh_D; + Eigen::Vector3d enu_B_wrt_A, enu_C_wrt_A, enu_D_wrt_A; + Eigen::Vector3d enu_A_wrt_B, enu_C_wrt_B, enu_D_wrt_B; + Eigen::Vector3d enu_A_wrt_C, enu_B_wrt_C, enu_D_wrt_C; + Eigen::Vector3d enu_A_wrt_D, enu_B_wrt_D, enu_C_wrt_D; - double calculateNorm(const double input[3]) { - double result = - sqrt(pow(input[0], 2) + pow(input[1], 2) + pow(input[2], 2)); - return result; + double calculateNormDiff(const Eigen::Vector3d input, + const Eigen::Vector3d expected) { + Eigen::Vector3d diff = input - expected; + return diff.norm(); } // Set ground truth points as a square from ENU set at point A void problemSetup() { diagonal_size = sqrt(2.0) * square_size; - enu_B_wrt_A[0] = square_size; - enu_B_wrt_A[1] = 0; - enu_B_wrt_A[2] = 0; - - enu_C_wrt_A[0] = 0; - enu_C_wrt_A[1] = -square_size; - enu_C_wrt_A[2] = 0; - - enu_D_wrt_A[0] = square_size; - enu_D_wrt_A[1] = -square_size; - enu_D_wrt_A[2] = 0; + enu_B_wrt_A << square_size, 0, 0; + enu_C_wrt_A << 0, -square_size, 0; + enu_D_wrt_A << square_size, -square_size, 0; // Compute other LLH values from datum_A - llhPointFromENU(enu_B_wrt_A, datum_A, llh_B, datum_is_llh); - llhPointFromENU(enu_C_wrt_A, datum_A, llh_C, datum_is_llh); - llhPointFromENU(enu_D_wrt_A, datum_A, llh_D, datum_is_llh); + if (datum_is_llh) { + llh_A = datum_A; + } else { + llh_A = llhPointFromECEF(datum_A); + } // Set datum points for other 3 points, and set llh_A + llh_B = llhPointFromENU(enu_B_wrt_A, llh_A); + llh_C = llhPointFromENU(enu_C_wrt_A, llh_A); + llh_D = llhPointFromENU(enu_D_wrt_A, llh_A); if (datum_is_llh) { - llh_A[0] = datum_A[0]; - llh_A[1] = datum_A[1]; - llh_A[2] = datum_A[2]; - datum_B[0] = llh_B[0]; - datum_B[1] = llh_B[1]; - datum_B[2] = llh_B[2]; - datum_C[0] = llh_C[0]; - datum_C[1] = llh_C[1]; - datum_C[2] = llh_C[2]; - datum_D[0] = llh_D[0]; - datum_D[1] = llh_D[1]; - datum_D[2] = llh_D[2]; + datum_B = llh_B; + datum_C = llh_C; + datum_D = llh_D; } else { - llhPointFromECEF(datum_A, llh_A); - ecefPointFromLLH(llh_B, datum_B); - ecefPointFromLLH(llh_C, datum_C); - ecefPointFromLLH(llh_D, datum_D); + datum_B = ecefPointFromLLH(llh_B); + datum_C = ecefPointFromLLH(llh_C); + datum_D = ecefPointFromLLH(llh_D); } } @@ -143,12 +87,19 @@ class enuAndLLHPointConversionTest : public ::testing::Test { } void checkENUFromA() { - double result_enu_B_from_A[3], result_enu_C_from_A[3], - result_enu_D_from_A[3]; + Eigen::Vector3d result_enu_B_from_A, result_enu_C_from_A, + result_enu_D_from_A; - enuPointFromLLH(llh_B, datum_A, result_enu_B_from_A, datum_is_llh); - enuPointFromLLH(llh_C, datum_A, result_enu_C_from_A, datum_is_llh); - enuPointFromLLH(llh_D, datum_A, result_enu_D_from_A, datum_is_llh); + Eigen::Vector3d enu_datum; + if (datum_is_llh) { + enu_datum = datum_A; + } else { + enu_datum = llhPointFromECEF(datum_A); + } + + result_enu_B_from_A = enuPointFromLLH(llh_B, enu_datum); + result_enu_C_from_A = enuPointFromLLH(llh_C, enu_datum); + result_enu_D_from_A = enuPointFromLLH(llh_D, enu_datum); EXPECT_NEAR(0.0, calculateNormDiff(enu_B_wrt_A, result_enu_B_from_A), @@ -161,14 +112,11 @@ class enuAndLLHPointConversionTest : public ::testing::Test { cartesian_check_threshold); // Check that mapping back to LLH matches original 4 points - double result_llh_B_from_A[3], result_llh_C_from_A[3], - result_llh_D_from_A[3]; - llhPointFromENU( - result_enu_B_from_A, datum_A, result_llh_B_from_A, datum_is_llh); - llhPointFromENU( - result_enu_C_from_A, datum_A, result_llh_C_from_A, datum_is_llh); - llhPointFromENU( - result_enu_D_from_A, datum_A, result_llh_D_from_A, datum_is_llh); + Eigen::Vector3d result_llh_B_from_A, result_llh_C_from_A, + result_llh_D_from_A; + result_llh_B_from_A = llhPointFromENU(result_enu_B_from_A, enu_datum); + result_llh_C_from_A = llhPointFromENU(result_enu_C_from_A, enu_datum); + result_llh_D_from_A = llhPointFromENU(result_enu_D_from_A, enu_datum); EXPECT_NEAR(0.0, calculateNormDiff(llh_B, result_llh_B_from_A), llh_check_threshold); @@ -181,22 +129,26 @@ class enuAndLLHPointConversionTest : public ::testing::Test { } void checkENUFromB() { - double result_enu_A_from_B[3], result_enu_C_from_B[3], - result_enu_D_from_B[3]; + Eigen::Vector3d result_enu_A_from_B, result_enu_C_from_B, + result_enu_D_from_B; - enuPointFromLLH(llh_A, datum_B, result_enu_A_from_B, datum_is_llh); - enuPointFromLLH(llh_C, datum_B, result_enu_C_from_B, datum_is_llh); - enuPointFromLLH(llh_D, datum_B, result_enu_D_from_B, datum_is_llh); + Eigen::Vector3d enu_datum; + if (datum_is_llh) { + enu_datum = datum_B; + } else { + enu_datum = llhPointFromECEF(datum_B); + } - EXPECT_NEAR(square_size, - calculateNorm(result_enu_A_from_B), - cartesian_check_threshold); - EXPECT_NEAR(diagonal_size, - calculateNorm(result_enu_C_from_B), - cartesian_check_threshold); - EXPECT_NEAR(square_size, - calculateNorm(result_enu_D_from_B), - cartesian_check_threshold); + result_enu_A_from_B = enuPointFromLLH(llh_A, enu_datum); + result_enu_C_from_B = enuPointFromLLH(llh_C, enu_datum); + result_enu_D_from_B = enuPointFromLLH(llh_D, enu_datum); + + EXPECT_NEAR( + square_size, result_enu_A_from_B.norm(), cartesian_check_threshold); + EXPECT_NEAR( + diagonal_size, result_enu_C_from_B.norm(), cartesian_check_threshold); + EXPECT_NEAR( + square_size, result_enu_D_from_B.norm(), cartesian_check_threshold); // Check ENU signs EXPECT_TRUE(result_enu_A_from_B[0] < 0); @@ -205,14 +157,11 @@ class enuAndLLHPointConversionTest : public ::testing::Test { EXPECT_TRUE(result_enu_D_from_B[1] < 0); // Check that mapping back to LLH matches original 4 points - double result_llh_A_from_B[3], result_llh_C_from_B[3], - result_llh_D_from_B[3]; - llhPointFromENU( - result_enu_A_from_B, datum_B, result_llh_A_from_B, datum_is_llh); - llhPointFromENU( - result_enu_C_from_B, datum_B, result_llh_C_from_B, datum_is_llh); - llhPointFromENU( - result_enu_D_from_B, datum_B, result_llh_D_from_B, datum_is_llh); + Eigen::Vector3d result_llh_A_from_B, result_llh_C_from_B, + result_llh_D_from_B; + result_llh_A_from_B = llhPointFromENU(result_enu_A_from_B, enu_datum); + result_llh_C_from_B = llhPointFromENU(result_enu_C_from_B, enu_datum); + result_llh_D_from_B = llhPointFromENU(result_enu_D_from_B, enu_datum); EXPECT_NEAR(0.0, calculateNormDiff(llh_A, result_llh_A_from_B), llh_check_threshold); @@ -225,22 +174,26 @@ class enuAndLLHPointConversionTest : public ::testing::Test { } void checkENUFromC() { - double result_enu_A_from_C[3], result_enu_B_from_C[3], - result_enu_D_from_C[3]; + Eigen::Vector3d result_enu_A_from_C, result_enu_B_from_C, + result_enu_D_from_C; - enuPointFromLLH(llh_A, datum_C, result_enu_A_from_C, datum_is_llh); - enuPointFromLLH(llh_B, datum_C, result_enu_B_from_C, datum_is_llh); - enuPointFromLLH(llh_D, datum_C, result_enu_D_from_C, datum_is_llh); + Eigen::Vector3d enu_datum; + if (datum_is_llh) { + enu_datum = datum_C; + } else { + enu_datum = llhPointFromECEF(datum_C); + } - EXPECT_NEAR(square_size, - calculateNorm(result_enu_A_from_C), - cartesian_check_threshold); - EXPECT_NEAR(diagonal_size, - calculateNorm(result_enu_B_from_C), - cartesian_check_threshold); - EXPECT_NEAR(square_size, - calculateNorm(result_enu_D_from_C), - cartesian_check_threshold); + result_enu_A_from_C = enuPointFromLLH(llh_A, enu_datum); + result_enu_B_from_C = enuPointFromLLH(llh_B, enu_datum); + result_enu_D_from_C = enuPointFromLLH(llh_D, enu_datum); + + EXPECT_NEAR( + square_size, result_enu_A_from_C.norm(), cartesian_check_threshold); + EXPECT_NEAR( + diagonal_size, result_enu_B_from_C.norm(), cartesian_check_threshold); + EXPECT_NEAR( + square_size, result_enu_D_from_C.norm(), cartesian_check_threshold); // Check ENU signs EXPECT_TRUE(result_enu_A_from_C[1] > 0); @@ -249,14 +202,11 @@ class enuAndLLHPointConversionTest : public ::testing::Test { EXPECT_TRUE(result_enu_D_from_C[0] > 0); // Check that mapping back to LLH matches original 4 points - double result_llh_A_from_C[3], result_llh_B_from_C[3], - result_llh_D_from_C[3]; - llhPointFromENU( - result_enu_A_from_C, datum_C, result_llh_A_from_C, datum_is_llh); - llhPointFromENU( - result_enu_B_from_C, datum_C, result_llh_B_from_C, datum_is_llh); - llhPointFromENU( - result_enu_D_from_C, datum_C, result_llh_D_from_C, datum_is_llh); + Eigen::Vector3d result_llh_A_from_C, result_llh_B_from_C, + result_llh_D_from_C; + result_llh_A_from_C = llhPointFromENU(result_enu_A_from_C, enu_datum); + result_llh_B_from_C = llhPointFromENU(result_enu_B_from_C, enu_datum); + result_llh_D_from_C = llhPointFromENU(result_enu_D_from_C, enu_datum); EXPECT_NEAR(0.0, calculateNormDiff(llh_A, result_llh_A_from_C), llh_check_threshold); @@ -269,22 +219,26 @@ class enuAndLLHPointConversionTest : public ::testing::Test { } void checkENUFromD() { - double result_enu_A_from_D[3], result_enu_B_from_D[3], - result_enu_C_from_D[3]; + Eigen::Vector3d result_enu_A_from_D, result_enu_B_from_D, + result_enu_C_from_D; + + Eigen::Vector3d enu_datum; + if (datum_is_llh) { + enu_datum = datum_D; + } else { + enu_datum = llhPointFromECEF(datum_D); + } - enuPointFromLLH(llh_A, datum_D, result_enu_A_from_D, datum_is_llh); - enuPointFromLLH(llh_B, datum_D, result_enu_B_from_D, datum_is_llh); - enuPointFromLLH(llh_C, datum_D, result_enu_C_from_D, datum_is_llh); + result_enu_A_from_D = enuPointFromLLH(llh_A, enu_datum); + result_enu_B_from_D = enuPointFromLLH(llh_B, enu_datum); + result_enu_C_from_D = enuPointFromLLH(llh_C, enu_datum); - EXPECT_NEAR(diagonal_size, - calculateNorm(result_enu_A_from_D), - cartesian_check_threshold); - EXPECT_NEAR(square_size, - calculateNorm(result_enu_B_from_D), - cartesian_check_threshold); - EXPECT_NEAR(square_size, - calculateNorm(result_enu_C_from_D), - cartesian_check_threshold); + EXPECT_NEAR( + diagonal_size, result_enu_A_from_D.norm(), cartesian_check_threshold); + EXPECT_NEAR( + square_size, result_enu_B_from_D.norm(), cartesian_check_threshold); + EXPECT_NEAR( + square_size, result_enu_C_from_D.norm(), cartesian_check_threshold); // Check ENU signs EXPECT_TRUE(result_enu_A_from_D[0] < 0); @@ -293,14 +247,11 @@ class enuAndLLHPointConversionTest : public ::testing::Test { EXPECT_TRUE(result_enu_C_from_D[0] < 0); // Check that mapping back to LLH matches original 4 points - double result_llh_A_from_D[3], result_llh_B_from_D[3], - result_llh_C_from_D[3]; - llhPointFromENU( - result_enu_A_from_D, datum_D, result_llh_A_from_D, datum_is_llh); - llhPointFromENU( - result_enu_B_from_D, datum_D, result_llh_B_from_D, datum_is_llh); - llhPointFromENU( - result_enu_C_from_D, datum_D, result_llh_C_from_D, datum_is_llh); + Eigen::Vector3d result_llh_A_from_D, result_llh_B_from_D, + result_llh_C_from_D; + result_llh_A_from_D = llhPointFromENU(result_enu_A_from_D, enu_datum); + result_llh_B_from_D = llhPointFromENU(result_enu_B_from_D, enu_datum); + result_llh_C_from_D = llhPointFromENU(result_enu_C_from_D, enu_datum); EXPECT_NEAR(0.0, calculateNormDiff(llh_A, result_llh_A_from_D), llh_check_threshold); @@ -324,25 +275,24 @@ class enuAndLLHPointConversionTest : public ::testing::Test { checkENUFromD(); } - void checkResults(const double datum_llh[3], double input_square_size) { + void checkResults(const Eigen::Vector3d datum_llh, + double input_square_size) { square_size = input_square_size; // Check LLH Pipeline datum_is_llh = true; - datum_A[0] = datum_llh[0]; - datum_A[1] = datum_llh[1]; - datum_A[2] = datum_llh[2]; + datum_A = datum_llh; checkPipeline(); // Check ECEF datum pipeline datum_is_llh = false; - ecefPointFromLLH(datum_llh, datum_A); + datum_A = ecefPointFromLLH(datum_llh); checkPipeline(); } }; TEST_F(enuAndLLHPointConversionTest, LatAt45LongAt0) { - double datum_llh[3] = {45, 0.0, 0.0}; + Eigen::Vector3d datum_llh(45, 0.0, 0.0); checkResults(datum_llh, 1.0); checkResults(datum_llh, 10.0); @@ -352,7 +302,7 @@ TEST_F(enuAndLLHPointConversionTest, LatAt45LongAt0) { } TEST_F(enuAndLLHPointConversionTest, LatAt0LongUnder180Pos) { - double datum_llh[3] = {0, near180, 0.0}; + Eigen::Vector3d datum_llh(0, near180, 0.0); checkResults(datum_llh, 1.0); checkResults(datum_llh, 10.0); @@ -362,7 +312,7 @@ TEST_F(enuAndLLHPointConversionTest, LatAt0LongUnder180Pos) { } TEST_F(enuAndLLHPointConversionTest, LatAt0LongUnder180Neg) { - double datum_llh[3] = {0, -near180, 0.0}; + Eigen::Vector3d datum_llh(0, -near180, 0.0); checkResults(datum_llh, 1.0); checkResults(datum_llh, 10.0); diff --git a/wave_utils/include/wave/wave_test.hpp b/wave_utils/include/wave/wave_test.hpp index da52e27f..44b1014d 100644 --- a/wave_utils/include/wave/wave_test.hpp +++ b/wave_utils/include/wave/wave_test.hpp @@ -29,6 +29,13 @@ inline bool MatricesNear(const MatX &m1, const MatX &m2) { return m1.isApprox(m2); } +/** Predicate to check if matrices are approximately equal, with given + * precision. + * Use with EXPECT_PRED3 */ +inline bool MatricesNearPrec(const MatX &m1, const MatX &m2, double prec) { + return m1.isApprox(m2, prec); +} + } // namespace wave #endif // WAVE_TEST_HPP