Utility class for converting between WGS84 lat/lon and an ortho-rectified LocalXY coordinate system. More...
#include <local_xy_util.h>
Public Member Functions | |
std::string | Frame () const |
Return the TF frame ID corresponding to the local origin. | |
bool | Initialized () const |
Return whether the object has been initialized. | |
LocalXyWgs84Util (double reference_latitude, double reference_longitude, double reference_angle=0, double reference_altitude=0) | |
Initializing constructor. | |
LocalXyWgs84Util () | |
Zero-argument constructor. | |
double | ReferenceAltitude () const |
Return the altitude coordinate of the local origin. | |
double | ReferenceAngle () const |
Return the reference angle in degrees ENU. | |
double | ReferenceLatitude () const |
Return the latitude coordinate of the local origin. | |
double | ReferenceLongitude () const |
Return the longitude coordinate of the local origin. | |
bool | ToLocalXy (double latitude, double longitude, double &x, double &y) const |
Convert WGS84 latitude and longitude to LocalXY. | |
bool | ToWgs84 (double x, double y, double &latitude, double &longitude) const |
Convert LocalXY to WGS84 latitude and longitude. | |
Protected Member Functions | |
void | HandleOrigin (const topic_tools::ShapeShifter::ConstPtr origin) |
void | Initialize () |
Protected Attributes | |
double | cos_angle_ |
std::string | frame_ |
bool | initialized_ |
ros::Subscriber | origin_sub_ |
double | reference_altitude_ |
double | reference_angle_ |
double | reference_latitude_ |
double | reference_longitude_ |
double | rho_lat_ |
double | rho_lon_ |
double | sin_angle_ |
Utility class for converting between WGS84 lat/lon and an ortho-rectified LocalXY coordinate system.
To use this class, first construct it with a reference origin. The reference origin should be a latitude, longitude, angle, and altitude in WGS84 coordinates. Once initialized, a LocalXyWgs84Util can be used to convert WGS84 coordinates to and from an ortho-rectified frame with its origin at the reference origin. Because the earth is spherical, the error in the ortho-rectified frame will accumulate as the distance from the reference origin increases. For this reason, the reference origin should be chosen to be close to the region of interest (<10 km).
It is strongly recommended to use 0 degrees for the angle. This corresponds to the X-axis of the ortho-rectified frame pointing east.
Definition at line 99 of file local_xy_util.h.
swri_transform_util::LocalXyWgs84Util::LocalXyWgs84Util | ( | double | reference_latitude, |
double | reference_longitude, | ||
double | reference_angle = 0 , |
||
double | reference_altitude = 0 |
||
) |
Initializing constructor.
This constructor creates and initializes a LocalXyWgs84Util.
[in] | reference_latitude | Reference latitude in degrees. |
[in] | reference_longitude | Reference longitude in degrees. |
[in] | reference_angle | Reference angle in degrees ENU. |
[in] | reference_altitude | Reference altitude in meters. |
Zero-argument constructor.
This constructor creates an uninitialized LocalXyWgs84Util. This constructor is only used to create placeholder objects in containers that require a zero-argument constructor.
std::string swri_transform_util::LocalXyWgs84Util::Frame | ( | ) | const [inline] |
Return the TF frame ID corresponding to the local origin.
Definition at line 168 of file local_xy_util.h.
void swri_transform_util::LocalXyWgs84Util::HandleOrigin | ( | const topic_tools::ShapeShifter::ConstPtr | origin | ) | [protected] |
void swri_transform_util::LocalXyWgs84Util::Initialize | ( | ) | [protected] |
bool swri_transform_util::LocalXyWgs84Util::Initialized | ( | ) | const [inline] |
Return whether the object has been initialized.
The object is not usable unless it has been initialized (see the two constructors).
Definition at line 135 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::ReferenceAltitude | ( | ) | const |
Return the altitude coordinate of the local origin.
double swri_transform_util::LocalXyWgs84Util::ReferenceAngle | ( | ) | const |
Return the reference angle in degrees ENU.
double swri_transform_util::LocalXyWgs84Util::ReferenceLatitude | ( | ) | const |
Return the latitude coordinate of the local origin.
double swri_transform_util::LocalXyWgs84Util::ReferenceLongitude | ( | ) | const |
Return the longitude coordinate of the local origin.
bool swri_transform_util::LocalXyWgs84Util::ToLocalXy | ( | double | latitude, |
double | longitude, | ||
double & | x, | ||
double & | y | ||
) | const |
Convert WGS84 latitude and longitude to LocalXY.
[in] | latitude | Latitude value in degrees. |
[in] | longitude | Longitude value in degrees. |
[out] | x | X coordinate in meters from origin. |
[out] | y | Y coordinate in meters from origin. |
bool swri_transform_util::LocalXyWgs84Util::ToWgs84 | ( | double | x, |
double | y, | ||
double & | latitude, | ||
double & | longitude | ||
) | const |
Convert LocalXY to WGS84 latitude and longitude.
[in] | x | X coordinate in meters from origin. |
[in] | y | Y coordinate in meters from origin. |
[out] | latitude | Latitude value in degrees. |
[out] | longitude | Longitude value in degrees. |
double swri_transform_util::LocalXyWgs84Util::cos_angle_ [protected] |
Definition at line 210 of file local_xy_util.h.
std::string swri_transform_util::LocalXyWgs84Util::frame_ [protected] |
Definition at line 213 of file local_xy_util.h.
bool swri_transform_util::LocalXyWgs84Util::initialized_ [protected] |
Definition at line 216 of file local_xy_util.h.
Definition at line 215 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::reference_altitude_ [protected] |
Definition at line 206 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::reference_angle_ [protected] |
Definition at line 205 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::reference_latitude_ [protected] |
Definition at line 203 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::reference_longitude_ [protected] |
Definition at line 204 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::rho_lat_ [protected] |
Definition at line 208 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::rho_lon_ [protected] |
Definition at line 209 of file local_xy_util.h.
double swri_transform_util::LocalXyWgs84Util::sin_angle_ [protected] |
Definition at line 211 of file local_xy_util.h.