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 <boost/array.hpp>
00034
00035 #include <tf/transform_datatypes.h>
00036
00037 #include <swri_transform_util/local_xy_util.h>
00038
00039 namespace swri_transform_util
00040 {
00041 tf::Transform GetRelativeTransform(
00042 double latitude,
00043 double longitude,
00044 double yaw,
00045 double reference_latitude,
00046 double reference_longitude,
00047 double reference_yaw);
00048
00059 double GreatCircleDistance(
00060 double src_latitude,
00061 double src_longitude,
00062 double dst_latitude,
00063 double dst_longitude);
00064
00073 double GreatCircleDistance(const tf::Vector3& src, const tf::Vector3& dst);
00074
00084 double GetBearing(
00085 double source_latitude,
00086 double source_longitude,
00087 double destination_latitude,
00088 double destination_longitude);
00089
00100 void GetMidpointLatLon(
00101 double latitude1,
00102 double longitude1,
00103 double latitude2,
00104 double longitude2,
00105 double& mid_latitude,
00106 double& mid_longitude);
00107
00121 double GetHeading(double src_x, double src_y, double dst_x, double dst_y);
00122
00132 double ToHeading(double yaw);
00133
00143 double ToYaw(double heading);
00144
00152 tf::Quaternion SnapToRightAngle(const tf::Quaternion& rotation);
00153
00161 tf::Vector3 GetPrimaryAxis(const tf::Vector3& vector);
00162
00170 bool IsRotation(tf::Matrix3x3 matrix);
00171
00179 tf::Matrix3x3 GetUpperLeft(const boost::array<double, 36>& matrix);
00180
00188 tf::Matrix3x3 GetLowerRight(const boost::array<double, 36>& matrix);
00189
00197 tf::Matrix3x3 Get3x3Cov(const boost::array<double, 9>& matrix);
00198
00207 void Set3x3Cov(const tf::Matrix3x3& matrix_in,
00208 boost::array<double, 9>& matrix_out);
00216 void SetUpperLeft(
00217 const tf::Matrix3x3& sub_matrix,
00218 boost::array<double, 36>& matrix);
00219
00227 void SetLowerRight(
00228 const tf::Matrix3x3& sub_matrix,
00229 boost::array<double, 36>& matrix);
00230
00241 double LongitudeDegreesFromMeters(
00242 double latitude,
00243 double altitude,
00244 double arc_length);
00245
00255 double LatitudeDegreesFromMeters(
00256 double altitude,
00257 double arc_length);
00258 }
00259
00260 #endif // TRANSFORM_UTIL_TRANSFORM_UTIL_H_
00261