|
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. More...
|
|
tf::Matrix3x3 | swri_transform_util::Get3x3Cov (const boost::array< double, 9 > &matrix) |
| Converts the 3x3 covariance matrices from Imu messages to a Matrix3x3. More...
|
|
double | swri_transform_util::GetBearing (double source_latitude, double source_longitude, double destination_latitude, double destination_longitude) |
| Calculates the bearing between two points. More...
|
|
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. More...
|
|
tf::Matrix3x3 | swri_transform_util::GetLowerRight (const boost::array< double, 36 > &matrix) |
| Gets the lower-right 3x3 sub-matrix of a 6x6 matrix. More...
|
|
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. More...
|
|
tf::Vector3 | swri_transform_util::GetPrimaryAxis (const tf::Vector3 &vector) |
| Return an axis aligned unit vector that is nearest the provided vector. More...
|
|
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. More...
|
|
double | swri_transform_util::GreatCircleDistance (const tf::Vector3 &src, const tf::Vector3 &dst) |
| Calculates the great circle distance between two points. More...
|
|
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. More...
|
|
bool | swri_transform_util::IsRotation (tf::Matrix3x3 matrix) |
| Validate that a 3x3 matrix is a rotation. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
tf::Quaternion | swri_transform_util::SnapToRightAngle (const tf::Quaternion &rotation) |
| Snaps a quaternion rotation to the closest right angle rotation. More...
|
|
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. More...
|
|
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. More...
|
|