Skip to content

Commit

Permalink
🌐 Spherical coordinates: bug fix, docs and sanity checks (#235)
Browse files Browse the repository at this point in the history 
* Migration guide and LOCAL2

Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Carlos Agüero <caguero@osrfoundation.org>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjo@openrobotics.org>
  • Loading branch information
@chapulina @caguero
@scpeters @arjo129
3 people authored and arjo129 committed Sep 20, 2021
1 parent b1a172f commit 9979f83
Show file tree
Hide file tree
Showing 4 changed files with 370 additions and 16 deletions.
7 changes: 7 additions & 0 deletions Migration.md
View file
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
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
/// `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
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

0 comments on commit 9979f83

Please sign in to comment.