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

SphericalCoordinates: Added SPHERICAL_DEG and SPHERICAL_RAD, deprecated SPHERICAL #597

Closed
wants to merge 1 commit into from
Closed

SphericalCoordinates: Added SPHERICAL_DEG and SPHERICAL_RAD, deprecated SPHERICAL #597

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
11 changes: 11 additions & 0 deletions Migration.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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