From 46d5324c2053151702e59d7e36bf6982a1e3ad71 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 16 Sep 2021 11:34:49 -0700 Subject: [PATCH] =?UTF-8?q?=F0=9F=8C=90=20Spherical=20coordinates:=20bug?= =?UTF-8?q?=20fix,=20docs=20and=20sanity=20checks=20(#235)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Migration guide and LOCAL2 Signed-off-by: Louise Poubel Co-authored-by: Carlos Agüero Co-authored-by: Steve Peters --- Migration.md | 7 + include/ignition/math/SphericalCoordinates.hh | 46 ++- src/SphericalCoordinates.cc | 39 ++- src/SphericalCoordinates_TEST.cc | 294 +++++++++++++++++- 4 files changed, 370 insertions(+), 16 deletions(-) diff --git a/Migration.md b/Migration.md index 4a5cfb2ef..ff607372e 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,13 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Math 6.8 to 6.9 + +1. **SphericalCoordinates**: A bug related to the LOCAL frame was fixed. To + preserve behaviour, the `LOCAL` frame was left with the bug, and a new + `LOCAL2` frame was introduced, which can be used to get the correct + calculations. + ## Ignition Math 4.X to 5.X ### Additions diff --git a/include/ignition/math/SphericalCoordinates.hh b/include/ignition/math/SphericalCoordinates.hh index 8ee48ccc4..120407ab6 100644 --- a/include/ignition/math/SphericalCoordinates.hh +++ b/include/ignition/math/SphericalCoordinates.hh @@ -34,7 +34,6 @@ namespace ignition // class SphericalCoordinatesPrivate; - /// \class SphericalCoordinates SphericalCoordinates.hh commmon/common.hh /// \brief Convert spherical coordinates for planetary surfaces. class IGNITION_MATH_VISIBLE SphericalCoordinates { @@ -61,7 +60,12 @@ namespace ignition GLOBAL = 3, /// \brief Heading-adjusted tangent plane (X, Y, Z) - LOCAL = 4 + /// This has kept a bug for backwards compatibility, use + /// LOCAL2 for the correct behaviour. + LOCAL = 4, + + /// \brief Heading-adjusted tangent plane (X, Y, Z) + LOCAL2 = 5 }; /// \brief Constructor. @@ -91,7 +95,16 @@ namespace ignition public: ~SphericalCoordinates(); /// \brief Convert a Cartesian position vector to geodetic coordinates. - /// \param[in] _xyz Cartesian position vector in the world frame. + /// This performs a `PositionTransform` from LOCAL to SPHERICAL. + /// + /// There's a known bug with this computation that can't be fixed on + /// version 6 to avoid behaviour changes. Directly call + /// `PositionTransform(_xyz, LOCAL2, SPHERICAL)` for correct results. + /// Note that `PositionTransform` returns spherical coordinates in + /// radians. + /// + /// \param[in] _xyz Cartesian position vector in the heading-adjusted + /// world frame. /// \return Cooordinates: geodetic latitude (deg), longitude (deg), /// altitude above sea level (m). public: ignition::math::Vector3d SphericalFromLocalPosition( @@ -99,7 +112,14 @@ namespace ignition /// \brief Convert a Cartesian velocity vector in the local frame /// to a global Cartesian frame with components East, North, Up. - /// \param[in] _xyz Cartesian velocity vector in the world frame. + /// This is a wrapper around `VelocityTransform(_xyz, LOCAL, GLOBAL)` + /// + /// There's a known bug with this computation that can't be fixed on + /// version 6 to avoid behaviour changes. Directly call + /// `VelocityTransform(_xyz, LOCAL2, GLOBAL)` for correct results. + /// + /// \param[in] _xyz Cartesian velocity vector in the heading-adjusted + /// world frame. /// \return Rotated vector with components (x,y,z): (East, North, Up). public: ignition::math::Vector3d GlobalFromLocalVelocity( const ignition::math::Vector3d &_xyz) const; @@ -110,6 +130,11 @@ namespace ignition /// \return Conversion to SurfaceType. public: static SurfaceType Convert(const std::string &_str); + /// \brief Convert a SurfaceType to a string. + /// \param[in] _type Surface type + /// \return Type as string + public: static std::string Convert(SurfaceType _type); + /// \brief Get the distance between two points expressed in geographic /// latitude and longitude. It assumes that both points are at sea level. /// Example: _latA = 38.0016667 and _lonA = -123.0016667) represents @@ -167,13 +192,16 @@ namespace ignition public: void SetHeadingOffset(const ignition::math::Angle &_angle); /// \brief Convert a geodetic position vector to Cartesian coordinates. - /// \param[in] _xyz Geodetic position in the planetary frame of reference - /// \return Cartesian position vector in the world frame + /// This performs a `PositionTransform` from SPHERICAL to LOCAL. + /// \param[in] _latLonEle Geodetic position in the planetary frame of + /// reference. X: latitude (deg), Y: longitude (deg), X: altitude. + /// \return Cartesian position vector in the heading-adjusted world frame. public: ignition::math::Vector3d LocalFromSphericalPosition( - const ignition::math::Vector3d &_xyz) const; + const ignition::math::Vector3d &_latLonEle) const; /// \brief Convert a Cartesian velocity vector with components East, /// North, Up to a local cartesian frame vector XYZ. + /// This is a wrapper around `VelocityTransform(_xyz, GLOBAL, LOCAL)` /// \param[in] _xyz Vector with components (x,y,z): (East, North, Up). /// \return Cartesian vector in the world frame. public: ignition::math::Vector3d LocalFromGlobalVelocity( @@ -183,15 +211,17 @@ namespace ignition public: void UpdateTransformationMatrix(); /// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// Spherical coordinates use radians, while the other frames use meters. /// \param[in] _pos Position vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output - /// \return Transformed coordinate using cached orgin + /// \return Transformed coordinate using cached origin. public: ignition::math::Vector3d PositionTransform(const ignition::math::Vector3d &_pos, const CoordinateType &_in, const CoordinateType &_out) const; /// \brief Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame + /// Spherical coordinates use radians, while the other frames use meters. /// \param[in] _vel Velocity vector in frame defined by parameter _in /// \param[in] _in CoordinateType for input /// \param[in] _out CoordinateType for output diff --git a/src/SphericalCoordinates.cc b/src/SphericalCoordinates.cc index d528b68e0..b73e7b94f 100644 --- a/src/SphericalCoordinates.cc +++ b/src/SphericalCoordinates.cc @@ -99,6 +99,18 @@ SphericalCoordinates::SurfaceType SphericalCoordinates::Convert( return EARTH_WGS84; } +////////////////////////////////////////////////// +std::string SphericalCoordinates::Convert( + SphericalCoordinates::SurfaceType _type) +{ + if (_type == EARTH_WGS84) + return "EARTH_WGS84"; + + std::cerr << "SurfaceType not recognized, " + << "EARTH_WGS84 returned by default" << std::endl; + return "EARTH_WGS84"; +} + ////////////////////////////////////////////////// SphericalCoordinates::SphericalCoordinates() : dataPtr(new SphericalCoordinatesPrivate) @@ -372,8 +384,19 @@ ignition::math::Vector3d SphericalCoordinates::PositionTransform( this->dataPtr->sinHea); tmp.Y(-_pos.X() * this->dataPtr->sinHea - _pos.Y() * this->dataPtr->cosHea); + tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + break; + } + + case LOCAL2: + { + tmp.X(_pos.X() * this->dataPtr->cosHea + _pos.Y() * + this->dataPtr->sinHea); + tmp.Y(-_pos.X() * this->dataPtr->sinHea + _pos.Y() * + this->dataPtr->cosHea); + tmp = this->dataPtr->origin + this->dataPtr->rotGlobalToECEF * tmp; + break; } - /* Falls through. */ case GLOBAL: { @@ -440,6 +463,7 @@ ignition::math::Vector3d SphericalCoordinates::PositionTransform( // Convert from ECEF TO LOCAL case LOCAL: + case LOCAL2: tmp = this->dataPtr->rotECEFToGlobal * (tmp - this->dataPtr->origin); tmp = ignition::math::Vector3d( @@ -477,13 +501,21 @@ ignition::math::Vector3d SphericalCoordinates::VelocityTransform( // First, convert to an ECEF vector switch (_in) { - // ENU (note no break at end of case) + // ENU case LOCAL: tmp.X(-_vel.X() * this->dataPtr->cosHea + _vel.Y() * this->dataPtr->sinHea); tmp.Y(-_vel.X() * this->dataPtr->sinHea - _vel.Y() * this->dataPtr->cosHea); - /* Falls through. */ + tmp = this->dataPtr->rotGlobalToECEF * tmp; + break; + case LOCAL2: + tmp.X(_vel.X() * this->dataPtr->cosHea + _vel.Y() * + this->dataPtr->sinHea); + tmp.Y(-_vel.X() * this->dataPtr->sinHea + _vel.Y() * + this->dataPtr->cosHea); + tmp = this->dataPtr->rotGlobalToECEF * tmp; + break; // spherical case GLOBAL: tmp = this->dataPtr->rotGlobalToECEF * tmp; @@ -511,6 +543,7 @@ ignition::math::Vector3d SphericalCoordinates::VelocityTransform( // Convert from ECEF to local case LOCAL: + case LOCAL2: tmp = this->dataPtr->rotECEFToGlobal * tmp; tmp = ignition::math::Vector3d( tmp.X() * this->dataPtr->cosHea - tmp.Y() * this->dataPtr->sinHea, diff --git a/src/SphericalCoordinates_TEST.cc b/src/SphericalCoordinates_TEST.cc index b7b115991..9af116ec9 100644 --- a/src/SphericalCoordinates_TEST.cc +++ b/src/SphericalCoordinates_TEST.cc @@ -77,6 +77,10 @@ TEST(SphericalCoordinatesTest, Convert) EXPECT_EQ(math::SphericalCoordinates::EARTH_WGS84, math::SphericalCoordinates::Convert("OTHER-COORD")); + + EXPECT_EQ("EARTH_WGS84", math::SphericalCoordinates::Convert(st)); + EXPECT_EQ("EARTH_WGS84", math::SphericalCoordinates::Convert( + static_cast(2))); } ////////////////////////////////////////////////// @@ -128,6 +132,8 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) math::SphericalCoordinates sc(st, lat, lon, elev, heading); // Check GlobalFromLocal with heading offset of 90 degrees + // Heading 0: X == East, Y == North, Z == Up + // Heading 90: X == North, Y == West , Z == Up { // local frame ignition::math::Vector3d xyz; @@ -136,8 +142,8 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) xyz.Set(1, 0, 0); enu = sc.GlobalFromLocalVelocity(xyz); - EXPECT_NEAR(enu.Y(), xyz.X(), 1e-6); - EXPECT_NEAR(enu.X(), -xyz.Y(), 1e-6); + EXPECT_NEAR(enu.Y()/*North*/, xyz.X(), 1e-6); + EXPECT_NEAR(enu.X()/*East*/, -xyz.Y(), 1e-6); EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); xyz.Set(0, 1, 0); @@ -253,6 +259,49 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms) EXPECT_NEAR(tmp.Z(), goog_s.Z(), 1e-2); } } + + // Give no heading offset to confirm ENU frame + { + ignition::math::Angle lat(0.3), lon(-1.2), heading(0.0); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Check GlobalFromLocal with no heading offset + { + // local frame + ignition::math::Vector3d xyz; + // east, north, up + ignition::math::Vector3d enu; + + xyz.Set(1, 0, 0); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(0, 1, 0); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(1, -1, 0); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + + xyz.Set(2243.52334, 556.35, 435.6553); + enu = sc.VelocityTransform(xyz, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(xyz, enu); + EXPECT_EQ(xyz, sc.LocalFromGlobalVelocity(enu)); + } + } } ////////////////////////////////////////////////// @@ -306,7 +355,7 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) math::SphericalCoordinates sc; math::Vector3d pos(1, 2, -4); math::Vector3d result = sc.PositionTransform(pos, - static_cast(5), + static_cast(7), static_cast(6)); EXPECT_EQ(result, pos); @@ -328,13 +377,13 @@ TEST(SphericalCoordinatesTest, BadCoordinateType) EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, - static_cast(5), + static_cast(7), math::SphericalCoordinates::ECEF); EXPECT_EQ(result, pos); result = sc.VelocityTransform(pos, math::SphericalCoordinates::ECEF, - static_cast(5)); + static_cast(7)); EXPECT_EQ(result, pos); } @@ -383,3 +432,238 @@ TEST(SphericalCoordinatesTest, AssignmentOp) math::SphericalCoordinates sc2 = sc1; EXPECT_EQ(sc1, sc2); } + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, NoHeading) +{ + // Default heading + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(IGN_DTOR(-22.9)); + math::Angle lon(IGN_DTOR(-43.2)); + math::Angle heading(0.0); + double elev = 0; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Origin matches input + auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); + EXPECT_DOUBLE_EQ(elev, latLonAlt.Z()); + + auto xyzOrigin = sc.LocalFromSphericalPosition(latLonAlt); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +Y + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() + 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_LT(xyzOrigin.Y(), xyz.Y()); + } + + // Decrease latitude == go South == go -Y + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() - 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Increase longitude == go East == go +X + // Also move a bit -Y because this is the Southern Hemisphere + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() + 1.0, elev}); + EXPECT_LT(xyzOrigin.X(), xyz.X()); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Decrease longitude == go West == go -X + // Also move a bit -Y because this is the Southern Hemisphere + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() - 1.0, elev}); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + } + + // Increase altitude + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree(), elev + 10.0}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_NEAR(xyzOrigin.Z() + 10.0, xyz.Z(), 1e-6); + } + + // Decrease altitude + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree(), elev - 10.0}); + EXPECT_NEAR(xyzOrigin.X(), xyz.X(), 1e-6); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_NEAR(xyzOrigin.Z() - 10.0, xyz.Z(), 1e-6); + } + + // Check how global and local velocities are connected + + // Velocity in + // +X (East), +Y (North), -X (West), -Y (South), +Z (up), -Z (down) + for (auto global : { + math::Vector3d::UnitX, + math::Vector3d::UnitY, + math::Vector3d::UnitZ, + -math::Vector3d::UnitX, + -math::Vector3d::UnitY, + -math::Vector3d::UnitZ}) + { + auto local = sc.LocalFromGlobalVelocity(global); + EXPECT_EQ(global, local); + + // This function is broken for horizontal velocities + global = sc.GlobalFromLocalVelocity(local); + if (abs(global.Z()) < 0.1) + { + EXPECT_NE(global, local); + } + else + { + EXPECT_EQ(global, local); + } + + // Directly call fixed version + global = sc.VelocityTransform(local, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(global, local); + } +} + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, WithHeading) +{ + // Heading 90 deg: X == North, Y == West , Z == Up + auto st = math::SphericalCoordinates::EARTH_WGS84; + math::Angle lat(IGN_DTOR(-22.9)); + math::Angle lon(IGN_DTOR(-43.2)); + math::Angle heading(IGN_DTOR(90.0)); + double elev = 0; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // Origin matches input + auto latLonAlt = sc.SphericalFromLocalPosition({0, 0, 0}); + EXPECT_DOUBLE_EQ(lat.Degree(), latLonAlt.X()); + EXPECT_DOUBLE_EQ(lon.Degree(), latLonAlt.Y()); + EXPECT_DOUBLE_EQ(elev, latLonAlt.Z()); + + auto xyzOrigin = sc.LocalFromSphericalPosition(latLonAlt); + EXPECT_EQ(math::Vector3d::Zero, xyzOrigin); + + // Check how different lat/lon affect the local position + + // Increase latitude == go North == go +X + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() + 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_LT(xyzOrigin.X(), xyz.X()); + } + + // Decrease latitude == go South == go -X + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree() - 1.0, lon.Degree(), elev}); + EXPECT_NEAR(xyzOrigin.Y(), xyz.Y(), 1e-6); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Increase longitude == go East == go -Y (and a bit -X) + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() + 1.0, elev}); + EXPECT_GT(xyzOrigin.Y(), xyz.Y()); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Decrease longitude == go West == go +Y (and a bit -X) + { + auto xyz = sc.LocalFromSphericalPosition( + {lat.Degree(), lon.Degree() - 1.0, elev}); + EXPECT_LT(xyzOrigin.Y(), xyz.Y()); + EXPECT_GT(xyzOrigin.X(), xyz.X()); + } + + // Check how global and local velocities are connected + + // Global | Local + // ---------- | ------ + // +X (East) | -Y + // -X (West) | +Y + // +Y (North) | +X + // -Y (South) | -X + std::vector> globalLocal = + {{math::Vector3d::UnitX, -math::Vector3d::UnitY}, + {-math::Vector3d::UnitX, math::Vector3d::UnitY}, + {math::Vector3d::UnitY, math::Vector3d::UnitX}, + {-math::Vector3d::UnitY, -math::Vector3d::UnitX}}; + for (auto [global, local] : globalLocal) + { + auto localRes = sc.LocalFromGlobalVelocity(global); + EXPECT_EQ(local, localRes); + + // Directly call fixed version + auto globalRes = sc.VelocityTransform(local, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_EQ(global, globalRes); + } +} + +////////////////////////////////////////////////// +TEST(SphericalCoordinatesTest, Inverse) +{ + auto st = math::SphericalCoordinates::EARTH_WGS84; + ignition::math::Angle lat(0.3), lon(-1.2), heading(0.5); + double elev = 354.1; + math::SphericalCoordinates sc(st, lat, lon, elev, heading); + + // GLOBAL <-> LOCAL2 + { + math::Vector3d in(1, 2, -4); + auto out = sc.VelocityTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_NE(in, out); + auto reverse = sc.VelocityTransform(out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } + + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::GLOBAL); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::GLOBAL, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } + + // SPHERICAL <-> LOCAL2 + { + math::Vector3d in(1, 2, -4); + auto out = sc.PositionTransform(in, + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::SPHERICAL); + EXPECT_NE(in, out); + auto reverse = sc.PositionTransform(out, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + EXPECT_EQ(in, reverse); + } +}