From e80a78e3cf62aaf8e5c6246a9c52803abd966cd7 Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Mon, 13 Aug 2018 13:39:59 -0400 Subject: [PATCH 01/11] Remove Autonomoose headers from wave_geography --- .../geography/world_frame_conversions.hpp | 36 ------------------- .../src/world_frame_conversions.cpp | 36 ------------------- .../geography/test_ecef_enu_transforms.cpp | 36 ------------------- .../test_ecef_llh_point_conversions.cpp | 36 ------------------- .../test_enu_llh_point_conversions.cpp | 36 ------------------- 5 files changed, 180 deletions(-) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index d4fe060b..77fb5af8 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/wave_geography/include/wave/geography/world_frame_conversions.hpp @@ -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: world_frame_conversions.hpp - * Desc: Header file for world frame conversion functions - * Auth: Michael Smart - * - * ############################################################################ -*/ - #ifndef WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP #define WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP diff --git a/wave_geography/src/world_frame_conversions.cpp b/wave_geography/src/world_frame_conversions.cpp index 7bcb463d..3e99e107 100644 --- a/wave_geography/src/world_frame_conversions.cpp +++ b/wave_geography/src/world_frame_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: world_frame_conversions.cpp - * Desc: Implementation file for world frame conversion functions - * Auth: Michael Smart - * - * ############################################################################ -*/ - #include #include "wave/geography/world_frame_conversions.hpp" diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index 787cd6e6..bc02d34e 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.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_enu_transforms.cpp - * Desc: Tests for ECEF <-> ENU transform matrices - * Auth: Michael Smart - * - * ############################################################################ -*/ - #include #include "wave/geography/world_frame_conversions.hpp" 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..5885b65a 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" 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..eed838e3 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" From 4d6cf7d238998370336a297349dc72eb2fe3bc93 Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Fri, 17 Aug 2018 17:29:34 -0400 Subject: [PATCH 02/11] Update wave_geography interface Replaces raw arrays of doubles with Eigen matrices/vectors. Also updates wave_geography_tests. --- .../geography/world_frame_conversions.hpp | 177 +++++++-- .../src/world_frame_conversions.cpp | 367 ++++++++++-------- .../geography/test_ecef_enu_transforms.cpp | 123 +++--- .../test_ecef_llh_point_conversions.cpp | 40 +- .../test_enu_llh_point_conversions.cpp | 124 +++--- 5 files changed, 498 insertions(+), 333 deletions(-) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index 77fb5af8..ef389e35 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/wave_geography/include/wave/geography/world_frame_conversions.hpp @@ -2,6 +2,7 @@ #define WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP #include +#include #include #include @@ -14,7 +15,20 @@ namespace wave { * is relative to the WGS84 ellipsoid. * @param[out] ecef the corresponding point in the geocentric ECEF frame. */ -void ecefPointFromLLH(const double llh[3], double ecef[3]); +template +void ecefPointFromLLH(const Eigen::MatrixBase& llh, Eigen::MatrixBase const & ecef){ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 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); + + const_cast< Eigen::MatrixBase& >(ecef) << X, Y, Z; +}; /** Converts a point from LLH (Latitude [deg], Longitude [deg], Height[m]) to * ECEF. @@ -23,41 +37,101 @@ void ecefPointFromLLH(const double llh[3], double ecef[3]); * @param[out] llh 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 +void llhPointFromECEF(const Eigen::MatrixBase& ecef, Eigen::MatrixBase const & llh){ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); -/** Computes the 3D Affine transform from ECEF to a local datum-defined ENU - * frame as a 4x4 row-major matrix. + 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); + + const_cast< Eigen::MatrixBase& >(llh) << 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_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[out] T_ecef_enu 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 enuFromECEFTransformMatrix(const double datum[3], - double T_enu_ecef[4][4], - bool datum_is_llh = true); +template +void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, + Eigen::MatrixBase const & T_ecef_enu, + bool datum_is_llh = true){ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); -/** Computes the 3D Affine transform from a local datum-defined ENU frame to - * ECEF as a 4x4 row-major matrix. + // 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); + } + + const_cast< Eigen::MatrixBase& >(T_ecef_enu) << R_ecef_enu[0], R_ecef_enu[1], R_ecef_enu[2], datum_X, + R_ecef_enu[3], R_ecef_enu[4], R_ecef_enu[5], datum_Y, + R_ecef_enu[6], R_ecef_enu[7], R_ecef_enu[8], datum_Z, + 0.0, 0.0, 0.0, 1.0; +}; + +/** 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_ecef_enu the 4x4 row-major transformation matrix converting - * column-vector points from the local ENU frame defined by the datum point to - * ECEF. + * @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 ecefFromENUTransformMatrix(const double datum[3], - double T_ecef_enu[4][4], - bool datum_is_llh = true); +template +void enuFromECEFTransformMatrix(const Eigen::MatrixBase& datum, + Eigen::MatrixBase const & T_enu_ecef, + bool datum_is_llh = true){ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); + EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); + + // Get T_ecef_enu and then invert it + Eigen::Matrix4d T_ecef_enu; + ecefFromENUTransformMatrix(datum, T_ecef_enu, datum_is_llh); + + // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t] + (const_cast< Eigen::MatrixBase& >(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 + (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).topRightCorner(3,1) = - T_ecef_enu.topLeftCorner(3,3).transpose() * T_ecef_enu.topRightCorner(3,1); + (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).row(3) = Eigen::MatrixXd::Zero(1,4); + (const_cast< Eigen::MatrixBase& >(T_enu_ecef))(3,3) = 1.0; +}; /** Converts a source point from LLH to a target ENU point in the local * Cartesian ENU frame defined by the provided datum. @@ -71,10 +145,36 @@ void ecefFromENUTransformMatrix(const double datum[3], * (default).
* \b false: The given datum values are ECEF */ -void enuPointFromLLH(const double point_llh[3], - const double enu_datum[3], - double point_enu[3], - bool datum_is_llh = true); +template +void enuPointFromLLH(const Eigen::MatrixBase& point_llh, + const Eigen::MatrixBase& enu_datum, + Eigen::MatrixBase const & point_enu, + bool datum_is_llh = true){ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); + + Eigen::Vector3d enu_datum_llh; + 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), + (const_cast< Eigen::MatrixBase& >(point_enu))(0), + (const_cast< Eigen::MatrixBase& >(point_enu))(1), + (const_cast< Eigen::MatrixBase& >(point_enu))(2)); +}; /** Converts a source point from ENU in the local Cartesian ENU frame * defined by the provided datum to a target LLH point. @@ -88,10 +188,35 @@ void enuPointFromLLH(const double point_llh[3], * (default).
* \b false: The given datum values are ECEF */ -void llhPointFromENU(const double point_enu[3], - const double enu_datum[3], - double point_llh[3], - bool datum_is_llh = true); +template +void llhPointFromENU(const Eigen::MatrixBase& point_enu, + const Eigen::MatrixBase& enu_datum, + Eigen::MatrixBase const & point_llh, + bool datum_is_llh = true){ + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); + Eigen::Vector3d enu_datum_llh; + 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), + (const_cast< Eigen::MatrixBase& >(point_llh))(0), + (const_cast< Eigen::MatrixBase& >(point_llh))(1), + (const_cast< Eigen::MatrixBase& >(point_llh))(2)); +}; } // 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 index 3e99e107..0b78566d 100644 --- a/wave_geography/src/world_frame_conversions.cpp +++ b/wave_geography/src/world_frame_conversions.cpp @@ -1,170 +1,197 @@ -#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 +//#include +//#include "wave/geography/world_frame_conversions.hpp" +// +//namespace wave { +// +//template +//void ecefPointFromLLH(const Eigen::MatrixBase& llh, Eigen::MatrixBase const & ecef) { +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 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); +// +// (const_cast< Eigen::MatrixBase& >(ecef))(0) = X; +// (const_cast< Eigen::MatrixBase& >(ecef))(1) = Y; +// (const_cast< Eigen::MatrixBase& >(ecef))(2) = Z; +//} +// +//template +//void llhPointFromECEF(const Eigen::MatrixBase& ecef, Eigen::MatrixBase const & llh) { +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 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); +// +// (const_cast< Eigen::MatrixBase& >(llh))(0) = latitude; +// (const_cast< Eigen::MatrixBase& >(llh))(1) = longitude; +// (const_cast< Eigen::MatrixBase& >(llh))(2) = height; +//} +// +//template +//void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, +// Eigen::MatrixBase const & T_ecef_enu, +// bool datum_is_llh) { +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); +// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); +// +// // 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); +// } +// +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,0) = R_ecef_enu[0]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,1) = R_ecef_enu[1]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,2) = R_ecef_enu[2]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,3) = datum_X; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,0) = R_ecef_enu[3]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,1) = R_ecef_enu[4]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,2) = R_ecef_enu[5]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,3) = datum_Y; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,0) = R_ecef_enu[6]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,1) = R_ecef_enu[7]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,2) = R_ecef_enu[8]; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,3) = datum_Z; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,0) = 0.0; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,1) = 0.0; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,2) = 0.0; +// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,3) = 1.0; +//} +// +//template +//void enuFromECEFTransformMatrix(const Eigen::MatrixBase& datum, +// Eigen::MatrixBase const & T_enu_ecef, +// bool datum_is_llh) { +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); +// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); +// +// // Get T_ecef_enu and then invert it +// Eigen::Matrix4d T_ecef_enu; +// 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; */ +// +// (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).topLeftCorner(3,3) = T_ecef_enu.topLeftCorner(3,3).transpose(); +// (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).col(4) = - T_ecef_enu.topLeftCorner(3,3).transpose() * T_ecef_enu.col(4); +// (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).row(4) = Eigen::MatrixXd::Zero(1,4); +// (const_cast< Eigen::MatrixBase& >(T_enu_ecef))(3)(3) = 1.0; +//} +// +//template +//void enuPointFromLLH(const Eigen::MatrixBase& point_llh, +// const Eigen::MatrixBase& enu_datum, +// Eigen::MatrixBase const & point_enu, +// bool datum_is_llh){ +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); +// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); +// +// Eigen::Vector3d enu_datum_llh; +// 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), +// (const_cast< Eigen::MatrixBase& >(point_enu))(0), +// (const_cast< Eigen::MatrixBase& >(point_enu))(1), +// (const_cast< Eigen::MatrixBase& >(point_enu))(2)); +//} +// +//template +//void llhPointFromENU(const Eigen::MatrixBase& point_enu, +// const Eigen::MatrixBase& enu_datum, +// Eigen::MatrixBase const & point_llh, +// bool datum_is_llh) { +// Eigen::Vector3d enu_datum_llh; +// 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), +// (const_cast< Eigen::MatrixBase& >(point_llh))(0), +// (const_cast< Eigen::MatrixBase& >(point_llh))(1), +// (const_cast< Eigen::MatrixBase& >(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 bc02d34e..af648c6e 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -10,59 +10,59 @@ 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], + EXPECT_NEAR(expected_R_ENU_ECEF(i_row,j_col), + T_ECEF_ENU(j_col,i_row), 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], + EXPECT_NEAR(expected_R_ENU_ECEF(i_row,j_col), + T_ENU_ECEF(i_row,j_col), 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]; + 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 { @@ -72,10 +72,10 @@ class ECEFtoENUTest : public ::testing::Test { } } - 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]; + void checkLLHDatumPipeline(Eigen::Vector3d datum_llh, + Eigen::Matrix3d expected_R_ENU_ECEF) { + Eigen::Matrix4d T_ENU_ECEF; + Eigen::Matrix4d T_ECEF_ENU; ecefFromENUTransformMatrix(datum_llh, T_ECEF_ENU); enuFromECEFTransformMatrix(datum_llh, T_ENU_ECEF); @@ -87,10 +87,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]; + void checkECEFDatumPipeline(Eigen::Vector3d datum_ECEF, + Eigen::Matrix3d expected_R_ENU_ECEF) { + Eigen::Matrix4d T_ENU_ECEF; + Eigen::Matrix4d T_ECEF_ENU; ecefFromENUTransformMatrix(datum_ECEF, T_ECEF_ENU, false); enuFromECEFTransformMatrix(datum_ECEF, T_ENU_ECEF, false); @@ -102,12 +102,12 @@ 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]; + Eigen::Vector3d datum_ecef; ecefPointFromLLH(datum_llh, datum_ecef); checkECEFDatumPipeline(datum_ecef, expected_R_ENU_ECEF); } @@ -127,11 +127,15 @@ 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}, // + 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; + /*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}}; + {0.0, 0.0, 1.0}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } @@ -150,11 +154,16 @@ 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 R_ENU_ECEF_result[3][3] = {{0.0, -1.0, 0.0}, // + //double datum_llh[3] = {near90, near180, 0.0}; + Eigen::Vector3d datum_llh(near90, near180, 0.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; + /*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}}; + {0.0, 0.0, 1.0}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } @@ -174,11 +183,15 @@ 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}; + Eigen::Vector3d datum_llh(0.0, 90.0, 0.0); - double R_ENU_ECEF_result[3][3] = {{-1.0, 0.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; + /*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}}; + {0.0, 1.0, 0.0}};*/ checkResult(datum_llh, R_ENU_ECEF_result); @@ -199,11 +212,15 @@ 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}, // + 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; + /*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}}; + {0.0, -1.0, 0.0}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } @@ -223,11 +240,15 @@ 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}, // + 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; + /*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}}; + {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 5885b65a..5255658d 100644 --- a/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp @@ -10,26 +10,26 @@ 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]; + Eigen::Vector3d point_ECEF_Results; ecefPointFromLLH(point_llh, point_ECEF_Results); // 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]; + Eigen::Vector3d point_llh_results; llhPointFromECEF(point_ECEF_Results, point_llh_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) { @@ -39,24 +39,24 @@ 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]; + Eigen::Vector3d point_ECEF_Results; ecefPointFromLLH(point_llh, point_ECEF_Results); // 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]; + Eigen::Vector3d point_llh_results; llhPointFromECEF(point_ECEF_Results, point_llh_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 eed838e3..f56b9394 100644 --- a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp @@ -34,42 +34,44 @@ 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]; + Eigen::Vector3d datum_A, datum_B, datum_C, datum_D; - 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 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 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; + double calculateNormDiff(const Eigen::Vector3d input, const Eigen::Vector3d expected) { + Eigen::Vector3d diff = input - expected; + return diff.norm(); } - double calculateNorm(const double input[3]) { + /*double calculateNorm(const double input[3]) { double result = sqrt(pow(input[0], 2) + pow(input[1], 2) + pow(input[2], 2)); return result; - } + }*/ // 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_B_wrt_A << square_size, 0, 0; + enu_C_wrt_A << 0, -square_size, 0; + enu_D_wrt_A << square_size, -square_size, 0; + + /* 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_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_D_wrt_A(0) = square_size; + enu_D_wrt_A(1) = -square_size; + enu_D_wrt_A(2) = 0;*/ // Compute other LLH values from datum_A llhPointFromENU(enu_B_wrt_A, datum_A, llh_B, datum_is_llh); @@ -78,18 +80,10 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // Set datum points for other 3 points, and set 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]; + llh_A = datum_A; + datum_B = llh_B; + datum_C = llh_C; + datum_D = llh_D; } else { llhPointFromECEF(datum_A, llh_A); ecefPointFromLLH(llh_B, datum_B); @@ -107,8 +101,8 @@ 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); @@ -125,8 +119,8 @@ 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]; + Eigen::Vector3d result_llh_B_from_A, result_llh_C_from_A, + result_llh_D_from_A; llhPointFromENU( result_enu_B_from_A, datum_A, result_llh_B_from_A, datum_is_llh); llhPointFromENU( @@ -145,21 +139,21 @@ 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); EXPECT_NEAR(square_size, - calculateNorm(result_enu_A_from_B), + result_enu_A_from_B.norm(), cartesian_check_threshold); EXPECT_NEAR(diagonal_size, - calculateNorm(result_enu_C_from_B), + result_enu_C_from_B.norm(), cartesian_check_threshold); EXPECT_NEAR(square_size, - calculateNorm(result_enu_D_from_B), + result_enu_D_from_B.norm(), cartesian_check_threshold); // Check ENU signs @@ -169,8 +163,8 @@ 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]; + Eigen::Vector3d result_llh_A_from_B, result_llh_C_from_B, + result_llh_D_from_B; llhPointFromENU( result_enu_A_from_B, datum_B, result_llh_A_from_B, datum_is_llh); llhPointFromENU( @@ -189,21 +183,21 @@ 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); EXPECT_NEAR(square_size, - calculateNorm(result_enu_A_from_C), + result_enu_A_from_C.norm(), cartesian_check_threshold); EXPECT_NEAR(diagonal_size, - calculateNorm(result_enu_B_from_C), + result_enu_B_from_C.norm(), cartesian_check_threshold); EXPECT_NEAR(square_size, - calculateNorm(result_enu_D_from_C), + result_enu_D_from_C.norm(), cartesian_check_threshold); // Check ENU signs @@ -213,8 +207,8 @@ 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]; + Eigen::Vector3d result_llh_A_from_C, result_llh_B_from_C, + result_llh_D_from_C; llhPointFromENU( result_enu_A_from_C, datum_C, result_llh_A_from_C, datum_is_llh); llhPointFromENU( @@ -233,21 +227,21 @@ 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; 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); EXPECT_NEAR(diagonal_size, - calculateNorm(result_enu_A_from_D), + result_enu_A_from_D.norm(), cartesian_check_threshold); EXPECT_NEAR(square_size, - calculateNorm(result_enu_B_from_D), + result_enu_B_from_D.norm(), cartesian_check_threshold); EXPECT_NEAR(square_size, - calculateNorm(result_enu_C_from_D), + result_enu_C_from_D.norm(), cartesian_check_threshold); // Check ENU signs @@ -257,8 +251,8 @@ 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]; + Eigen::Vector3d result_llh_A_from_D, result_llh_B_from_D, + result_llh_C_from_D; llhPointFromENU( result_enu_A_from_D, datum_D, result_llh_A_from_D, datum_is_llh); llhPointFromENU( @@ -288,14 +282,12 @@ 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 @@ -306,7 +298,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { }; 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); @@ -316,7 +308,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); @@ -326,7 +318,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); From 7da7ea4ae856f37a02a736c80f0efee873ef1a0f Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Fri, 17 Aug 2018 17:45:39 -0400 Subject: [PATCH 03/11] Remove src folder from wave_geography --- wave_geography/CMakeLists.txt | 4 +- .../src/world_frame_conversions.cpp | 197 ------------------ 2 files changed, 1 insertion(+), 200 deletions(-) delete mode 100644 wave_geography/src/world_frame_conversions.cpp 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/src/world_frame_conversions.cpp b/wave_geography/src/world_frame_conversions.cpp deleted file mode 100644 index 0b78566d..00000000 --- a/wave_geography/src/world_frame_conversions.cpp +++ /dev/null @@ -1,197 +0,0 @@ -//#include -//#include "wave/geography/world_frame_conversions.hpp" -// -//namespace wave { -// -//template -//void ecefPointFromLLH(const Eigen::MatrixBase& llh, Eigen::MatrixBase const & ecef) { -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 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); -// -// (const_cast< Eigen::MatrixBase& >(ecef))(0) = X; -// (const_cast< Eigen::MatrixBase& >(ecef))(1) = Y; -// (const_cast< Eigen::MatrixBase& >(ecef))(2) = Z; -//} -// -//template -//void llhPointFromECEF(const Eigen::MatrixBase& ecef, Eigen::MatrixBase const & llh) { -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 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); -// -// (const_cast< Eigen::MatrixBase& >(llh))(0) = latitude; -// (const_cast< Eigen::MatrixBase& >(llh))(1) = longitude; -// (const_cast< Eigen::MatrixBase& >(llh))(2) = height; -//} -// -//template -//void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, -// Eigen::MatrixBase const & T_ecef_enu, -// bool datum_is_llh) { -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); -// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); -// -// // 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); -// } -// -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,0) = R_ecef_enu[0]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,1) = R_ecef_enu[1]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,2) = R_ecef_enu[2]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(0,3) = datum_X; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,0) = R_ecef_enu[3]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,1) = R_ecef_enu[4]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,2) = R_ecef_enu[5]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(1,3) = datum_Y; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,0) = R_ecef_enu[6]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,1) = R_ecef_enu[7]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,2) = R_ecef_enu[8]; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(2,3) = datum_Z; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,0) = 0.0; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,1) = 0.0; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,2) = 0.0; -// (const_cast< Eigen::MatrixBase& >(T_ecef_enu))(3,3) = 1.0; -//} -// -//template -//void enuFromECEFTransformMatrix(const Eigen::MatrixBase& datum, -// Eigen::MatrixBase const & T_enu_ecef, -// bool datum_is_llh) { -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); -// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); -// -// // Get T_ecef_enu and then invert it -// Eigen::Matrix4d T_ecef_enu; -// 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; */ -// -// (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).topLeftCorner(3,3) = T_ecef_enu.topLeftCorner(3,3).transpose(); -// (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).col(4) = - T_ecef_enu.topLeftCorner(3,3).transpose() * T_ecef_enu.col(4); -// (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).row(4) = Eigen::MatrixXd::Zero(1,4); -// (const_cast< Eigen::MatrixBase& >(T_enu_ecef))(3)(3) = 1.0; -//} -// -//template -//void enuPointFromLLH(const Eigen::MatrixBase& point_llh, -// const Eigen::MatrixBase& enu_datum, -// Eigen::MatrixBase const & point_enu, -// bool datum_is_llh){ -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); -// EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); -// -// Eigen::Vector3d enu_datum_llh; -// 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), -// (const_cast< Eigen::MatrixBase& >(point_enu))(0), -// (const_cast< Eigen::MatrixBase& >(point_enu))(1), -// (const_cast< Eigen::MatrixBase& >(point_enu))(2)); -//} -// -//template -//void llhPointFromENU(const Eigen::MatrixBase& point_enu, -// const Eigen::MatrixBase& enu_datum, -// Eigen::MatrixBase const & point_llh, -// bool datum_is_llh) { -// Eigen::Vector3d enu_datum_llh; -// 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), -// (const_cast< Eigen::MatrixBase& >(point_llh))(0), -// (const_cast< Eigen::MatrixBase& >(point_llh))(1), -// (const_cast< Eigen::MatrixBase& >(point_llh))(2)); -//} -// -//} // namespace wave From b9cfda64e9f37cc1054f70f9b44f1cebbee09923 Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Mon, 20 Aug 2018 15:34:23 -0400 Subject: [PATCH 04/11] Fix documentation generation Before, the API reference was not properly generated when running `make doc` --- CMakeLists.txt | 1 + docs/CMakeLists.txt | 7 +++---- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 76084e5c..e46979b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,7 @@ ADD_SUBDIRECTORY(wave_geography) # Documentation SET(WAVE_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}) SET(WAVE_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}) +SET(WAVE_MODULES utils geometry containers benchmark controls kinematics matching vision optimization gtsam geography) ADD_SUBDIRECTORY(docs) # This is where .cmake files will be installed (default under share/) diff --git a/docs/CMakeLists.txt b/docs/CMakeLists.txt index 5d5d8f5b..9c3c07b6 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(wave_${module}/include) + set(WAVE_DOXY_INPUT "${WAVE_DOXY_INPUT} ${WAVE_SOURCE_DIR}/wave_${module}") endforeach() - find_package(Doxygen) if(DOXYGEN_FOUND) configure_file(Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY) From 5450b185a1e694bced835ba85869e909b45cf753 Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Mon, 20 Aug 2018 16:22:38 -0400 Subject: [PATCH 05/11] Include wave_geography in doxygen documentation Descriptions of the functions already existed, just needed to add the doxygen tags so that the functions show up in the docs. --- .../include/wave/geography/world_frame_conversions.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index ef389e35..ec4be218 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/wave_geography/include/wave/geography/world_frame_conversions.hpp @@ -1,3 +1,7 @@ +/** @file + * @ingroup geography + */ + #ifndef WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP #define WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP @@ -7,6 +11,8 @@ #include namespace wave { +/** @addtogroup geography + * @{ */ /** Converts a point from LLH (Latitude [deg], Longitude [deg], Height[m]) to * ECEF. @@ -218,5 +224,6 @@ void llhPointFromENU(const Eigen::MatrixBase& point_enu, (const_cast< Eigen::MatrixBase& >(point_llh))(2)); }; +/** @} group geography */ } // namespace wave #endif // WAVE_GEOGRAPHY_WORLD_FRAME_CONVERSIONS_HPP From 50be75202b3b6ef96321a351407c244fa0b1858c Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Mon, 20 Aug 2018 16:31:15 -0400 Subject: [PATCH 06/11] Run format script --- .../geography/world_frame_conversions.hpp | 91 ++++++++++--------- .../geography/test_ecef_enu_transforms.cpp | 65 ++++++------- .../test_enu_llh_point_conversions.cpp | 69 +++++++------- 3 files changed, 108 insertions(+), 117 deletions(-) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index ec4be218..11d5f4dc 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/wave_geography/include/wave/geography/world_frame_conversions.hpp @@ -21,8 +21,9 @@ namespace wave { * is relative to the WGS84 ellipsoid. * @param[out] ecef the corresponding point in the geocentric ECEF frame. */ -template -void ecefPointFromLLH(const Eigen::MatrixBase& llh, Eigen::MatrixBase const & ecef){ +template +void ecefPointFromLLH(const Eigen::MatrixBase &llh, + Eigen::MatrixBase const &ecef) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); @@ -33,7 +34,7 @@ void ecefPointFromLLH(const Eigen::MatrixBase& llh, Eigen::MatrixBase< double X, Y, Z; earth.Forward(latitude, longitude, height, X, Y, Z); - const_cast< Eigen::MatrixBase& >(ecef) << X, Y, Z; + const_cast &>(ecef) << X, Y, Z; }; /** Converts a point from LLH (Latitude [deg], Longitude [deg], Height[m]) to @@ -43,8 +44,9 @@ void ecefPointFromLLH(const Eigen::MatrixBase& llh, Eigen::MatrixBase< * @param[out] llh the corresponding llh point as (Latitude, Longitude, * Height). Height is relative to the WGS84 ellipsoid. */ -template -void llhPointFromECEF(const Eigen::MatrixBase& ecef, Eigen::MatrixBase const & llh){ +template +void llhPointFromECEF(const Eigen::MatrixBase &ecef, + Eigen::MatrixBase const &llh) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); @@ -55,7 +57,8 @@ void llhPointFromECEF(const Eigen::MatrixBase& ecef, Eigen::MatrixBase double latitude, longitude, height; earth.Reverse(X, Y, Z, latitude, longitude, height); - const_cast< Eigen::MatrixBase& >(llh) << latitude, longitude, height; + const_cast &>(llh) << latitude, longitude, + height; }; /** Computes the 3D Affine transform from a local datum-defined ENU frame to @@ -71,10 +74,10 @@ void llhPointFromECEF(const Eigen::MatrixBase& ecef, Eigen::MatrixBase * (default).
* \b false: The given datum values are ECEF */ -template -void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, - Eigen::MatrixBase const & T_ecef_enu, - bool datum_is_llh = true){ +template +void ecefFromENUTransformMatrix(const Eigen::MatrixBase &datum, + Eigen::MatrixBase const &T_ecef_enu, + bool datum_is_llh = true) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); @@ -88,7 +91,7 @@ void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, double latitude = datum(0), longitude = datum(1), height = datum(2); earth.Forward( - latitude, longitude, height, datum_X, datum_Y, datum_Z, R_ecef_enu); + latitude, longitude, height, datum_X, datum_Y, datum_Z, R_ecef_enu); } else { // Datum is already given in ECEF datum_X = datum(0); @@ -97,13 +100,13 @@ void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, double latitude, longitude, height; earth.Reverse( - datum_X, datum_Y, datum_Z, latitude, longitude, height, R_ecef_enu); + datum_X, datum_Y, datum_Z, latitude, longitude, height, R_ecef_enu); } - const_cast< Eigen::MatrixBase& >(T_ecef_enu) << R_ecef_enu[0], R_ecef_enu[1], R_ecef_enu[2], datum_X, - R_ecef_enu[3], R_ecef_enu[4], R_ecef_enu[5], datum_Y, - R_ecef_enu[6], R_ecef_enu[7], R_ecef_enu[8], datum_Z, - 0.0, 0.0, 0.0, 1.0; + const_cast &>(T_ecef_enu) << R_ecef_enu[0], + R_ecef_enu[1], R_ecef_enu[2], datum_X, R_ecef_enu[3], R_ecef_enu[4], + R_ecef_enu[5], datum_Y, R_ecef_enu[6], R_ecef_enu[7], R_ecef_enu[8], + datum_Z, 0.0, 0.0, 0.0, 1.0; }; /** Computes the 3D Affine transform from ECEF to a local datum-defined ENU @@ -119,10 +122,10 @@ void ecefFromENUTransformMatrix(const Eigen::MatrixBase& datum, * (default).
* \b false: The given datum values are ECEF */ -template -void enuFromECEFTransformMatrix(const Eigen::MatrixBase& datum, - Eigen::MatrixBase const & T_enu_ecef, - bool datum_is_llh = true){ +template +void enuFromECEFTransformMatrix(const Eigen::MatrixBase &datum, + Eigen::MatrixBase const &T_enu_ecef, + bool datum_is_llh = true) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); @@ -131,12 +134,16 @@ void enuFromECEFTransformMatrix(const Eigen::MatrixBase& datum, ecefFromENUTransformMatrix(datum, T_ecef_enu, datum_is_llh); // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t] - (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).topLeftCorner(3,3) = T_ecef_enu.topLeftCorner(3,3).transpose(); + (const_cast &>(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 - (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).topRightCorner(3,1) = - T_ecef_enu.topLeftCorner(3,3).transpose() * T_ecef_enu.topRightCorner(3,1); - (const_cast< Eigen::MatrixBase& >(T_enu_ecef)).row(3) = Eigen::MatrixXd::Zero(1,4); - (const_cast< Eigen::MatrixBase& >(T_enu_ecef))(3,3) = 1.0; + (const_cast &>(T_enu_ecef)) + .topRightCorner(3, 1) = -T_ecef_enu.topLeftCorner(3, 3).transpose() * + T_ecef_enu.topRightCorner(3, 1); + (const_cast &>(T_enu_ecef)).row(3) = + Eigen::MatrixXd::Zero(1, 4); + (const_cast &>(T_enu_ecef))(3, 3) = 1.0; }; /** Converts a source point from LLH to a target ENU point in the local @@ -151,11 +158,11 @@ void enuFromECEFTransformMatrix(const Eigen::MatrixBase& datum, * (default).
* \b false: The given datum values are ECEF */ -template -void enuPointFromLLH(const Eigen::MatrixBase& point_llh, - const Eigen::MatrixBase& enu_datum, - Eigen::MatrixBase const & point_enu, - bool datum_is_llh = true){ +template +void enuPointFromLLH(const Eigen::MatrixBase &point_llh, + const Eigen::MatrixBase &enu_datum, + Eigen::MatrixBase const &point_enu, + bool datum_is_llh = true) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); @@ -172,14 +179,14 @@ void enuPointFromLLH(const Eigen::MatrixBase& point_llh, GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84(); GeographicLib::LocalCartesian localENU( - enu_datum_llh(0), enu_datum_llh(1), enu_datum_llh(2), earth); + enu_datum_llh(0), enu_datum_llh(1), enu_datum_llh(2), earth); localENU.Forward(point_llh(0), point_llh(1), point_llh(2), - (const_cast< Eigen::MatrixBase& >(point_enu))(0), - (const_cast< Eigen::MatrixBase& >(point_enu))(1), - (const_cast< Eigen::MatrixBase& >(point_enu))(2)); + (const_cast &>(point_enu))(0), + (const_cast &>(point_enu))(1), + (const_cast &>(point_enu))(2)); }; /** Converts a source point from ENU in the local Cartesian ENU frame @@ -194,11 +201,11 @@ void enuPointFromLLH(const Eigen::MatrixBase& point_llh, * (default).
* \b false: The given datum values are ECEF */ -template -void llhPointFromENU(const Eigen::MatrixBase& point_enu, - const Eigen::MatrixBase& enu_datum, - Eigen::MatrixBase const & point_llh, - bool datum_is_llh = true){ +template +void llhPointFromENU(const Eigen::MatrixBase &point_enu, + const Eigen::MatrixBase &enu_datum, + Eigen::MatrixBase const &point_llh, + bool datum_is_llh = true) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); @@ -214,14 +221,14 @@ void llhPointFromENU(const Eigen::MatrixBase& point_enu, GeographicLib::Geocentric earth = GeographicLib::Geocentric::WGS84(); GeographicLib::LocalCartesian localENU( - enu_datum_llh(0), enu_datum_llh(1), enu_datum_llh(2), earth); + enu_datum_llh(0), enu_datum_llh(1), enu_datum_llh(2), earth); localENU.Reverse(point_enu(0), point_enu(1), point_enu(2), - (const_cast< Eigen::MatrixBase& >(point_llh))(0), - (const_cast< Eigen::MatrixBase& >(point_llh))(1), - (const_cast< Eigen::MatrixBase& >(point_llh))(2)); + (const_cast &>(point_llh))(0), + (const_cast &>(point_llh))(1), + (const_cast &>(point_llh))(2)); }; /** @} group geography */ diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index af648c6e..bb49f277 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -15,22 +15,22 @@ class ECEFtoENUTest : public ::testing::Test { // 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), + EXPECT_NEAR(expected_R_ENU_ECEF(i_row, j_col), + T_ECEF_ENU(j_col, i_row), 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(Eigen::Matrix4d T_ENU_ECEF, @@ -38,31 +38,31 @@ class ECEFtoENUTest : public ::testing::Test { // 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), + EXPECT_NEAR(expected_R_ENU_ECEF(i_row, j_col), + T_ENU_ECEF(i_row, j_col), 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(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); + 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 { @@ -102,7 +102,8 @@ class ECEFtoENUTest : public ::testing::Test { checkTransformInverse(T_ECEF_ENU, T_ENU_ECEF); } - void checkResult(Eigen::Vector3d datum_llh, Eigen::Matrix3d expected_R_ENU_ECEF) { + 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); @@ -130,9 +131,7 @@ TEST_F(ECEFtoENUTest, LatNear90LongAt0) { Eigen::Vector3d datum_llh(near90, 0.0, 0.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; + R_ENU_ECEF_result << 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.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}};*/ @@ -154,13 +153,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); 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; + R_ENU_ECEF_result << 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.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}};*/ @@ -186,9 +183,7 @@ TEST_F(ECEFtoENUTest, LatNear0LongNear90) { 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; + R_ENU_ECEF_result << -1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.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}};*/ @@ -215,9 +210,7 @@ TEST_F(ECEFtoENUTest, LatNear0LongNearN90) { 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; + R_ENU_ECEF_result << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.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}};*/ @@ -243,9 +236,7 @@ TEST_F(ECEFtoENUTest, IdentityPoint) { Eigen::Vector3d datum_llh(near90, -90.0, 0.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; + R_ENU_ECEF_result << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.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}};*/ 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 f56b9394..858b06b3 100644 --- a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp @@ -42,7 +42,8 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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 calculateNormDiff(const Eigen::Vector3d input, const Eigen::Vector3d expected) { + double calculateNormDiff(const Eigen::Vector3d input, + const Eigen::Vector3d expected) { Eigen::Vector3d diff = input - expected; return diff.norm(); } @@ -61,17 +62,17 @@ class enuAndLLHPointConversionTest : public ::testing::Test { enu_C_wrt_A << 0, -square_size, 0; enu_D_wrt_A << square_size, -square_size, 0; - /* enu_B_wrt_A(0) = square_size; - enu_B_wrt_A(1) = 0; - enu_B_wrt_A(2) = 0; + /* 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_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_D_wrt_A(0) = square_size; + enu_D_wrt_A(1) = -square_size; + enu_D_wrt_A(2) = 0;*/ // Compute other LLH values from datum_A llhPointFromENU(enu_B_wrt_A, datum_A, llh_B, datum_is_llh); @@ -146,15 +147,12 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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); - 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); + 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); @@ -190,15 +188,12 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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); - 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); + 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); @@ -234,15 +229,12 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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); - 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); + 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); @@ -282,7 +274,8 @@ class enuAndLLHPointConversionTest : public ::testing::Test { checkENUFromD(); } - void checkResults(const Eigen::Vector3d datum_llh, double input_square_size) { + void checkResults(const Eigen::Vector3d datum_llh, + double input_square_size) { square_size = input_square_size; // Check LLH Pipeline From 32e7da872793e6a79fba54343bfcd83b791c3e62 Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Thu, 23 Aug 2018 16:38:39 -0400 Subject: [PATCH 07/11] Loop through WAVE_MODULES list when adding subdirectories --- CMakeLists.txt | 27 +++++++++++++++------------ docs/CMakeLists.txt | 4 ++-- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e46979b5..458524aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,22 +106,25 @@ 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}) SET(WAVE_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}) -SET(WAVE_MODULES utils geometry containers benchmark controls kinematics matching vision optimization gtsam geography) ADD_SUBDIRECTORY(docs) # This is where .cmake files will be installed (default under share/) diff --git a/docs/CMakeLists.txt b/docs/CMakeLists.txt index 9c3c07b6..35ef3424 100644 --- a/docs/CMakeLists.txt +++ b/docs/CMakeLists.txt @@ -5,8 +5,8 @@ set(WAVE_DOXY_INPUT "${WAVE_SOURCE_DIR}/README.md ${CMAKE_CURRENT_SOURCE_DIR}/ref") foreach(module ${WAVE_MODULES}) - include_directories(wave_${module}/include) - set(WAVE_DOXY_INPUT "${WAVE_DOXY_INPUT} ${WAVE_SOURCE_DIR}/wave_${module}") + include_directories(${module}/include) + set(WAVE_DOXY_INPUT "${WAVE_DOXY_INPUT} ${WAVE_SOURCE_DIR}/${module}") endforeach() find_package(Doxygen) From 72f006c9fcaa03e15fd15ea695c82a8beaafdc3a Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Fri, 24 Aug 2018 14:36:49 -0400 Subject: [PATCH 08/11] Use return values instead of output parameters --- .../geography/world_frame_conversions.hpp | 188 ++++++------------ .../geography/test_ecef_enu_transforms.cpp | 18 +- .../test_ecef_llh_point_conversions.cpp | 12 +- .../test_enu_llh_point_conversions.cpp | 109 +++++----- 4 files changed, 137 insertions(+), 190 deletions(-) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index 11d5f4dc..d24bdb26 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/wave_geography/include/wave/geography/world_frame_conversions.hpp @@ -19,13 +19,11 @@ namespace wave { * * @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. */ -template -void ecefPointFromLLH(const Eigen::MatrixBase &llh, - Eigen::MatrixBase const &ecef) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 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); @@ -34,21 +32,19 @@ void ecefPointFromLLH(const Eigen::MatrixBase &llh, double X, Y, Z; earth.Forward(latitude, longitude, height, X, Y, Z); - const_cast &>(ecef) << 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. */ -template -void llhPointFromECEF(const Eigen::MatrixBase &ecef, - Eigen::MatrixBase const &llh) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); +template +Eigen::Vector3d llhPointFromECEF(const Eigen::MatrixBase &ecef) { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); double X = ecef(0), Y = ecef(1), Z = ecef(2); @@ -57,178 +53,120 @@ void llhPointFromECEF(const Eigen::MatrixBase &ecef, double latitude, longitude, height; earth.Reverse(X, Y, Z, latitude, longitude, height); - const_cast &>(llh) << 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 */ -template -void ecefFromENUTransformMatrix(const Eigen::MatrixBase &datum, - Eigen::MatrixBase const &T_ecef_enu, - bool datum_is_llh = true) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); +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 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); - } - - const_cast &>(T_ecef_enu) << R_ecef_enu[0], - R_ecef_enu[1], R_ecef_enu[2], datum_X, R_ecef_enu[3], R_ecef_enu[4], - R_ecef_enu[5], datum_Y, R_ecef_enu[6], R_ecef_enu[7], R_ecef_enu[8], - datum_Z, 0.0, 0.0, 0.0, 1.0; + + 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 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 + * @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. - * @param[in] datum_is_llh \b true: The given datum values are LLH - * (default).
- * \b false: The given datum values are ECEF */ -template -void enuFromECEFTransformMatrix(const Eigen::MatrixBase &datum, - Eigen::MatrixBase const &T_enu_ecef, - bool datum_is_llh = true) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); - EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(DerivedB, 4, 4); +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; - ecefFromENUTransformMatrix(datum, T_ecef_enu, datum_is_llh); + Eigen::Matrix4d T_ecef_enu = ecefEnuTransformFromEcef(datum); + Eigen::Matrix4d T_enu_ecef; // Affine inverse: [R | t]^(-1) = [ R^T | - R^T * t] - (const_cast &>(T_enu_ecef)) - .topLeftCorner(3, 3) = T_ecef_enu.topLeftCorner(3, 3).transpose(); + 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 - (const_cast &>(T_enu_ecef)) - .topRightCorner(3, 1) = -T_ecef_enu.topLeftCorner(3, 3).transpose() * + T_enu_ecef.topRightCorner(3, 1) = -T_ecef_enu.topLeftCorner(3, 3).transpose() * T_ecef_enu.topRightCorner(3, 1); - (const_cast &>(T_enu_ecef)).row(3) = - Eigen::MatrixXd::Zero(1, 4); - (const_cast &>(T_enu_ecef))(3, 3) = 1.0; + 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. */ -template -void enuPointFromLLH(const Eigen::MatrixBase &point_llh, - const Eigen::MatrixBase &enu_datum, - Eigen::MatrixBase const &point_enu, - 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); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); - - Eigen::Vector3d enu_datum_llh; - 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); + 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), - (const_cast &>(point_enu))(0), - (const_cast &>(point_enu))(1), - (const_cast &>(point_enu))(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. */ -template -void llhPointFromENU(const Eigen::MatrixBase &point_enu, - const Eigen::MatrixBase &enu_datum, - Eigen::MatrixBase const &point_llh, - 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); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedC, 3); - Eigen::Vector3d enu_datum_llh; - 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); + 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), - (const_cast &>(point_llh))(0), - (const_cast &>(point_llh))(1), - (const_cast &>(point_llh))(2)); + A, + B, + C); + + return Eigen::Vector3d(A,B,C); }; /** @} group geography */ diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index bb49f277..19cd1487 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -74,11 +74,9 @@ class ECEFtoENUTest : public ::testing::Test { void checkLLHDatumPipeline(Eigen::Vector3d datum_llh, Eigen::Matrix3d expected_R_ENU_ECEF) { - Eigen::Matrix4d T_ENU_ECEF; - Eigen::Matrix4d T_ECEF_ENU; - - ecefFromENUTransformMatrix(datum_llh, T_ECEF_ENU); - enuFromECEFTransformMatrix(datum_llh, T_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); @@ -89,11 +87,8 @@ class ECEFtoENUTest : public ::testing::Test { void checkECEFDatumPipeline(Eigen::Vector3d datum_ECEF, Eigen::Matrix3d expected_R_ENU_ECEF) { - Eigen::Matrix4d T_ENU_ECEF; - Eigen::Matrix4d T_ECEF_ENU; - - ecefFromENUTransformMatrix(datum_ECEF, T_ECEF_ENU, false); - enuFromECEFTransformMatrix(datum_ECEF, T_ENU_ECEF, false); + 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); @@ -108,8 +103,7 @@ class ECEFtoENUTest : public ::testing::Test { checkLLHDatumPipeline(datum_llh, expected_R_ENU_ECEF); // Get ECEF datum from LLH and check ECEF version of pipeline - Eigen::Vector3d datum_ecef; - ecefPointFromLLH(datum_llh, datum_ecef); + Eigen::Vector3d datum_ecef = ecefPointFromLLH(datum_llh); checkECEFDatumPipeline(datum_ecef, expected_R_ENU_ECEF); } }; 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 5255658d..53f06fbc 100644 --- a/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_ecef_llh_point_conversions.cpp @@ -14,8 +14,7 @@ TEST(ecefAndLLHPointConversionTest, everestTest) { Eigen::Vector3d point_ECEF(3.023e5, 5.636e6, 2.980e6); // LLH to ECEF - Eigen::Vector3d point_ECEF_Results; - 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. @@ -24,8 +23,7 @@ TEST(ecefAndLLHPointConversionTest, everestTest) { ASSERT_NEAR(point_ECEF(2), point_ECEF_Results(2), 1e3); // ECEF back to LLH - Eigen::Vector3d point_llh_results; - 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); @@ -42,8 +40,7 @@ TEST(ecefAndLLHPointConversionTest, grandCanyonTest) { Eigen::Vector3d point_llh(36.250278, -116.825833, -85.9536); Eigen::Vector3d point_ECEF(-2.323892e6, -4.595374e6, 3.750572e6); - Eigen::Vector3d point_ECEF_Results; - 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); @@ -51,8 +48,7 @@ TEST(ecefAndLLHPointConversionTest, grandCanyonTest) { ASSERT_NEAR(point_ECEF(2), point_ECEF_Results(2), 1e1); // ECEF back to LLH - Eigen::Vector3d point_llh_results; - 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); 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 858b06b3..9dab685b 100644 --- a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp @@ -75,21 +75,24 @@ class enuAndLLHPointConversionTest : public ::testing::Test { enu_D_wrt_A(2) = 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 = datum_A; 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); } } @@ -105,9 +108,16 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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), @@ -122,12 +132,9 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // Check that mapping back to LLH matches original 4 points Eigen::Vector3d result_llh_B_from_A, result_llh_C_from_A, result_llh_D_from_A; - 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); + 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); @@ -143,9 +150,16 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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); + } + + 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); @@ -163,12 +177,9 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // Check that mapping back to LLH matches original 4 points Eigen::Vector3d result_llh_A_from_B, result_llh_C_from_B, result_llh_D_from_B; - 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); + 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); @@ -184,9 +195,16 @@ class enuAndLLHPointConversionTest : public ::testing::Test { 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); + } + + 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); @@ -204,12 +222,9 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // Check that mapping back to LLH matches original 4 points Eigen::Vector3d result_llh_A_from_C, result_llh_B_from_C, result_llh_D_from_C; - 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); + 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); @@ -225,9 +240,16 @@ class enuAndLLHPointConversionTest : public ::testing::Test { Eigen::Vector3d result_enu_A_from_D, result_enu_B_from_D, result_enu_C_from_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); + Eigen::Vector3d enu_datum; + if (datum_is_llh){ + enu_datum = datum_D; + } else { + enu_datum = llhPointFromECEF(datum_D); + } + + 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, result_enu_A_from_D.norm(), cartesian_check_threshold); @@ -245,12 +267,9 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // Check that mapping back to LLH matches original 4 points Eigen::Vector3d result_llh_A_from_D, result_llh_B_from_D, result_llh_C_from_D; - 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); + 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); @@ -285,7 +304,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { // Check ECEF datum pipeline datum_is_llh = false; - ecefPointFromLLH(datum_llh, datum_A); + datum_A = ecefPointFromLLH(datum_llh); checkPipeline(); } }; From f4316c3c8001529e3666b273baa7d5f90e1ab271 Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Fri, 24 Aug 2018 14:43:52 -0400 Subject: [PATCH 09/11] Remove commented out code --- .../geography/test_ecef_enu_transforms.cpp | 16 ---------------- .../test_enu_llh_point_conversions.cpp | 18 ------------------ 2 files changed, 34 deletions(-) diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index 19cd1487..c31cefcc 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -126,9 +126,6 @@ TEST_F(ECEFtoENUTest, LatNear90LongAt0) { 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; - /*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}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } @@ -152,9 +149,6 @@ TEST_F(ECEFtoENUTest, LatNear90LongNear180) { 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; - /*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}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } @@ -178,10 +172,6 @@ TEST_F(ECEFtoENUTest, LatNear0LongNear90) { 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; - /*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}};*/ - checkResult(datum_llh, R_ENU_ECEF_result); } @@ -205,9 +195,6 @@ TEST_F(ECEFtoENUTest, LatNear0LongNearN90) { 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; - /*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}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } @@ -231,9 +218,6 @@ TEST_F(ECEFtoENUTest, IdentityPoint) { 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; - /*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}};*/ checkResult(datum_llh, R_ENU_ECEF_result); } 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 9dab685b..57645128 100644 --- a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp @@ -48,12 +48,6 @@ class enuAndLLHPointConversionTest : public ::testing::Test { return diff.norm(); } - /*double calculateNorm(const double input[3]) { - double result = - sqrt(pow(input[0], 2) + pow(input[1], 2) + pow(input[2], 2)); - return result; - }*/ - // Set ground truth points as a square from ENU set at point A void problemSetup() { diagonal_size = sqrt(2.0) * square_size; @@ -62,18 +56,6 @@ class enuAndLLHPointConversionTest : public ::testing::Test { enu_C_wrt_A << 0, -square_size, 0; enu_D_wrt_A << square_size, -square_size, 0; - /* 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;*/ - // Compute other LLH values from datum_A if (datum_is_llh){ llh_A = datum_A; From 015f755cc603efa5c8b248a29bf670b1e340b97b Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Fri, 24 Aug 2018 15:57:05 -0400 Subject: [PATCH 10/11] Use Eigen's isApprox() instead of looping through a matrix --- .../geography/test_ecef_enu_transforms.cpp | 30 +++---------------- wave_utils/include/wave/wave_test.hpp | 6 ++++ 2 files changed, 10 insertions(+), 26 deletions(-) diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index c31cefcc..f17bc1ae 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -1,5 +1,6 @@ #include #include "wave/geography/world_frame_conversions.hpp" +#include "wave/wave_test.hpp" namespace wave { @@ -13,13 +14,7 @@ class ECEFtoENUTest : public ::testing::Test { 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) + @@ -36,13 +31,7 @@ class ECEFtoENUTest : public ::testing::Test { 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); @@ -58,18 +47,7 @@ class ECEFtoENUTest : public ::testing::Test { 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(Eigen::Vector3d datum_llh, diff --git a/wave_utils/include/wave/wave_test.hpp b/wave_utils/include/wave/wave_test.hpp index da52e27f..04445121 100644 --- a/wave_utils/include/wave/wave_test.hpp +++ b/wave_utils/include/wave/wave_test.hpp @@ -29,6 +29,12 @@ 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 From 158f1370e96cbd71fea72610f535b323eaf7423d Mon Sep 17 00:00:00 2001 From: Kathleen Wang Date: Fri, 24 Aug 2018 16:04:16 -0400 Subject: [PATCH 11/11] Run format script --- .../geography/world_frame_conversions.hpp | 44 ++++++++----------- .../geography/test_ecef_enu_transforms.cpp | 13 ++++-- .../test_enu_llh_point_conversions.cpp | 10 ++--- wave_utils/include/wave/wave_test.hpp | 3 +- 4 files changed, 35 insertions(+), 35 deletions(-) diff --git a/wave_geography/include/wave/geography/world_frame_conversions.hpp b/wave_geography/include/wave/geography/world_frame_conversions.hpp index d24bdb26..ba7af74d 100644 --- a/wave_geography/include/wave/geography/world_frame_conversions.hpp +++ b/wave_geography/include/wave/geography/world_frame_conversions.hpp @@ -65,7 +65,8 @@ Eigen::Vector3d llhPointFromECEF(const Eigen::MatrixBase &ecef) { * ECEF. */ template -Eigen::Matrix4d ecefEnuTransformFromEcef(const Eigen::MatrixBase &datum) { +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 @@ -78,10 +79,9 @@ Eigen::Matrix4d ecefEnuTransformFromEcef(const Eigen::MatrixBase &datum 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; + 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; }; @@ -94,7 +94,8 @@ Eigen::Matrix4d ecefEnuTransformFromEcef(const Eigen::MatrixBase &datum * point. */ template -Eigen::Matrix4d enuEcefTransformFromEcef(const Eigen::MatrixBase &datum) { +Eigen::Matrix4d enuEcefTransformFromEcef( + const Eigen::MatrixBase &datum) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); // Get T_ecef_enu and then invert it @@ -105,8 +106,9 @@ Eigen::Matrix4d enuEcefTransformFromEcef(const Eigen::MatrixBase &datum 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.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; @@ -122,7 +124,7 @@ Eigen::Matrix4d enuEcefTransformFromEcef(const Eigen::MatrixBase &datum */ template Eigen::Vector3d enuPointFromLLH(const Eigen::MatrixBase &point_llh, - const Eigen::MatrixBase &enu_datum) { + const Eigen::MatrixBase &enu_datum) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); @@ -130,15 +132,10 @@ Eigen::Vector3d enuPointFromLLH(const Eigen::MatrixBase &point_llh, 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); + double A, B, C; + localENU.Forward(point_llh(0), point_llh(1), point_llh(2), A, B, C); - return Eigen::Vector3d(A,B,C); + return Eigen::Vector3d(A, B, C); }; /** Converts a source point from ENU in the local Cartesian ENU frame @@ -150,7 +147,7 @@ Eigen::Vector3d enuPointFromLLH(const Eigen::MatrixBase &point_llh, */ template Eigen::Vector3d llhPointFromENU(const Eigen::MatrixBase &point_enu, - const Eigen::MatrixBase &enu_datum) { + const Eigen::MatrixBase &enu_datum) { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedA, 3); EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(DerivedB, 3); @@ -158,15 +155,10 @@ Eigen::Vector3d llhPointFromENU(const Eigen::MatrixBase &point_enu, 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); + double A, B, C; + localENU.Reverse(point_enu(0), point_enu(1), point_enu(2), A, B, C); - return Eigen::Vector3d(A,B,C); + return Eigen::Vector3d(A, B, C); }; /** @} group geography */ diff --git a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp index f17bc1ae..95338c72 100644 --- a/wave_geography/tests/geography/test_ecef_enu_transforms.cpp +++ b/wave_geography/tests/geography/test_ecef_enu_transforms.cpp @@ -14,7 +14,10 @@ class ECEFtoENUTest : public ::testing::Test { void checkT_ECEF_ENU(Eigen::Matrix4d T_ECEF_ENU, Eigen::Matrix3d expected_R_ENU_ECEF) { // Compare R vs R^T - EXPECT_PRED3(MatricesNearPrec, expected_R_ENU_ECEF, T_ECEF_ENU.topLeftCorner(3,3).transpose(), 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) + @@ -31,7 +34,10 @@ class ECEFtoENUTest : public ::testing::Test { void checkT_ENU_ECEF(Eigen::Matrix4d T_ENU_ECEF, Eigen::Matrix3d expected_R_ENU_ECEF) { // Compare R's directly - EXPECT_PRED3(MatricesNearPrec, expected_R_ENU_ECEF, T_ENU_ECEF.topLeftCorner(3,3), 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); @@ -47,7 +53,8 @@ class ECEFtoENUTest : public ::testing::Test { void checkTransformInverse(Eigen::Matrix4d T1, Eigen::Matrix4d T2) { // Multiplication should produce identity - EXPECT_PRED3(MatricesNearPrec, T1*T2, Eigen::Matrix4d::Identity(), 1e-6); + EXPECT_PRED3( + MatricesNearPrec, T1 * T2, Eigen::Matrix4d::Identity(), 1e-6); } void checkLLHDatumPipeline(Eigen::Vector3d datum_llh, 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 57645128..8429d880 100644 --- a/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp +++ b/wave_geography/tests/geography/test_enu_llh_point_conversions.cpp @@ -57,7 +57,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { enu_D_wrt_A << square_size, -square_size, 0; // Compute other LLH values from datum_A - if (datum_is_llh){ + if (datum_is_llh) { llh_A = datum_A; } else { llh_A = llhPointFromECEF(datum_A); @@ -91,7 +91,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { result_enu_D_from_A; Eigen::Vector3d enu_datum; - if (datum_is_llh){ + if (datum_is_llh) { enu_datum = datum_A; } else { enu_datum = llhPointFromECEF(datum_A); @@ -133,7 +133,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { result_enu_D_from_B; Eigen::Vector3d enu_datum; - if (datum_is_llh){ + if (datum_is_llh) { enu_datum = datum_B; } else { enu_datum = llhPointFromECEF(datum_B); @@ -178,7 +178,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { result_enu_D_from_C; Eigen::Vector3d enu_datum; - if (datum_is_llh){ + if (datum_is_llh) { enu_datum = datum_C; } else { enu_datum = llhPointFromECEF(datum_C); @@ -223,7 +223,7 @@ class enuAndLLHPointConversionTest : public ::testing::Test { result_enu_C_from_D; Eigen::Vector3d enu_datum; - if (datum_is_llh){ + if (datum_is_llh) { enu_datum = datum_D; } else { enu_datum = llhPointFromECEF(datum_D); diff --git a/wave_utils/include/wave/wave_test.hpp b/wave_utils/include/wave/wave_test.hpp index 04445121..44b1014d 100644 --- a/wave_utils/include/wave/wave_test.hpp +++ b/wave_utils/include/wave/wave_test.hpp @@ -29,7 +29,8 @@ inline bool MatricesNear(const MatX &m1, const MatX &m2) { return m1.isApprox(m2); } -/** Predicate to check if matrices are approximately equal, with given precision. +/** 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);