grvc_ual  2.2
An abstraction layer for unmanned aerial vehicles
geographic_to_cartesian.h
1 
10 #ifndef GEOGRAPHIC_TO_CARTESIAN
11 #define GEOGRAPHIC_TO_CARTESIAN
12 
13 #include <math.h>
14 #include <algorithm>
15 #include <stdlib.h>
16 #include <geometry_msgs/Point32.h>
17 #include <geographic_msgs/GeoPoint.h>
18 #include <geodesy/utm.h> // IMPORTANT TO INSTALL GEODESY: sudo apt-get install ros-kinetic-geographic-info
19 
20 inline geometry_msgs::Point32 geographic_to_cartesian (geographic_msgs::GeoPoint actual_coordinate_geo, geographic_msgs::GeoPoint origin_geo)
21 {
22  geodesy::UTMPoint actual_coordinate_UTM(actual_coordinate_geo); // Conversion from geographic coordinates to UTM.
23  geodesy::UTMPoint origin_UTM(origin_geo); // Conversion from geographic coordinates to UTM.
24 
25  geometry_msgs::Point32 actual_coordinate_cartesian; // Cartesian coordinate that this function will return.
26 
27  // The problem with UTM is that if there are coordinates in more than one zone, it's difficult to merge the coordinates of the different zones.
28  // This is because each zone has 6 degrees of longitude, and the width in meters of the zone is variable from equator to the poles.
29  // Something similar when the coordinates are in different hemispheres, separated by the equator. Each hemisphere has a different origin for the y axis.
30  // These are the reasons why the x and y assignation for the Cartesian conversion isn't a simple UTM substraction.
31 
32  // Assignating "actual_coordinate_cartesian.x" is tricky when the actual coordinate and the origin of the Cartesian coordinates are on different zones.
33  if( int(actual_coordinate_UTM.zone)==60 && origin_UTM.zone==1 ) { // Coordinate and origin separated by the +-180º longitude
34  geographic_msgs::GeoPoint geo_180_w;
35  geographic_msgs::GeoPoint geo_180_e;
36  geo_180_w.longitude = 179.9999999;
37  geo_180_w.latitude = actual_coordinate_geo.latitude;
38  geo_180_w.altitude = actual_coordinate_geo.altitude;
39  geo_180_e.longitude = -179.9999999;
40  geo_180_e.latitude = actual_coordinate_geo.latitude;
41  geo_180_e.altitude = actual_coordinate_geo.altitude;
42  geodesy::UTMPoint utm_180_w(geo_180_w);
43  geodesy::UTMPoint utm_180_e(geo_180_e);
44  actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_180_w.easting + utm_180_e.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
45  } else if( origin_UTM.zone==60 && int(actual_coordinate_UTM.zone)==1 ) { // Coordinate and origin separated by the +-180º longitude
46  geographic_msgs::GeoPoint geo_180_w;
47  geographic_msgs::GeoPoint geo_180_e;
48  geo_180_w.longitude = 179.9999999;
49  geo_180_w.latitude = actual_coordinate_geo.latitude;
50  geo_180_w.altitude = actual_coordinate_geo.altitude;
51  geo_180_e.longitude = -179.9999999;
52  geo_180_e.latitude = actual_coordinate_geo.latitude;
53  geo_180_e.altitude = actual_coordinate_geo.altitude;
54  geodesy::UTMPoint utm_180_w(geo_180_w);
55  geodesy::UTMPoint utm_180_e(geo_180_e);
56  actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_180_e.easting + utm_180_w.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
57  } else if( int(actual_coordinate_UTM.zone) < origin_UTM.zone ) {
58  int quotient_from_int_division = (int) ( std::max(std::abs(actual_coordinate_geo.longitude),std::abs(origin_geo.longitude))/6 ); // int division of the max longitude (absolute, without sign)
59  double border_longitude = quotient_from_int_division * 6.0 *pow(-1,std::signbit(std::max(actual_coordinate_geo.longitude,origin_geo.longitude))); // border_longitude = quotient_from_int_division * 6 *(-1)^(1 if negative longitude, 0 if positive)
60  geographic_msgs::GeoPoint geo_w;
61  geographic_msgs::GeoPoint geo_e;
62  geo_w.longitude = border_longitude - 0.0000001;
63  geo_w.latitude = actual_coordinate_geo.latitude;
64  geo_w.altitude = actual_coordinate_geo.altitude;
65  geo_e.longitude = border_longitude + 0.0000001;
66  geo_e.latitude = actual_coordinate_geo.latitude;
67  geo_e.altitude = actual_coordinate_geo.altitude;
68  geodesy::UTMPoint utm_w(geo_w);
69  geodesy::UTMPoint utm_e(geo_e);
70  actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_w.easting + utm_e.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
71  } else if( origin_UTM.zone < int(actual_coordinate_UTM.zone) ) {
72  int quotient_from_int_division = (int) ( std::max(std::abs(actual_coordinate_geo.longitude),std::abs(origin_geo.longitude))/6 ); // int division of the max longitude (absolute, without sign)
73  double border_longitude = quotient_from_int_division * 6.0 *pow(-1,std::signbit(std::max(actual_coordinate_geo.longitude,origin_geo.longitude))); // border_longitude = quotient_from_int_division * 6 *(-1)^(1 if negative longitude, 0 if positive)
74  geographic_msgs::GeoPoint geo_w;
75  geographic_msgs::GeoPoint geo_e;
76  geo_w.longitude = border_longitude - 0.0000001;
77  geo_w.latitude = actual_coordinate_geo.latitude;
78  geo_w.altitude = actual_coordinate_geo.altitude;
79  geo_e.longitude = border_longitude + 0.0000001;
80  geo_e.latitude = actual_coordinate_geo.latitude;
81  geo_e.altitude = actual_coordinate_geo.altitude;
82  geodesy::UTMPoint utm_w(geo_w);
83  geodesy::UTMPoint utm_e(geo_e);
84  actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_e.easting + utm_w.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
85  }
86  else
87  {
88  // The actual coordinate is in the same zone that the origin (the first station). This is the normal situation, the assigntation of the x axis value is trivial (simple subtraction).
89  actual_coordinate_cartesian.x = actual_coordinate_UTM.easting -origin_UTM.easting;
90  }
91 
92  // Assignating "actual_coordinate_cartesian.y" is also tricky when the actual coordinate and the origin of the Cartesian coordinates are on different hemispheres:
93  if( origin_UTM.band=='N' && actual_coordinate_UTM.band=='M' ) { // Coordinate and origin separated by the 0º latitude
94  geographic_msgs::GeoPoint geo_0_n;
95  geographic_msgs::GeoPoint geo_0_s;
96  geo_0_n.longitude = actual_coordinate_geo.longitude;
97  geo_0_n.latitude = 0.0000001;
98  geo_0_n.altitude = actual_coordinate_geo.altitude;
99  geo_0_s.longitude = actual_coordinate_geo.longitude;
100  geo_0_s.latitude = -0.0000001;
101  geo_0_s.altitude = actual_coordinate_geo.altitude;
102  geodesy::UTMPoint utm_0_n(geo_0_n);
103  geodesy::UTMPoint utm_0_s(geo_0_s);
104  actual_coordinate_cartesian.y = actual_coordinate_UTM.northing - utm_0_s.northing + utm_0_n.northing - origin_UTM.northing; // Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_s.northing + utm_0_n.northing " computes the y step in both sides of the equator.
105  } else if( actual_coordinate_UTM.band=='N' && origin_UTM.band=='M' ) { // Coordinate and origin separated by the 0º latitude
106  geographic_msgs::GeoPoint geo_0_n;
107  geographic_msgs::GeoPoint geo_0_s;
108  geo_0_n.longitude = actual_coordinate_geo.longitude;
109  geo_0_n.latitude = 0.0000001;
110  geo_0_n.altitude = actual_coordinate_geo.altitude;
111  geo_0_s.longitude = actual_coordinate_geo.longitude;
112  geo_0_s.latitude = -0.0000001;
113  geo_0_s.altitude = actual_coordinate_geo.altitude;
114  geodesy::UTMPoint utm_0_n(geo_0_n);
115  geodesy::UTMPoint utm_0_s(geo_0_s);
116  actual_coordinate_cartesian.y = actual_coordinate_UTM.northing - utm_0_n.northing + utm_0_s.northing - origin_UTM.northing; // Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_n.northing + utm_0_s.northing " computes the y step in both sides of the equator.
117  }
118  else
119  {
120  // The actual coordinate is in the same hemisphere that the origin (the first station). This is the normal situation, the assigntation of the y axis value is trivial (simple subtraction).
121  actual_coordinate_cartesian.y = actual_coordinate_UTM.northing-origin_UTM.northing;
122  }
123 
124  // Assignating "actual_coordinate_cartesian.z" is trivial always, simple subtraction.
125  actual_coordinate_cartesian.z = actual_coordinate_UTM.altitude-origin_UTM.altitude;
126 
127  return actual_coordinate_cartesian;
128 } // end inline function
129 
130 #endif
Generated by   doxygen 1.8.11