Skip to content

Commit bcef780

Browse files
aaronj0aaronj0
committed
Eigen helper functions + Code cleanup
1 parent bb450c3 commit bcef780

File tree

2 files changed

+154
-155
lines changed

2 files changed

+154
-155
lines changed

‎include/transforms/coord_types.h

+1-3
Original file line numberDiff line numberDiff line change
@@ -20,8 +20,6 @@ enum waypt_enum {
2020
airdrop_loc = 2,
2121
cvrg_start = 3,
2222
cvrg_end = 4,
23-
// map_start = 5,
24-
// map_end = 6
2523
};
2624

27-
#endif //LAZY_THETA_STAR_3D_INCLUDE_LAZY_THETA_STAR_3D_COORD_TYPES_H
25+
#endif

‎src/enu_transforms/enu_transforms.cpp

+153-152
Original file line numberDiff line numberDiff line change
@@ -2,14 +2,165 @@
22
// Created by aaronjomyjoseph on 5/11/21.
33
//
44

5-
// TODO : Add support to get ENU frame values for emergentLastKnownPos
65
#include <iostream>
76
#include <vector>
87
#include "enu_transforms/enu_transforms.h"
98
#include <grid_map_ros/grid_map_ros.hpp>
109

1110
namespace enu_transforms {
1211

12+
13+
void ENUtransforms::setParams(int offset_set, double resolution_set, int multiplier_set) {
14+
offset_ = offset_set;
15+
resolution_ = resolution_set;
16+
multiplier_ = multiplier_set;
17+
}
18+
19+
double ENUtransforms::max(const double &a, const double &b) {
20+
return a > b ? a : b;
21+
}
22+
23+
double ENUtransforms::min(const double &a, const double &b) {
24+
return a < b ? a : b;
25+
}
26+
27+
grid_map::Position ENUtransforms::GPStoGridMap(const double &latitude, const double &longitude) {
28+
double e, n, u;
29+
double altitude = 0;
30+
ENU_geodetic_obj_.geodetic2Enu(latitude, longitude, altitude, &e, &n, &u);
31+
grid_map::Position gmap = ENUtoMap(e, n);
32+
return gmap;
33+
}
34+
35+
grid_map::Position ENUtransforms::ENUtoMap(const double &east, const double &north) {
36+
return grid_map::Position{(east - min_east_) * multiplier_ - range_east_ / 2,
37+
(north - min_north_) * multiplier_ - range_north_ / 2};
38+
}
39+
40+
coordsW ENUtransforms::posToENU(const double &x, const double &y, const double &z) {
41+
return {(x + range_east_ / 2) / multiplier_ + min_east_, (y + range_north_ / 2) / multiplier_ + min_north_,
42+
(z)};
43+
}
44+
45+
coordsW ENUtransforms::ENUtoGPS(double e, double n, double u) {
46+
double lat, longitude, altitude;
47+
ENU_geodetic_obj_.enu2Geodetic(e, n, u, &lat, &longitude, &altitude);
48+
return {lat, longitude, altitude};
49+
}
50+
51+
52+
Eigen::Vector3d ENUtransforms::enuReferenceVector(const Eigen::Vector3d &enu) {
53+
Eigen::Vector3d delta = enu_ref_origin - enu;
54+
Eigen::Vector3d ref_vector = delta.normalized();
55+
return ref_vector;
56+
}
57+
58+
59+
Eigen::Vector2d ENUtransforms::gpsReferenceVector(const Eigen::Vector2d &gps) {
60+
Eigen::Vector2d delta = gps_origin - gps;
61+
Eigen::Vector2d ref_vector = delta.normalized();
62+
return ref_vector;
63+
}
64+
65+
double ENUtransforms::haversine(Eigen::Vector2d point1, Eigen::Vector2d point2) {
66+
double haversine;
67+
double temp;
68+
double dist_km;
69+
double lat1 = point1.x() * DEG_TO_RAD;
70+
double lon1 = point1.y() * DEG_TO_RAD;
71+
double lat2 = point2.x() * DEG_TO_RAD;
72+
double lon2 = point2.y() * DEG_TO_RAD;
73+
haversine = (pow(sin((1.0 / 2) * (lat2 - lat1)), 2)) +
74+
((cos(lat1)) * (cos(lat2)) * (pow(sin((1.0 / 2) * (lon2 - lon1)), 2)));
75+
temp = 2 * asin(min(1.0, sqrt(haversine)));
76+
dist_km = EARTH_RADIUS * temp;
77+
double dist_ft = dist_km * 3280.84;
78+
return dist_ft;
79+
}
80+
81+
double ENUtransforms::getBearing(Eigen::Vector2d point1, Eigen::Vector2d point2) {
82+
double lat1 = point2.x() * DEG_TO_RAD;
83+
double lon1 = point2.y() * DEG_TO_RAD;
84+
double lat2 = point1.x() * DEG_TO_RAD;
85+
double lon2 = point1.y() * DEG_TO_RAD;
86+
87+
double dLon = (lon2 - lon1);
88+
double y = sin(dLon) * cos(lat2);
89+
double x = cos(lat1) * sin(lat2) - sin(lat1)
90+
* cos(lat2) * cos(dLon);
91+
double brng = atan2(y, x);
92+
93+
brng = brng * RAD_TO_DEG;
94+
brng = ((int) brng + 360) % 360;
95+
return brng;
96+
97+
return brng;
98+
}
99+
100+
Eigen::Vector2d ENUtransforms::inverseHaversine(Eigen::Vector2d gps, double bearing, double dist_ft) {
101+
double lat1 = gps.x() * DEG_TO_RAD;
102+
double lon1 = gps.y() * DEG_TO_RAD;
103+
104+
double dist_km = dist_ft / 3280.84;
105+
double dist_angular = dist_km / EARTH_RADIUS;
106+
double theta = bearing * DEG_TO_RAD;
107+
double lat2 = asin(sin(lat1) * cos(dist_angular) + cos(lat1) * sin(dist_angular) * cos(theta));
108+
double lon2 =
109+
lon1 + atan2(sin(theta) * sin(dist_angular) * cos(lat2), cos(dist_angular) - sin(lat2) * sin(lat2));
110+
lat2 = lat2 * RAD_TO_DEG;
111+
lon2 = lon2 * RAD_TO_DEG;
112+
Eigen::Vector2d gps_obst_point(lat2, lon2);
113+
return gps_obst_point;
114+
}
115+
116+
std::vector<std::vector<double>>
117+
ENUtransforms::genPoly(Eigen::Vector2d rad_point, Eigen::Vector2d centre_point, int n) {
118+
std::vector<std::vector<double>> poly;
119+
double center_lat = centre_point.x();
120+
double center_long = centre_point.y();
121+
double rad_lat = rad_point.x();
122+
double rad_long = rad_point.y();
123+
124+
double radius = haversine(rad_point, centre_point);
125+
double rad_metres = radius / 3.28084;
126+
127+
for (int k = 0; k < n; k++) {
128+
double theta = M_PI * 2 * k / n;
129+
double dx = rad_metres * cos(theta);
130+
double dy = rad_metres * sin(theta);
131+
std::vector<double> point;
132+
point.push_back(center_lat + (180 / M_PI) * (dy / (EARTH_RADIUS * 1000)));
133+
point.push_back(center_long + (180 / M_PI) * (dx / (EARTH_RADIUS * 1000)) / cos(center_lat * M_PI / 180));
134+
poly.push_back(point);
135+
}
136+
poly.push_back(poly[0]);
137+
return poly;
138+
}
139+
140+
std::vector<std::vector<double>> ENUtransforms::GPSObstPoly(Eigen::Vector2d obst_centre, double radius) {
141+
double bearing = getBearing(gps_origin, obst_centre);
142+
Eigen::Vector2d rad_point = inverseHaversine(obst_centre, bearing, radius);
143+
Eigen::Vector2d delta_rad(obst_centre.x() - rad_point.x(), obst_centre.y() - rad_point.y());
144+
Eigen::Vector2d rad_point_further = rad_point + 2 * delta_rad;
145+
std::vector<std::vector<double>> polygon = genPoly(rad_point, obst_centre, SIDES_OF_POLYGON);
146+
return polygon;
147+
}
148+
149+
obstacle_container ENUtransforms::geoObstoENU(obstacle_container ob) {
150+
double obs_height = ob.height;
151+
obstacle_container obs_enu;
152+
double e, n, u;
153+
for (int i = 0; i < ob.poly.size(); i += 1) {
154+
double obs_lat = ob.poly[i][0];
155+
double obs_long = ob.poly[i][1];
156+
ENU_geodetic_obj_.geodetic2Enu(obs_lat, obs_long, obs_height, &e, &n, &u);
157+
obs_enu.poly.push_back({e, n});
158+
}
159+
obs_enu.height = obs_height;
160+
return obs_enu;
161+
}
162+
}
163+
13164
void ENUtransforms::runENU() {
14165
std::setprecision(9);
15166
double min_lat = -1, min_long = -1, max_lat = -1, max_long = -1;
@@ -198,157 +349,7 @@ namespace enu_transforms {
198349
range_east_ = (max_east - min_east) * multiplier_ + offset_ * 2 * resolution_;
199350
}
200351

201-
void ENUtransforms::setParams(int offset_set, double resolution_set, int multiplier_set) {
202-
offset_ = offset_set;
203-
resolution_ = resolution_set;
204-
multiplier_ = multiplier_set;
205-
}
206-
207-
double ENUtransforms::max(const double &a, const double &b) {
208-
return a > b ? a : b;
209-
}
210-
211-
double ENUtransforms::min(const double &a, const double &b) {
212-
return a < b ? a : b;
213-
}
214-
215-
grid_map::Position ENUtransforms::GPStoGridMap(const double &latitude, const double &longitude) {
216-
double e, n, u;
217-
double altitude = 0;
218-
ENU_geodetic_obj_.geodetic2Enu(latitude, longitude, altitude, &e, &n, &u);
219-
grid_map::Position gmap = ENUtoMap(e, n);
220-
return gmap;
221-
}
222-
223-
grid_map::Position ENUtransforms::ENUtoMap(const double &east, const double &north) {
224-
return grid_map::Position{(east - min_east_) * multiplier_ - range_east_ / 2,
225-
(north - min_north_) * multiplier_ - range_north_ / 2};
226-
}
227-
228-
coordsW ENUtransforms::posToENU(const double &x, const double &y, const double &z) {
229-
return {(x + range_east_ / 2) / multiplier_ + min_east_, (y + range_north_ / 2) / multiplier_ + min_north_,
230-
(z)};
231-
}
232-
233-
coordsW ENUtransforms::ENUtoGPS(double e, double n, double u) {
234-
double lat, longitude, altitude;
235-
ENU_geodetic_obj_.enu2Geodetic(e, n, u, &lat, &longitude, &altitude);
236-
return {lat, longitude, altitude};
237-
}
238-
239-
240-
Eigen::Vector3d ENUtransforms::enuReferenceVector(const Eigen::Vector3d &enu) {
241-
Eigen::Vector3d delta = enu_ref_origin - enu;
242-
Eigen::Vector3d ref_vector = delta.normalized();
243-
return ref_vector;
244-
}
245-
246-
247-
Eigen::Vector2d ENUtransforms::gpsReferenceVector(const Eigen::Vector2d &gps) {
248-
Eigen::Vector2d delta = gps_origin - gps;
249-
Eigen::Vector2d ref_vector = delta.normalized();
250-
return ref_vector;
251-
}
252-
253-
double ENUtransforms::haversine(Eigen::Vector2d point1, Eigen::Vector2d point2) {
254-
double haversine;
255-
double temp;
256-
double dist_km;
257-
double lat1 = point1.x() * DEG_TO_RAD;
258-
double lon1 = point1.y() * DEG_TO_RAD;
259-
double lat2 = point2.x() * DEG_TO_RAD;
260-
double lon2 = point2.y() * DEG_TO_RAD;
261-
haversine = (pow(sin((1.0 / 2) * (lat2 - lat1)), 2)) +
262-
((cos(lat1)) * (cos(lat2)) * (pow(sin((1.0 / 2) * (lon2 - lon1)), 2)));
263-
temp = 2 * asin(min(1.0, sqrt(haversine)));
264-
dist_km = EARTH_RADIUS * temp;
265-
double dist_ft = dist_km * 3280.84;
266-
return dist_ft;
267-
}
268-
269-
double ENUtransforms::getBearing(Eigen::Vector2d point1, Eigen::Vector2d point2) {
270-
double lat1 = point2.x() * DEG_TO_RAD;
271-
double lon1 = point2.y() * DEG_TO_RAD;
272-
double lat2 = point1.x() * DEG_TO_RAD;
273-
double lon2 = point1.y() * DEG_TO_RAD;
274-
275-
double dLon = (lon2 - lon1);
276-
double y = sin(dLon) * cos(lat2);
277-
double x = cos(lat1) * sin(lat2) - sin(lat1)
278-
* cos(lat2) * cos(dLon);
279-
double brng = atan2(y, x);
280-
281-
brng = brng * RAD_TO_DEG;
282-
brng = ((int) brng + 360) % 360;
283-
return brng;
284-
285-
return brng;
286-
}
287-
288-
Eigen::Vector2d ENUtransforms::inverseHaversine(Eigen::Vector2d gps, double bearing, double dist_ft) {
289-
double lat1 = gps.x() * DEG_TO_RAD;
290-
double lon1 = gps.y() * DEG_TO_RAD;
291-
292-
double dist_km = dist_ft / 3280.84;
293-
double dist_angular = dist_km / EARTH_RADIUS;
294-
double theta = bearing * DEG_TO_RAD;
295-
double lat2 = asin(sin(lat1) * cos(dist_angular) + cos(lat1) * sin(dist_angular) * cos(theta));
296-
double lon2 =
297-
lon1 + atan2(sin(theta) * sin(dist_angular) * cos(lat2), cos(dist_angular) - sin(lat2) * sin(lat2));
298-
lat2 = lat2 * RAD_TO_DEG;
299-
lon2 = lon2 * RAD_TO_DEG;
300-
Eigen::Vector2d gps_obst_point(lat2, lon2);
301-
return gps_obst_point;
302-
}
303-
304-
std::vector<std::vector<double>>
305-
ENUtransforms::genPoly(Eigen::Vector2d rad_point, Eigen::Vector2d centre_point, int n) {
306-
std::vector<std::vector<double>> poly;
307-
double center_lat = centre_point.x();
308-
double center_long = centre_point.y();
309-
double rad_lat = rad_point.x();
310-
double rad_long = rad_point.y();
311-
312-
double radius = haversine(rad_point, centre_point);
313-
double rad_metres = radius / 3.28084;
314-
315-
for (int k = 0; k < n; k++) {
316-
double theta = M_PI * 2 * k / n;
317-
double dx = rad_metres * cos(theta);
318-
double dy = rad_metres * sin(theta);
319-
std::vector<double> point;
320-
point.push_back(center_lat + (180 / M_PI) * (dy / (EARTH_RADIUS * 1000)));
321-
point.push_back(center_long + (180 / M_PI) * (dx / (EARTH_RADIUS * 1000)) / cos(center_lat * M_PI / 180));
322-
poly.push_back(point);
323-
}
324-
poly.push_back(poly[0]);
325-
return poly;
326-
}
327-
328-
std::vector<std::vector<double>> ENUtransforms::GPSObstPoly(Eigen::Vector2d obst_centre, double radius) {
329-
double bearing = getBearing(gps_origin, obst_centre);
330-
Eigen::Vector2d rad_point = inverseHaversine(obst_centre, bearing, radius);
331-
Eigen::Vector2d delta_rad(obst_centre.x() - rad_point.x(), obst_centre.y() - rad_point.y());
332-
Eigen::Vector2d rad_point_further = rad_point + 2 * delta_rad;
333-
std::vector<std::vector<double>> polygon = genPoly(rad_point, obst_centre, SIDES_OF_POLYGON);
334-
return polygon;
335-
}
336-
337-
obstacle_container ENUtransforms::geoObstoENU(obstacle_container ob) {
338-
double obs_height = ob.height;
339-
obstacle_container obs_enu;
340-
double e, n, u;
341-
for (int i = 0; i < ob.poly.size(); i += 1) {
342-
double obs_lat = ob.poly[i][0];
343-
double obs_long = ob.poly[i][1];
344-
ENU_geodetic_obj_.geodetic2Enu(obs_lat, obs_long, obs_height, &e, &n, &u);
345-
obs_enu.poly.push_back({e, n});
346-
}
347-
obs_enu.height = obs_height;
348-
return obs_enu;
349-
}
350-
} // namespace
351-
352+
352353

353354

354355

0 commit comments

Comments
 (0)