Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef TRANSFORM_UTIL_TRANSFORM_UTIL_H_
00031 #define TRANSFORM_UTIL_TRANSFORM_UTIL_H_
00032
00033 #include <string>
00034 #include <boost/array.hpp>
00035
00036 #include <tf/transform_datatypes.h>
00037
00038 namespace swri_transform_util
00039 {
00040 tf::Transform GetRelativeTransform(
00041 double latitude,
00042 double longitude,
00043 double yaw,
00044 double reference_latitude,
00045 double reference_longitude,
00046 double reference_yaw);
00047
00058 double GreatCircleDistance(
00059 double src_latitude,
00060 double src_longitude,
00061 double dst_latitude,
00062 double dst_longitude);
00063
00072 double GreatCircleDistance(const tf::Vector3& src, const tf::Vector3& dst);
00073
00083 double GetBearing(
00084 double source_latitude,
00085 double source_longitude,
00086 double destination_latitude,
00087 double destination_longitude);
00088
00099 void GetMidpointLatLon(
00100 double latitude1,
00101 double longitude1,
00102 double latitude2,
00103 double longitude2,
00104 double& mid_latitude,
00105 double& mid_longitude);
00106
00120 double GetHeading(double src_x, double src_y, double dst_x, double dst_y);
00121
00131 double ToHeading(double yaw);
00132
00142 double ToYaw(double heading);
00143
00151 tf::Quaternion SnapToRightAngle(const tf::Quaternion& rotation);
00152
00160 tf::Vector3 GetPrimaryAxis(const tf::Vector3& vector);
00161
00169 bool IsRotation(tf::Matrix3x3 matrix);
00170
00178 tf::Matrix3x3 GetUpperLeft(const boost::array<double, 36>& matrix);
00179
00187 tf::Matrix3x3 GetLowerRight(const boost::array<double, 36>& matrix);
00188
00196 tf::Matrix3x3 Get3x3Cov(const boost::array<double, 9>& matrix);
00197
00206 void Set3x3Cov(const tf::Matrix3x3& matrix_in,
00207 boost::array<double, 9>& matrix_out);
00215 void SetUpperLeft(
00216 const tf::Matrix3x3& sub_matrix,
00217 boost::array<double, 36>& matrix);
00218
00226 void SetLowerRight(
00227 const tf::Matrix3x3& sub_matrix,
00228 boost::array<double, 36>& matrix);
00229
00240 double LongitudeDegreesFromMeters(
00241 double latitude,
00242 double altitude,
00243 double arc_length);
00244
00254 double LatitudeDegreesFromMeters(
00255 double altitude,
00256 double arc_length);
00257
00266 std::string NormalizeFrameId(const std::string& frame_id);
00267
00282 bool FrameIdsEqual(const std::string& frame1, const std::string& frame2);
00283 }
00284
00285 #endif // TRANSFORM_UTIL_TRANSFORM_UTIL_H_
00286