Classes | Typedefs | Functions | Variables
swri_transform_util Namespace Reference

Classes

class  GeoReference
 
class  IdentityTransform
 Specialization of swri_transform_util::TransformImpl that represents the identity transform. More...
 
class  LocalXyWgs84Util
 Utility class for converting between WGS84 lat/lon and an ortho-rectified LocalXY coordinate system. More...
 
class  TfToUtmTransform
 Class to implement transformation from TF to UTM. More...
 
class  TfToWgs84Transform
 Specialization of TransformImpl for transforming from TF to WGS84. More...
 
class  TfTransform
 Specialization of swri_transform_util::TransformImpl that performs TF transformation. More...
 
class  Transform
 An abstraction of the tf::Transform class to support transforms in addition to the rigid transforms supported by tf. More...
 
class  Transformer
 A base class for transformers. More...
 
class  TransformImpl
 Base class for Transform implementations. More...
 
class  TransformManager
 Wrapper around tf::TransformListener to support non-TF transforms. More...
 
class  UtmToTfTransform
 Class to implement transformation from UTM to TF. More...
 
class  UtmToWgs84Transform
 Class to implement transformation from UTM to WGS84. More...
 
class  UtmTransformer
 Instantiation of Transformer to handle transforms to/from UTM. More...
 
class  UtmUtil
 Utility class for converting between latitude/longitude and UTM. More...
 
class  Wgs84ToTfTransform
 Specialization of TransformImpl for transforming from WGS84 to TF. More...
 
class  Wgs84ToUtmTransform
 Class to implement transformation from WGS84 to UTM. More...
 
class  Wgs84Transformer
 

Typedefs

typedef boost::shared_ptr< LocalXyWgs84UtilLocalXyWgs84UtilPtr
 
typedef std::map< std::string, TransformerMapSourceTargetMap
 
typedef std::map< std::string, boost::shared_ptr< Transformer > > TransformerMap
 
typedef boost::shared_ptr< TransformImplTransformImplPtr
 
typedef boost::shared_ptr< TransformManagerTransformManagerPtr
 

Functions

bool FrameIdsEqual (const std::string &frame1, const std::string &frame2)
 Compare two TF frame IDs, assuming that frames with relative paths are in the global namespace. More...
 
tf::Matrix3x3 Get3x3Cov (const boost::array< double, 9 > &matrix)
 Converts the 3x3 covariance matrices from Imu messages to a Matrix3x3. More...
 
char GetBand (double latitude)
 Given a latitude angle, get the UTM band letter. More...
 
double GetBearing (double source_latitude, double source_longitude, double destination_latitude, double destination_longitude)
 Calculates the bearing between two points. More...
 
double GetHeading (double src_x, double src_y, double dst_x, double dst_y)
 Calculates the heading in degrees from a source and destination point in a north-oriented (+y = north, +x = east), ortho-rectified coordinate system. More...
 
tf::Matrix3x3 GetLowerRight (const boost::array< double, 36 > &matrix)
 Gets the lower-right 3x3 sub-matrix of a 6x6 matrix. More...
 
void GetMidpointLatLon (double latitude1, double longitude1, double latitude2, double longitude2, double &mid_latitude, double &mid_longitude)
 Find the midpoint on the arc between two lat/lon points. More...
 
tf::Vector3 GetPrimaryAxis (const tf::Vector3 &vector)
 Return an axis aligned unit vector that is nearest the provided vector. More...
 
tf::Transform GetRelativeTransform (double latitude, double longitude, double yaw, double reference_latitude, double reference_longitude, double reference_yaw)
 
tf::Matrix3x3 GetUpperLeft (const boost::array< double, 36 > &matrix)
 Gets the upper-left 3x3 sub-matrix of a 6x6 matrix. More...
 
uint32_t GetZone (double longitude)
 Given a longitude angle, get the UTM zone. More...
 
double GreatCircleDistance (double src_latitude, double src_longitude, double dst_latitude, double dst_longitude)
 Calculates the great circle distance between two points. More...
 
double GreatCircleDistance (const tf::Vector3 &src, const tf::Vector3 &dst)
 Calculates the great circle distance between two points. More...
 
bool IsRotation (tf::Matrix3x3 matrix)
 Validate that a 3x3 matrix is a rotation. More...
 
double LatitudeDegreesFromMeters (double altitude, double arc_length)
 Calculate the subtending angle of an arc (aligned with the latitudinal coordinate axis at the specified altitude) with the specied arc length. More...
 
void LocalXyFromWgs84 (double latitude, double longitude, double reference_latitude, double reference_longitude, double &x, double &y)
 Transform a point from WGS84 lat/lon to an ortho-rectified LocalXY coordinate system. More...
 
double LongitudeDegreesFromMeters (double latitude, double altitude, double arc_length)
 Calculate the subtending angle of an arc (aligned with the longitudinal coordinate axis at the specified latitude and altitude) with the specied arc length. More...
 
std::string NormalizeFrameId (const std::string &frame_id)
 Normalize a TF frame ID by assuming that frames with relative paths are in the global namespace and adding a leading slash. More...
 
void Set3x3Cov (const tf::Matrix3x3 &matrix_in, boost::array< double, 9 > &matrix_out)
 Converts the Matrix3x3 matrix into a 9 element covariance matrix from Imu messages. More...
 
void SetLowerRight (const tf::Matrix3x3 &sub_matrix, boost::array< double, 36 > &matrix)
 Sets the lower-right quadrant of a 6x6 matrix with the specified 3x3 sub-matrix. More...
 
void SetUpperLeft (const tf::Matrix3x3 &sub_matrix, boost::array< double, 36 > &matrix)
 Sets the upper-left quadrant of a 6x6 matrix with the specified 3x3 sub-matrix. More...
 
tf::Quaternion SnapToRightAngle (const tf::Quaternion &rotation)
 Snaps a quaternion rotation to the closest right angle rotation. More...
 
double ToHeading (double yaw)
 Convert yaw to heading, where yaw is in radians, counter-clockwise, with 0 on the positive x-axis and heading is in degrees, clockwise, with 0 on the positive y-axis. More...
 
double ToYaw (double heading)
 Convert heading to yaw, where yaw is in radians, counter-clockwise, with 0 on the positive x-axis and heading is in degrees, clockwise, with 0 on the positive y-axis. More...
 
void Wgs84FromLocalXy (double x, double y, double reference_latitude, double reference_longitude, double &latitude, double &longitude)
 Transform a point from an ortho-rectified LocalXY coordinate system into WGS84 latitude and longitude. More...
 

Variables

static const double _earth_eccentricity = 0.08181919084261
 Earth 'first' eccentricity according to WGS84. More...
 
static const double _earth_equator_circumference = 40075016.69
 Earth equatorial circumference in meters according to WGS84. More...
 
static const double _earth_equator_radius = 6378137.0
 Earth equatorial radius in meters according to WGS84. More...
 
static const double _earth_flattening = 3.35281066475e-3
 Earth flattening according to WGS84. More...
 
static const double _earth_mean_radius = 6371009.0
 Earth mean radius in meters. More...
 
static const double _earth_rotation_rate = 7.292115e-5
 Earth rotation rate in radians per second. More...
 
static const std::string _local_xy_frame = "/local_xy"
 Special frame id for data defined a LocalXY coordinate system. More...
 
static const std::string _tf_frame = "/tf"
 Special frame id used internally to denote frames that are part of the ROS TF tree. More...
 
static const std::string _utm_frame = "/utm"
 Special frame id for data defined in the UTM coordinate system. More...
 
static const std::string _wgs84_frame = "/wgs84"
 Special frame id for data defined in the WGS84 lat/lon coordinate system. More...
 

Typedef Documentation

Definition at line 236 of file local_xy_util.h.

typedef std::map<std::string, TransformerMap> swri_transform_util::SourceTargetMap

Definition at line 49 of file transform_manager.h.

Definition at line 48 of file transform_manager.h.

Definition at line 95 of file transform.h.

Definition at line 255 of file transform_manager.h.

Function Documentation

bool swri_transform_util::FrameIdsEqual ( const std::string &  frame1,
const std::string &  frame2 
)

Compare two TF frame IDs, assuming that frames with relative paths are in the global namespace.

E.g. "map" == "/map" "map" == "map" "//map" != "/map" "/ns/map" == "ns/map" "/ns/map" != "map" "/ns/map" != "/map"

Parameters
[in]frame1A TF frame ID, with or without a leading slash
tf::Matrix3x3 swri_transform_util::Get3x3Cov ( const boost::array< double, 9 > &  matrix)

Converts the 3x3 covariance matrices from Imu messages to a Matrix3x3.

Parameters
[in]matrixThe input matrix
Return values
Returnsthe input matrix as a Matrix3x3 object
char swri_transform_util::GetBand ( double  latitude)

Given a latitude angle, get the UTM band letter.

Parameters
latitudeLatitude angle in degrees
Returns
UTM band letter
double swri_transform_util::GetBearing ( double  source_latitude,
double  source_longitude,
double  destination_latitude,
double  destination_longitude 
)

Calculates the bearing between two points.

Parameters
[in]source_latitudeThe latitude of the origin point in degrees
[in]source_longitudeThe longitude of the origin point in degrees
[in]destination_latitudeThe latitude of the destination point in degrees
[in]destination_longitudeThe longitude of the destination point in degrees
Returns
The bearing between the origin and destination in degrees ENU
double swri_transform_util::GetHeading ( double  src_x,
double  src_y,
double  dst_x,
double  dst_y 
)

Calculates the heading in degrees from a source and destination point in a north-oriented (+y = north, +x = east), ortho-rectified coordinate system.

The heading is positive clock-wise with 0 degrees at north.

Parameters
[in]src_xThe source X coordinate.
[in]src_yThe source Y coordinate.
[in]dst_xThe destination X coordinate.
[in]dst_yThe destination Y coordinate.
Returns
The heading in degrees along the vector between the source and destination points.
tf::Matrix3x3 swri_transform_util::GetLowerRight ( const boost::array< double, 36 > &  matrix)

Gets the lower-right 3x3 sub-matrix of a 6x6 matrix.

Parameters
[in]matrixThe 6x6 matrix.
Returns
The lower-right 3x3 sub-matrix.
void swri_transform_util::GetMidpointLatLon ( double  latitude1,
double  longitude1,
double  latitude2,
double  longitude2,
double &  mid_latitude,
double &  mid_longitude 
)

Find the midpoint on the arc between two lat/lon points.

Parameters
[in]latitude1Endpoint 1 latitude in degrees
[in]longitude1Endpoint 1 longitude in degrees
[in]latitude2Endpoint 2 latitude in degrees
[in]longitude2Endpoint 2 longitude in degrees
[out]mid_latitudeMidpoint latitude in degrees
[out]mid_longitudeMidpoint longitude in degrees
tf::Vector3 swri_transform_util::GetPrimaryAxis ( const tf::Vector3 vector)

Return an axis aligned unit vector that is nearest the provided vector.

Parameters
[in]vectorThe input vector.
Returns
The axis aligned unit vector.
tf::Transform swri_transform_util::GetRelativeTransform ( double  latitude,
double  longitude,
double  yaw,
double  reference_latitude,
double  reference_longitude,
double  reference_yaw 
)
tf::Matrix3x3 swri_transform_util::GetUpperLeft ( const boost::array< double, 36 > &  matrix)

Gets the upper-left 3x3 sub-matrix of a 6x6 matrix.

Parameters
[in]matrixThe 6x6 matrix.
Returns
The upper-left 3x3 sub-matrix.
uint32_t swri_transform_util::GetZone ( double  longitude)

Given a longitude angle, get the UTM zone.

Parameters
longitudeLongitude angle in degrees
Returns
UTM zone number
double swri_transform_util::GreatCircleDistance ( double  src_latitude,
double  src_longitude,
double  dst_latitude,
double  dst_longitude 
)

Calculates the great circle distance between two points.

Parameters
[in]src_latitudeSource latitude
[in]src_longitudeSource longitude
[in]dst_latitudeDestination latitude
[in]dst_longitudeDestination longitude
Returns
Distance in meters.
double swri_transform_util::GreatCircleDistance ( const tf::Vector3 src,
const tf::Vector3 dst 
)

Calculates the great circle distance between two points.

Parameters
[in]srcSource(x = longitude, y = latitude, z ignored)
[in]dstDestination(x = longitude, y = latitude, z ignored)
Returns
Distance in meters.
bool swri_transform_util::IsRotation ( tf::Matrix3x3  matrix)

Validate that a 3x3 matrix is a rotation.

Parameters
[in]matrixThe matrix to validate.
Returns
True if the matrix is a valid rotation.
double swri_transform_util::LatitudeDegreesFromMeters ( double  altitude,
double  arc_length 
)

Calculate the subtending angle of an arc (aligned with the latitudinal coordinate axis at the specified altitude) with the specied arc length.

This conversion is invariant to longitude.

Parameters
[in]altitudeThe radius of the arc.
[in]arc_lengthThe arc length in meters.
Returns
The angle subtended by the arc (in degrees)
void swri_transform_util::LocalXyFromWgs84 ( double  latitude,
double  longitude,
double  reference_latitude,
double  reference_longitude,
double &  x,
double &  y 
)

Transform a point from WGS84 lat/lon to an ortho-rectified LocalXY coordinate system.

Parameters
[in]latitudeThe input latitude in degrees.
[in]longitudeThe input latitude in degrees.
[in]reference_latitudeThe reference WGS84 latitude in degrees.
[in]reference_longitudeThe reference WGS84 longitude in degrees.
[out]xThe output X coordinate in meters.
[out]yThe output Y coordinate in meters.
double swri_transform_util::LongitudeDegreesFromMeters ( double  latitude,
double  altitude,
double  arc_length 
)

Calculate the subtending angle of an arc (aligned with the longitudinal coordinate axis at the specified latitude and altitude) with the specied arc length.

Parameters
[in]latitudeThe latitude of the arc.
[in]altitudeThe radius of the arc.
[in]arc_lengthThe arc length in meters.
Returns
The angle subtended by the arc (in degrees)
std::string swri_transform_util::NormalizeFrameId ( const std::string &  frame_id)

Normalize a TF frame ID by assuming that frames with relative paths are in the global namespace and adding a leading slash.

Parameters
[in]frame_idA TF frame ID, with or without a leading slash
Returns
frame_id with a leading slash if it did not have one previously
void swri_transform_util::Set3x3Cov ( const tf::Matrix3x3 matrix_in,
boost::array< double, 9 > &  matrix_out 
)

Converts the Matrix3x3 matrix into a 9 element covariance matrix from Imu messages.

Parameters
[in]matrix_inThe input matrix
[out]matrix_outThe output matrix
void swri_transform_util::SetLowerRight ( const tf::Matrix3x3 sub_matrix,
boost::array< double, 36 > &  matrix 
)

Sets the lower-right quadrant of a 6x6 matrix with the specified 3x3 sub-matrix.

Parameters
[in]sub_matrixThe 3x3 sub-matrix.
[out]matrixThe 6x6 matrix to modify.
void swri_transform_util::SetUpperLeft ( const tf::Matrix3x3 sub_matrix,
boost::array< double, 36 > &  matrix 
)

Sets the upper-left quadrant of a 6x6 matrix with the specified 3x3 sub-matrix.

Parameters
[in]sub_matrixThe 3x3 sub-matrix.
[out]matrixThe 6x6 matrix to modify.
tf::Quaternion swri_transform_util::SnapToRightAngle ( const tf::Quaternion rotation)

Snaps a quaternion rotation to the closest right angle rotation.

Parameters
[in]rotationThe input quaternion rotation.
Returns
The closest right angle rotation to the input rotation.
double swri_transform_util::ToHeading ( double  yaw)

Convert yaw to heading, where yaw is in radians, counter-clockwise, with 0 on the positive x-axis and heading is in degrees, clockwise, with 0 on the positive y-axis.

Parameters
[in]yawThe yaw in radians.
Returns
The heading in degrees.
double swri_transform_util::ToYaw ( double  heading)

Convert heading to yaw, where yaw is in radians, counter-clockwise, with 0 on the positive x-axis and heading is in degrees, clockwise, with 0 on the positive y-axis.

Parameters
[in]headingThe heading in degrees.
Returns
The yaw in radians.
void swri_transform_util::Wgs84FromLocalXy ( double  x,
double  y,
double  reference_latitude,
double  reference_longitude,
double &  latitude,
double &  longitude 
)

Transform a point from an ortho-rectified LocalXY coordinate system into WGS84 latitude and longitude.

Assumes the LocalXY data was generated with respect to the WGS84 datum.

Parameters
[in]xThe input X coordinate in meters.
[in]yThe input Y coordinate in meters.
[in]reference_latitudeThe reference WGS84 latitude in degrees.
[in]reference_longitudeThe reference WGS84 longitude in degrees.
[out]latitudeThe output latitude in degrees.
[out]longitudeThe output latitude in degrees.

Variable Documentation

const double swri_transform_util::_earth_eccentricity = 0.08181919084261
static

Earth 'first' eccentricity according to WGS84.

Definition at line 53 of file earth_constants.h.

const double swri_transform_util::_earth_equator_circumference = 40075016.69
static

Earth equatorial circumference in meters according to WGS84.

Definition at line 48 of file earth_constants.h.

const double swri_transform_util::_earth_equator_radius = 6378137.0
static

Earth equatorial radius in meters according to WGS84.

Definition at line 38 of file earth_constants.h.

const double swri_transform_util::_earth_flattening = 3.35281066475e-3
static

Earth flattening according to WGS84.

Flattening is a measure of the compression of a sphere along a diameter to form an ellipsoid of revolution.

See: http://en.wikipedia.org/wiki/Flattening

Definition at line 63 of file earth_constants.h.

const double swri_transform_util::_earth_mean_radius = 6371009.0
static

Earth mean radius in meters.

Definition at line 43 of file earth_constants.h.

const double swri_transform_util::_earth_rotation_rate = 7.292115e-5
static

Earth rotation rate in radians per second.

Definition at line 68 of file earth_constants.h.

const std::string swri_transform_util::_local_xy_frame = "/local_xy"
static

Special frame id for data defined a LocalXY coordinate system.

Dependent on the LocalXY origin of the system.

Definition at line 55 of file frames.h.

const std::string swri_transform_util::_tf_frame = "/tf"
static

Special frame id used internally to denote frames that are part of the ROS TF tree.

Definition at line 60 of file frames.h.

const std::string swri_transform_util::_utm_frame = "/utm"
static

Special frame id for data defined in the UTM coordinate system.

The zone is assumed to be the same as the LocalXY origin of the system. Because of this zone transitions are not supported.

Definition at line 48 of file frames.h.

const std::string swri_transform_util::_wgs84_frame = "/wgs84"
static

Special frame id for data defined in the WGS84 lat/lon coordinate system.

Definition at line 40 of file frames.h.



swri_transform_util
Author(s): Marc Alban
autogenerated on Tue Apr 6 2021 02:50:46