Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

🌐 Spherical coordinates: bug fix, docs and sanity checks #235

Merged
merged 11 commits into from
Sep 16, 2021
+370 −16
Merged

🌐 Spherical coordinates: bug fix, docs and sanity checks #235

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
81b3353
Spherical coordinates: bug fix, docs and sanity checks
chapulina Sep 3, 2021
60b9785
Migration guide and LOCAL2
chapulina Sep 3, 2021
009cc49
codecheck
chapulina Sep 3, 2021
42dbfb5
convert back
chapulina Sep 3, 2021
083216a
fix warning
chapulina Sep 3, 2021
c8465db
more tests, explain Y change
chapulina Sep 10, 2021
c925cf0
Merge branch 'ign-math6' into chapulina/6/spherical
chapulina Sep 10, 2021
a8933ca
Z tests
chapulina Sep 10, 2021
e16fce4
Document radians vs deg
chapulina Sep 15, 2021
0d74ad9
Merge branch 'ign-math6' into chapulina/6/spherical
caguero Sep 15, 2021
c64b173
Merge branch 'ign-math6' into chapulina/6/spherical
scpeters Sep 16, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions Migration.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
46 changes: 38 additions & 8 deletions include/ignition/math/SphericalCoordinates.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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.
Expand Down Expand Up @@ -91,15 +95,31 @@ 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
Comment on lines +100 to +101
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@scpeters The comments indicate that the behavior was only intended to be kept for gz-math6. However, it slipped through to gz-math7. Is there some will to straighten this up in 8, or will we live with this forever?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

sure, we can fix it in gz-math8

can you open an issue or pull request to main?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good, I'll try the PR.

I've noticed another discrepancy in the interface: SphericalFromLocalPosition returns spherical coordinates in degrees, while PositionTransform uses radians. Shouldn't this be also united to using degrees everywhere? Or was there a reason for keeping it this way?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think consistency would be preferred; it was probably oversight in the first place that caused them to diverge. A std::vector<math::Angle> type would handle the units more clearly but it a little more overhead than Vector3d

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, I've sent the PRs: #596, #597.

/// `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(
const ignition::math::Vector3d &_xyz) const;

/// \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;
Expand All @@ -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
Expand Down Expand Up @@ -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(
Expand All @@ -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
Expand Down
39 changes: 36 additions & 3 deletions src/SphericalCoordinates.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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:
{
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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,
Expand Down
Loading