Skip to content

Commit

Permalink
SphericalCoordinates: Added SPHERICAL_DEG and SPHERICAL_RAD, deprecat…
Browse files Browse the repository at this point in the history 
…ed SPHERICAL.

This change is made to clarify which units are inputs/outputs to the coordinate transforms.

Signed-off-by: Martin Pecka <[email protected]>
  • Loading branch information
@peci1
peci1 committed Aug 13, 2024
1 parent 00deb01 commit dffb6ab
Show file tree
Hide file tree
Showing 7 changed files with 252 additions and 93 deletions.
11 changes: 11 additions & 0 deletions Migration.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,12 @@ release will remove the deprecated code.

## Gazebo Math 8.X to 9.X

### Additions

1. **SphericalCoordinates.hh**
+ `SphericalCoordinates::CoordinateType::SPHERICAL_DEG`
+ `SphericalCoordinates::CoordinateType::SPHERICAL_RAD`

### Deprecations

1. **graph/Vertex.hh**
Expand All @@ -19,6 +25,11 @@ release will remove the deprecated code.
`Vertex::NullEdge()` instead.
E.g.: https://github.com/gazebosim/gz-math/pull/606/files#diff-0c0220a7e72be70337975433eeddc3f5e072ade5cd80dfb1ac03da233c39c983L222-R222

1. **SphericalCoordinates.hh**
+ `SphericalCoordinates::CoordinateType::SPHERICAL` has been deprecated
because it is unclear what angular units are used. Instead, explicitly
choose either `SPERICAL_DEG` or `SPHERICAL_RAD`.

## Gazebo Math 7.X to 8.X

### Breaking Changes
Expand Down
28 changes: 22 additions & 6 deletions include/gz/math/SphericalCoordinates.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,13 @@
#include <gz/math/config.hh>
#include <gz/utils/ImplPtr.hh>

// MSVC doesn't like deprecating enum values with function macros
#if _MSC_VER
#define GZ_SPHERICAL_DEPRECATED [[deprecated]]
#else
#define GZ_SPHERICAL_DEPRECATED GZ_DEPRECATED(8)
#endif

namespace gz::math
{
// Inline bracket to help doxygen filtering.
Expand Down Expand Up @@ -54,8 +61,15 @@ namespace gz::math
/// \brief Unique identifiers for coordinate types.
public: enum CoordinateType
{
/// \brief Latitude, Longitude and Altitude by SurfaceType
SPHERICAL = 1,
/// \brief Latitude, Longitude and Altitude by SurfaceType [rad].
/// \deprecated Use `SPHERICAL_RAD` or `SPHERICAL_DEG` instead.
SPHERICAL GZ_SPHERICAL_DEPRECATED = 1,

/// \brief Latitude, Longitude and Altitude by SurfaceType [rad].
SPHERICAL_RAD = 1,

/// \brief Latitude, Longitude and Altitude by SurfaceType [deg].
SPHERICAL_DEG = 6,

/// \brief Earth centered, earth fixed Cartesian
ECEF = 2,
Expand Down Expand Up @@ -262,8 +276,9 @@ namespace gz::math
/// \brief Update coordinate transformation matrix with reference location
public: void UpdateTransformationMatrix();

/// \brief Convert between positions in SPHERICAL/ECEF/LOCAL/GLOBAL frame
/// Spherical coordinates use radians, while the other frames use meters.
/// \brief Convert between positions in SPHERICAL_*/ECEF/LOCAL/GLOBAL frame.
/// Cartesian frames use meters. `SPHERICAL_DEG` frame uses degrees.
/// `SPHERICAL_RAD` frame uses radians.
/// \param[in] _pos Position vector in frame defined by parameter _in
/// \param[in] _in CoordinateType for input
/// \param[in] _out CoordinateType for output
Expand All @@ -272,8 +287,9 @@ namespace gz::math
PositionTransform(const gz::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.
/// \brief Convert between velocity in ECEF/LOCAL/GLOBAL frame.
/// Velocity should not be expressed in SPHERICAL_* frames (such values will
/// be passed through). Cartesian 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
52 changes: 26 additions & 26 deletions src/SphericalCoordinates.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -377,21 +377,14 @@ void SphericalCoordinates::SetHeadingOffset(const Angle &_angle)
Vector3d SphericalCoordinates::SphericalFromLocalPosition(
const Vector3d &_xyz) const
{
Vector3d result =
this->PositionTransform(_xyz, LOCAL, SPHERICAL);
result.X(GZ_RTOD(result.X()));
result.Y(GZ_RTOD(result.Y()));
return result;
return this->PositionTransform(_xyz, LOCAL, SPHERICAL_DEG);
}

//////////////////////////////////////////////////
Vector3d SphericalCoordinates::LocalFromSphericalPosition(
const Vector3d &_xyz) const
{
Vector3d result = _xyz;
result.X(GZ_DTOR(result.X()));
result.Y(GZ_DTOR(result.Y()));
return this->PositionTransform(result, SPHERICAL, LOCAL);
return this->PositionTransform(_xyz, SPHERICAL_DEG, LOCAL);
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -511,7 +504,7 @@ void SphericalCoordinates::UpdateTransformationMatrix()
this->dataPtr->longitudeReference.Radian(),
this->dataPtr->elevationReference);
this->dataPtr->origin =
this->PositionTransform(this->dataPtr->origin, SPHERICAL, ECEF);
this->PositionTransform(this->dataPtr->origin, SPHERICAL_RAD, ECEF);
}

/////////////////////////////////////////////////
Expand All @@ -521,17 +514,6 @@ Vector3d SphericalCoordinates::PositionTransform(
{
Vector3d tmp = _pos;

// Cache trig results
double cosLat = cos(_pos.X());
double sinLat = sin(_pos.X());
double cosLon = cos(_pos.Y());
double sinLon = sin(_pos.Y());

// Radius of planet curvature (meters)
double curvature = 1.0 -
this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat;
curvature = this->dataPtr->ellA / sqrt(curvature);

// Convert whatever arrives to a more flexible ECEF coordinate
switch (_in)
{
Expand Down Expand Up @@ -562,8 +544,24 @@ Vector3d SphericalCoordinates::PositionTransform(
break;
}

case SPHERICAL:
case SPHERICAL_DEG:
case SPHERICAL_RAD:
{
// Cache trig results
const double latRad =
(_in == SPHERICAL_DEG) ? GZ_DTOR(_pos.X()) : _pos.X();
const double lonRad =
(_in == SPHERICAL_DEG) ? GZ_DTOR(_pos.Y()) : _pos.Y();
double cosLat = cos(latRad);
double sinLat = sin(latRad);
double cosLon = cos(lonRad);
double sinLon = sin(lonRad);

// Radius of planet curvature (meters)
double curvature =
1.0 - this->dataPtr->ellE * this->dataPtr->ellE * sinLat * sinLat;
curvature = this->dataPtr->ellA / sqrt(curvature);

tmp.X((_pos.Z() + curvature) * cosLat * cosLon);
tmp.Y((_pos.Z() + curvature) * cosLat * sinLon);
tmp.Z(((this->dataPtr->ellB * this->dataPtr->ellB)/
Expand All @@ -585,7 +583,8 @@ Vector3d SphericalCoordinates::PositionTransform(
// Convert ECEF to the requested output coordinate system
switch (_out)
{
case SPHERICAL:
case SPHERICAL_DEG:
case SPHERICAL_RAD:
{
// Convert from ECEF to SPHERICAL
double p = sqrt(tmp.X() * tmp.X() + tmp.Y() * tmp.Y());
Expand All @@ -606,8 +605,8 @@ Vector3d SphericalCoordinates::PositionTransform(
std::pow(sin(lat), 2);
nCurvature = this->dataPtr->ellA / sqrt(nCurvature);

tmp.X(lat);
tmp.Y(lon);
tmp.X(_out == SPHERICAL_DEG ? GZ_RTOD(lat) : lat);
tmp.Y(_out == SPHERICAL_DEG ? GZ_RTOD(lon) : lon);

// Now calculate Z
tmp.Z(p/cos(lat) - nCurvature);
Expand Down Expand Up @@ -648,7 +647,8 @@ Vector3d SphericalCoordinates::VelocityTransform(
const CoordinateType &_in, const CoordinateType &_out) const
{
// Sanity check -- velocity should not be expressed in spherical coordinates
if (_in == SPHERICAL || _out == SPHERICAL)
if (_in == SPHERICAL_RAD || _in == SPHERICAL_DEG ||
_out == SPHERICAL_RAD || _out == SPHERICAL_DEG)
{
return _vel;
}
Expand Down
113 changes: 85 additions & 28 deletions src/SphericalCoordinates_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -291,7 +291,9 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms)
// > latitude longitude altitude
// > X Y Z
math::Vector3d tmp;
math::Vector3d osrf_s(37.3877349, -122.0651166, 32.0);
math::Vector3d osrf_s_deg(37.3877349, -122.0651166, 32.0);
math::Vector3d osrf_s_rad(
GZ_DTOR(osrf_s_deg.X()), GZ_DTOR(osrf_s_deg.Y()), osrf_s_deg.Z());
math::Vector3d osrf_e(
-2693701.91434394, -4299942.14687992, 3851691.0393571);
math::Vector3d goog_s(37.4216719, -122.0821853, 30.0);
Expand All @@ -304,40 +306,72 @@ TEST(SphericalCoordinatesTest, CoordinateTransforms)
// > -1510.88 3766.64 (EAST,NORTH)
math::Vector3d vec(-1510.88, 3766.64, -3.29);

// Convert degrees to radians
osrf_s.X() *= 0.0174532925;
osrf_s.Y() *= 0.0174532925;

// Set the ORIGIN to be the Open Source Robotics Foundation
math::SphericalCoordinates sc2(st, math::Angle(osrf_s.X()),
math::Angle(osrf_s.Y()), osrf_s.Z(),
math::SphericalCoordinates sc2(
st, osrf_s_rad.X(), osrf_s_rad.Y(), osrf_s_rad.Z(),
math::Angle::Zero);

// Check that SPHERICAL -> ECEF works
tmp = sc2.PositionTransform(osrf_s,
math::SphericalCoordinates::SPHERICAL,
// Check that SPHERICAL_DEG -> ECEF works
tmp = sc2.PositionTransform(osrf_s_deg,
math::SphericalCoordinates::SPHERICAL_DEG,
math::SphericalCoordinates::ECEF);

EXPECT_NEAR(tmp.X(), osrf_e.X(), 8e-2);
EXPECT_NEAR(tmp.Y(), osrf_e.Y(), 8e-2);
EXPECT_NEAR(tmp.Z(), osrf_e.Z(), 1e-2);

// Check that SPHERICAL_RAD -> ECEF works
tmp = sc2.PositionTransform(osrf_s_rad,
math::SphericalCoordinates::SPHERICAL_RAD,
math::SphericalCoordinates::ECEF);

EXPECT_NEAR(tmp.X(), osrf_e.X(), 8e-2);
EXPECT_NEAR(tmp.Y(), osrf_e.Y(), 8e-2);
EXPECT_NEAR(tmp.Z(), osrf_e.Z(), 1e-2);

// Check that ECEF -> SPHERICAL works
tmp = sc2.PositionTransform(tmp,
// Check that ECEF -> SPHERICAL_DEG works
tmp = sc2.PositionTransform(osrf_e,
math::SphericalCoordinates::ECEF,
math::SphericalCoordinates::SPHERICAL_DEG);

EXPECT_NEAR(tmp.X(), osrf_s_deg.X(), 1e-2);
EXPECT_NEAR(tmp.Y(), osrf_s_deg.Y(), 1e-2);
EXPECT_NEAR(tmp.Z(), osrf_s_deg.Z(), 1e-2);

// Check that ECEF -> SPHERICAL_RAD works
tmp = sc2.PositionTransform(osrf_e,
math::SphericalCoordinates::ECEF,
math::SphericalCoordinates::SPHERICAL);
math::SphericalCoordinates::SPHERICAL_RAD);

EXPECT_NEAR(tmp.X(), osrf_s_rad.X(), 1e-2);
EXPECT_NEAR(tmp.Y(), osrf_s_rad.Y(), 1e-2);
EXPECT_NEAR(tmp.Z(), osrf_s_rad.Z(), 1e-2);

// Check that SPHERICAL_DEG -> SPHERICAL_RAD works
tmp = sc2.PositionTransform(osrf_s_deg,
math::SphericalCoordinates::SPHERICAL_DEG,
math::SphericalCoordinates::SPHERICAL_RAD);

EXPECT_NEAR(tmp.X(), osrf_s.X(), 1e-2);
EXPECT_NEAR(tmp.Y(), osrf_s.Y(), 1e-2);
EXPECT_NEAR(tmp.Z(), osrf_s.Z(), 1e-2);
EXPECT_NEAR(tmp.X(), osrf_s_rad.X(), 1e-2);
EXPECT_NEAR(tmp.Y(), osrf_s_rad.Y(), 1e-2);
EXPECT_NEAR(tmp.Z(), osrf_s_rad.Z(), 1e-2);

// Check that SPHERICAL -> LOCAL works
// Check that SPHERICAL_RAD -> SPHERICAL_DEG works
tmp = sc2.PositionTransform(osrf_s_rad,
math::SphericalCoordinates::SPHERICAL_RAD,
math::SphericalCoordinates::SPHERICAL_DEG);

EXPECT_NEAR(tmp.X(), osrf_s_deg.X(), 1e-2);
EXPECT_NEAR(tmp.Y(), osrf_s_deg.Y(), 1e-2);
EXPECT_NEAR(tmp.Z(), osrf_s_deg.Z(), 1e-2);

// Check that SPHERICAL_DEG -> LOCAL works
tmp = sc2.LocalFromSphericalPosition(goog_s);
EXPECT_NEAR(tmp.X(), vec.X(), 8e-2);
EXPECT_NEAR(tmp.Y(), vec.Y(), 8e-2);
EXPECT_NEAR(tmp.Z(), vec.Z(), 1e-2);

// Check that SPHERICAL -> LOCAL -> SPHERICAL works
// Check that SPHERICAL_DEG -> LOCAL -> SPHERICAL_DEG works
tmp = sc2.SphericalFromLocalPosition(tmp);
EXPECT_NEAR(tmp.X(), goog_s.X(), 8e-2);
EXPECT_NEAR(tmp.Y(), goog_s.Y(), 8e-2);
Expand Down Expand Up @@ -474,35 +508,45 @@ TEST(SphericalCoordinatesTest, BadCoordinateType)
math::SphericalCoordinates sc;
math::Vector3d pos(1, 2, -4);
math::Vector3d result = sc.PositionTransform(pos,
static_cast<math::SphericalCoordinates::CoordinateType>(7),
static_cast<math::SphericalCoordinates::CoordinateType>(6));
static_cast<math::SphericalCoordinates::CoordinateType>(17),
static_cast<math::SphericalCoordinates::CoordinateType>(16));

EXPECT_EQ(result, pos);

result = sc.PositionTransform(pos,
static_cast<math::SphericalCoordinates::CoordinateType>(4),
static_cast<math::SphericalCoordinates::CoordinateType>(6));
static_cast<math::SphericalCoordinates::CoordinateType>(16));

EXPECT_EQ(result, pos);

result = sc.VelocityTransform(pos,
math::SphericalCoordinates::SPHERICAL_DEG,
math::SphericalCoordinates::ECEF);
EXPECT_EQ(result, pos);

result = sc.VelocityTransform(pos,
math::SphericalCoordinates::ECEF,
math::SphericalCoordinates::SPHERICAL_DEG);
EXPECT_EQ(result, pos);

result = sc.VelocityTransform(pos,
math::SphericalCoordinates::SPHERICAL,
math::SphericalCoordinates::SPHERICAL_RAD,
math::SphericalCoordinates::ECEF);
EXPECT_EQ(result, pos);

result = sc.VelocityTransform(pos,
math::SphericalCoordinates::ECEF,
math::SphericalCoordinates::SPHERICAL);
math::SphericalCoordinates::SPHERICAL_RAD);
EXPECT_EQ(result, pos);

result = sc.VelocityTransform(pos,
static_cast<math::SphericalCoordinates::CoordinateType>(7),
static_cast<math::SphericalCoordinates::CoordinateType>(17),
math::SphericalCoordinates::ECEF);
EXPECT_EQ(result, pos);

result = sc.VelocityTransform(pos,
math::SphericalCoordinates::ECEF,
static_cast<math::SphericalCoordinates::CoordinateType>(7));
static_cast<math::SphericalCoordinates::CoordinateType>(17));
EXPECT_EQ(result, pos);
}

Expand Down Expand Up @@ -773,15 +817,28 @@ TEST(SphericalCoordinatesTest, Inverse)
EXPECT_EQ(in, reverse);
}

// SPHERICAL <-> LOCAL2
// SPHERICAL_DEG <-> LOCAL2
{
math::Vector3d in(1, 2, -4);
auto out = sc.PositionTransform(in,
math::SphericalCoordinates::LOCAL2,
math::SphericalCoordinates::SPHERICAL_DEG);
EXPECT_NE(in, out);
auto reverse = sc.PositionTransform(out,
math::SphericalCoordinates::SPHERICAL_DEG,
math::SphericalCoordinates::LOCAL2);
EXPECT_EQ(in, reverse);
}

// SPHERICAL_RAD <-> LOCAL2
{
math::Vector3d in(1, 2, -4);
auto out = sc.PositionTransform(in,
math::SphericalCoordinates::LOCAL2,
math::SphericalCoordinates::SPHERICAL);
math::SphericalCoordinates::SPHERICAL_RAD);
EXPECT_NE(in, out);
auto reverse = sc.PositionTransform(out,
math::SphericalCoordinates::SPHERICAL,
math::SphericalCoordinates::SPHERICAL_RAD,
math::SphericalCoordinates::LOCAL2);
EXPECT_EQ(in, reverse);
}
Expand Down
Loading

0 comments on commit dffb6ab

Please sign in to comment.